//アームロボットキャリブレーション用ソースコード

#include <Servo.h>

Servo servo_0; // hand
Servo servo_1; // arm1
Servo servo_2; // arm2
Servo servo_3; // pan

int val0, val1, val2, val3; // Manipulator Potension meter value
int scv0, scv1, scv2, scv3; // Servo control value

void setup() {
  Serial.begin(115200);
  servo_0.attach(PB9, 500, 2500); // hand
  servo_1.attach(PB8, 500, 2500); // arm1
  servo_2.attach(PB7, 500, 2500); // arm2
  servo_3.attach(PB6, 500, 2500); // pan
  servo_1.write(90); // Arm1 Servo
  servo_2.write(90); // Arm2 Servo
  servo_3.write(90); // Pan Servo
}

void loop() {
  val1 = analogRead(PA1);
  val2 = analogRead(PA2);
  val3 = analogRead(PA3);

  if (digitalRead(PA0) == HIGH) {
    val0 = 3300;
  } else {
    val0 = 4000;
  }

  Serial.print("val0 = "),  Serial.print(val0);
  Serial.print(", val1 = "),  Serial.print(val1);
  Serial.print(", val2 = "),  Serial.print(val2);
  Serial.print(", val3 = "),  Serial.println(val3);

  scv0 = map(val0, 400, 4000, 0, 180); // hand Manipulator
  scv1 = map(val1, 400, 3200, 0, 180); // Arm1 Manipulator
  if (scv1 >= 180) {
    scv1 = 180;
  } else if (scv1 <= 0.0) {
    scv1 = 0.0;
  }
  scv2 = map(val2, 2800, 500, 0, 180); // Arm2 Manipulator
  if (scv2 >= 180) {
    scv2 = 180;
  } else if (scv2 <= 0) {
    scv2 = 0;
  }
  scv3 = map(val3, 2900, 220, 0, 180); //  pan Manipulator
  if (scv3 >= 180) {
    scv3 = 180;
  } else if (scv3 <= 0) {
    scv3 = 0;
  }
  servo_0.write(scv0); // Hand Servo
  servo_1.write(scv1); // Arm1 Servo
  servo_2.write(scv2); // Arm2 Servo
  servo_3.write(scv3); // Pan Servo

  Serial.print("scv0 = "),  Serial.print(scv0);
  Serial.print(", scv1 = "),  Serial.print(scv1);
  Serial.print(", scv2 = "),  Serial.print(scv2);
  Serial.print(", scv3 = "),  Serial.println(scv3);
}