ルデコのブログ

手首ロボットのYouTube公開。

回路図

サインスマート（SainSmart）バイオニック ロボット5本指ハンド ロボット パーム グリップ リンケージ サーボ制御 左右手 （5DOF Humanoid Five Fingers Metal Manipulator Arm Left Hand with Servos for Robot DIY）

Amazon（アマゾン）

12,999円

ELEGOO Arduino用 HC-SR04超音波距離センサー5PCS

Amazon（アマゾン）

950円

プログラム・スケッチ

//LEDECO.com
//2020.07.03
const int Trig = 12;
const int Echo = 13;
const int Led_1 = 2;
const int Led_2 = 3;
const int Led_3 = 4;
const int Led_4 = 10;
const int Led_5 = 11;
#include <Servo.h>
Servo myservo1;
Servo myservo2;
Servo myservo3;
Servo myservo4;
Servo myservo5;
int Duration;
float Distance;
void setup() {
  Serial.begin(9600);
  pinMode(Led_1,OUTPUT);
  pinMode(Led_2,OUTPUT);
  pinMode(Led_3,OUTPUT);
  pinMode(Led_4,OUTPUT);
  pinMode(Led_5,OUTPUT);
  pinMode(Trig,OUTPUT);
  pinMode(Echo,INPUT);
  myservo1.attach(5);
  myservo2.attach(6);
  myservo3.attach(7);
  myservo4.attach(8);
  myservo5.attach(9);}

void loop() {
  digitalWrite(Trig,LOW);
  delayMicroseconds(3);
  digitalWrite(Trig,HIGH);
  delayMicroseconds(3);
  digitalWrite(Trig,LOW);
  Duration = pulseIn(Echo,HIGH);
  Serial.print(Duration);
  if (Duration>0) {
    Distance = Duration/2;
    Distance = Distance*340*100/1000000; // ultrasonic speed is 340m/s = 34000cm/s = 0.034cm/us 
    Serial.print(Duration);
    Serial.print(" us ");
    Serial.print(Distance);
    Serial.println(" cm");
    
    if (Distance < 20){
      digitalWrite(Led_1,HIGH);
      digitalWrite(Led_2,LOW);
      digitalWrite(Led_3,LOW);
      digitalWrite(Led_4,LOW);
      digitalWrite(Led_5,LOW);
     myservo1.write(60);
     myservo2.write(60);
     myservo3.write(60);
     myservo4.write(0);
     myservo5.write(0);
     delay(300); 
     }
    else if(Distance < 30){
      digitalWrite(Led_1,LOW);
      digitalWrite(Led_2,HIGH);
      digitalWrite(Led_3,LOW);
      digitalWrite(Led_4,LOW);
      digitalWrite(Led_5,LOW);
     myservo1.write(0);
     myservo2.write(60);
     myservo3.write(60);
     myservo4.write(0);
     myservo5.write(0);
     delay(300);}
    else if(Distance < 40){
      digitalWrite(Led_1,LOW);
      digitalWrite(Led_2,LOW);
      digitalWrite(Led_3,HIGH);
      digitalWrite(Led_4,LOW);
      digitalWrite(Led_5,LOW);
     myservo1.write(0);
     myservo2.write(60);
     myservo3.write(60);
     myservo4.write(0);
     myservo5.write(75);
     delay(300);
     }
     else if(Distance < 50){      
      digitalWrite(Led_1,LOW);
      digitalWrite(Led_2,LOW);
      digitalWrite(Led_3,LOW);
      digitalWrite(Led_4,HIGH);
      digitalWrite(Led_5,LOW);
     myservo1.write(0);
     myservo2.write(60);
     myservo3.write(60);
     myservo4.write(60);
     myservo5.write(60);
     delay(300);}
     else{
      digitalWrite(Led_1,LOW);
      digitalWrite(Led_2,LOW);
      digitalWrite(Led_3,LOW);
      digitalWrite(Led_4,LOW);
      digitalWrite(Led_5,HIGH);}
     myservo1.write(0);
     myservo2.write(0);
     myservo3.write(0);
     myservo4.write(60);
     myservo5.write(60);
     delay(300);   }
  delay(300);
}