GoPro Arm Control

/*
NOTES TO MATT
*/

const int controlPin1 = 2; //Wrist
const int controlPin2 = 3;
const int control2Pin1 = 12; //Elbow
const int control2Pin2 = 13;
const int control3Pin1 = 8; //Spin
const int control3Pin2 = 7;
const int enablePin = 9; // Wrist motor
const int enablePin2 = 11; // Elbow motor
const int enablePin3 = 10; // Spin motor
const int onOffPin = 5;

int onOffState = LOW;
int previousOnOffState = LOW;
int previousDirectionSwitchState = 0;
int spinMotorEnabled = 0;
int elbowMotorEnabled = 0;
int wristMotorEnabled = 0;
int spinMotorSpeed = 55; // these speeds are for the 12v lead battery, they are defaults, real speeds are in the main loop
int elbowMotorSpeed = 50;
int wristMotorSpeed = 42; // use about 5/7 elbow because that's the ration of the length of the wrist to the arm
int spinMotorDirection = 0; //Left is 0
int elbowMotorDirection = 1;
int wristMotorDirection = 0; //Wrist's motor needs to move opposite to elbowMotor
int count = 0;
int spinCount = 0;
int upDist = 4500;
int downDist = 2700;
int spinDist = 3200;
int spinFlag = 0;
int mode = 0; //Mode can be 0, 1, or 2 representing the off state, motion and reverse motion

void setup(){
  //Serial.begin(9600);
  pinMode(onOffPin, INPUT);
  pinMode(controlPin1, OUTPUT);
  pinMode(controlPin2, OUTPUT);
  pinMode(control2Pin1, OUTPUT);
  pinMode(control2Pin2, OUTPUT);
  pinMode(control3Pin1, OUTPUT);
  pinMode(control3Pin2, OUTPUT);
  pinMode(enablePin, OUTPUT);
  digitalWrite(enablePin, LOW);
  pinMode(enablePin2, OUTPUT);
  digitalWrite(enablePin2, LOW);
  pinMode(enablePin3, OUTPUT);
  digitalWrite(enablePin3, LOW);
}

boolean debounce(boolean last)
{
  boolean current = digitalRead(onOffPin);
  if (last != current)
  {
    delay(5); //wait 5ms
    current = digitalRead(onOffPin);
  }
  return current;
}

void loop(){
  onOffState = debounce(previousOnOffState);
  delay(1);

  if(previousOnOffState == LOW && onOffState == HIGH){
    elbowMotorEnabled = !elbowMotorEnabled;
    spinMotorEnabled = !spinMotorEnabled;
  }

  //Check if it's time to start raising the arm
  if (spinCount >= 2900){
    spinFlag = 1;
  }

  // checking if the motor movement is complete
  if (elbowMotorEnabled == HIGH && spinFlag == 1) {
    if(count < upDist && elbowMotorDirection == 1){
      //Serial.println(spinFlag);
      //Serial.println(spinCount);
      count = count + 1;
    }
    else if (count < downDist && elbowMotorDirection == 0) {
      count = count + 1;
    }
    else{
      count = 0;
      spinFlag = 0;
      elbowMotorEnabled = 0;
      elbowMotorDirection = !elbowMotorDirection;
      wristMotorDirection = !wristMotorDirection;
    }
  }

  //checking spin motor movement against distance
  if (spinMotorEnabled == HIGH){
    if (spinCount < spinDist){
      spinCount = spinCount + 1;
    }
    else{
      spinCount = 0;
      spinMotorEnabled = 0;
      spinMotorDirection = !spinMotorDirection;
    }
  }

  // Telling the H-bridge the direction of the elbow motor
  if (elbowMotorDirection == 1){
    digitalWrite(control2Pin1, HIGH); // elbow
    digitalWrite(control2Pin2, LOW);
    //elbowMotorSpeed = 200; // for 6v batt
    elbowMotorSpeed = 70; // for 12v batt
  }
  else{
    digitalWrite(control2Pin1, LOW); // elbow
    digitalWrite(control2Pin2, HIGH);
    //elbowMotorSpeed = 200; // for 6v batt
    elbowMotorSpeed = 50; // for 12v batt
  }

  // Telling the H-bridge the direction of the wrist motor
  if (wristMotorDirection == 1){
    digitalWrite(controlPin1, LOW); // wrist
    digitalWrite(controlPin2, HIGH);
    //wristMotorSpeed = 200; // for 6v batt
    wristMotorSpeed = 53; // for 12v batt
  }
  else{
    digitalWrite(controlPin1, HIGH); // wrist
    digitalWrite(controlPin2, LOW);
    //wristMotorSpeed = 130; // for 6v batt
    wristMotorSpeed = 40; // for 12v batt, minimum value is ~35
  }

  // Telling the H-bridge the direction of the spin motor
  if (spinMotorDirection == 1){
    digitalWrite(control3Pin1, HIGH); // elbow
    digitalWrite(control3Pin2, LOW);
  }
  else{
    digitalWrite(control3Pin1, LOW); // elbow
    digitalWrite(control3Pin2, HIGH);
  }

  //Controlling vertical speed
  if(elbowMotorEnabled == 1 && spinFlag == 1){
    if (elbowMotorDirection == 1 && count % 2 == 0){
      analogWrite(enablePin, wristMotorSpeed);
      //Serial.println(count);
    }
    else if (elbowMotorDirection == 1 && count % 2 != 0){
      analogWrite(enablePin, 0);
    }
    else if (elbowMotorDirection == 0){
      analogWrite(enablePin, wristMotorSpeed);
    }

    analogWrite(enablePin2, elbowMotorSpeed);
  }
  else{
    analogWrite(enablePin, 0);
    analogWrite(enablePin2, 0);
  }

  // Controlling the spin motor speed
  if(spinMotorEnabled == 1){
    analogWrite(enablePin3, spinMotorSpeed);
  }
  else{
    analogWrite(enablePin3, 0);
  }
  //Serial.println(elbowMotorDirection);
  previousOnOffState = onOffState;
}