// Robotic ARM // Actual pins 2,3,4,5

// Robotic ARM

#define axis1 3
#define axis2 5
#define axis3 6
#define axis4 9

int a1Init=90;
int a2Init=90;
int a3Init=90;
int a4Init=90;

int a1=a1Init;
int a2=a2Init;
int a3=a3Init;
int a4=a4Init;

int tuneAngle = 5;

void setup() {
  Serial.begin(9600);
  pinMode(axis1, OUTPUT);
  pinMode(axis2, OUTPUT);
  pinMode(axis3, OUTPUT);
  pinMode(axis4, OUTPUT);
  rest();
  Serial.println("SetupComplete");
}

void loop() {
  if (Serial.available()){
    char ch = Serial.read();
    if (ch == 'a'){
      a1 = a1+tuneAngle;
      if (a1 >= 180)  a1 = 180;
      Serial.print("a1,");Serial.println(a1);
      setAngle(axis1, a1);
    }
    if (ch == 'A'){
      a1 = a1-tuneAngle;
      if (a1 <= 0)  a1 = 0;
      Serial.print("a1,");Serial.println(a1);
      setAngle(axis1, a1);
    }
    if (ch == 'b'){
      a2 = a2+tuneAngle;
      if (a2 >= 180)  a2 = 180;
      Serial.print("a2,");Serial.println(a2);
      setAngle(axis2, a2);
    }
    if (ch == 'B'){
      a2 = a2-tuneAngle;
      if (a2 <= 0)  a2 = 0;
      Serial.print("a2,");Serial.println(a2);
      setAngle(axis2, a2);
    }
    if (ch == 'c'){
      a3 = a3+tuneAngle;
      if (a3 >= 180)  a3 = 180;
      Serial.print("a3,");Serial.println(a3);
      setAngle(axis3, a3);
    }
    if (ch == 'C'){
      a3 = a3-tuneAngle;
      if (a3 <= 0)  a3 = 0;
      Serial.print("a3,");Serial.println(a3);
      setAngle(axis3, a3);
    }
    if (ch == 'd'){
      a4 = a4+tuneAngle;
      if (a4 >= 180)  a4 = 180;
      Serial.print("a4,");Serial.println(a4);
      setAngle(axis4, a4);
    }
    if (ch == 'D'){
      a4 = a4-tuneAngle;
      if (a4 <= 0)  a4 = 0;
      Serial.print("a4,");Serial.println(a4);
      setAngle(axis4, a4);
    }
    if (ch == 'r'){
      rest();
    }
  }
}

void setAngle(int pin, int val){
  int servoVal = map(val, 0, 180, 0, 255);
  Serial.print("->");Serial.print(pin);Serial.print(",");Serial.println(servoVal);
  analogWrite(pin, servoVal);
  delay(1);
}

void rest(){
  setAngle(axis1, a1Init);
  setAngle(axis2, a2Init);
  setAngle(axis3, a3Init);
  setAngle(axis4, a4Init);
}