ロボットハンド

#include <Servo.h>

Servo myservo0;

Servo myservo1;

Servo myservo2;

Servo myservo3;

Servo myservo4;

int pos0 = 90;

int pos1 = 90;

int pos2 = 90;

int pos3 = 90;

int pos4 = 90;

void setup() {

  myservo0.attach(3);

  myservo1.attach(5);

  myservo2.attach(9);

  myservo3.attach(10);

  myservo4.attach(11);

  Serial.begin(115200);

}

void loop() {

  // 曲げセンサーの値を取得

  int sensorValue0 = analogRead(A0);

  int sensorValue1 = analogRead(A1);

  int sensorValue2 = analogRead(A2);

  int sensorValue3 = analogRead(A3);

  int sensorValue4 = analogRead(A4);

  int sensorValue5 = analogRead(A5);

  Serial.print(" "); Serial.print(sensorValue0);

  Serial.print(" "); Serial.print(sensorValue1);

  Serial.print(" "); Serial.print(sensorValue2);

  Serial.print(" "); Serial.print(sensorValue3);

  Serial.print(" "); Serial.print(sensorValue4);

  Serial.println(" ;");

  int v0 = map(sensorValue0, 640, 800, 20, 160);

  int v1 = map(sensorValue1, 590, 780, 20, 160);

  int v2 = map(sensorValue2, 650, 820, 20, 160);

  int v3 = map(sensorValue3, 655, 820, 20, 160);

  int v4 = map(sensorValue4, 730, 830, 20, 160);

  pos0 = valc(pos0, v0);

  pos1 = valc(pos1, v1);

  pos2 = valc(pos2, v2);

  pos3 = valc(pos3, v3);

  pos4 = valc(pos4, v4);

  myservo0.write(valx(pos0));

  myservo1.write(valx(pos1));

  myservo2.write(valx(pos2));

  myservo3.write(valx(pos3));

  myservo4.write(valx(pos4));

  Serial.print(" "); Serial.print(pos0);

  Serial.print(" "); Serial.print(pos1);

  Serial.print(" "); Serial.print(pos2);

  Serial.print(" "); Serial.print(pos3);

  Serial.print(" "); Serial.print(pos4);

  Serial.println(" ;");

  delay(20);

}

int valx(int v){

  return 180 - v;

}

int valc(int pos, int x){

  int haba = 10;

  if(pos > x + haba){

    pos -= haba;

    if(pos < 20){ pos = 20; }

  }else if(pos < x - haba){

    pos += haba;

    if(pos > 160){ pos = 160; }

  }

  return pos;

}