#include <Stepper.h>

Stepper motors[] = {
  // The first value is the number of steps the motor needs for one revolution, adjust it accordingly
  Stepper (200, PA0, PA1, PA2, PA3), // Base
  Stepper (200, PB0, PB1, PB2, PB3), // Joint one, just double the wires to send the signal to both motors
  Stepper (200, PC0, PC1, PC2, PC3) // Joint two
 };

// This can be any position of the arm but the movements will be relative to it
int steps[] = {0, 0, 0};

// fin[] and finCount have to be reset before a new target 
boolean fin[] = {false, false, false};
int finCount = 0;

// These are used to hold the serial input
int a = 0; int b = 0; int c = 0;

void setup() 
{
    Serial.begin(9600);
}

void loop() {
  Serial.flush();
  Serial.write("Enter target position");
  
  Serial.write("Base");
  while(!Serial.available());
  a = Serial.read();
  Serial.flush();

  Serial.write("Joint 1");
  while(!Serial.available());
  b = Serial.read();
  Serial.flush();

  Serial.write("Joint 2");
  while(!Serial.available());
  c = Serial.read();
  Serial.flush();
  
  moveTo(a,b,c);
}

void moveTo(int baseTarget, int jointOneTarget, int jointTwoTarget){
  while(finCount < 3){ // When all three have finished we are done
        //Move each motor one step towards the target positiont
        moveMotorOneStep(0, baseTarget); 
        moveMotorOneStep(1, jointOneTarget);
        moveMotorOneStep(2, jointTwoTarget);
    }
}

void moveMotorOneStep(int motor, int target){
    if(steps[motor] < target){ // We need to move forward
              motors[motor].step(1);
              steps[motor]++;
            }
            else if(steps[motor] > target){ // We need to move back
              motors[motor].step(-1);
              steps[motor]--;
            }else if(fin[motor] == false) { //If we reach this we are in the target position so we check if we have already told the others
              fin[motor] = true;
              finCount++;
           }
}