//SuperRobotArm v0.2 - Eric Skiff & Bre Pettis
//based on 
//twitchie v0.5 by Raphael http://teuthis.com

//TODO:
//Configurable middle value array for each servo 
//timer servo
//leds to let you know which joint you're on?


#define servoPinA 2     // Control pin for servo motor
#define servoPinB 3     // Control pin for servo motor
#define servoPinC 4     // Control pin for servo motor
#define servoPinD 5     // Control pin for servo motor
#define servoPinE 6     // Control pin for servo motor
#define servoPinF 7     // Control pin for servo motor
#define servoPinG 8     // Control pin for servo motor
#define servoPinH 9     // Control pin for servo motor
#define Led 10          // activity light
//#define touchsensor 0          // activity light


#define minangle 550    // Minimum servo position
#define maxangle 2400   // Maximum servo position
#define middleangle 1425 // Middle servo
#define middleanglebase 1950 // bottom servo attention position.
#define pincheropen 1600
#define pincherclosed 2400

#define trigger 0       // the analog pin that the sensor's on

int pinchCounter = 0;
int mode = 2;
int servoPositions[8] = {middleangle, middleangle, middleangle, middleangle, middleanglebase, middleanglebase, middleangle, middleangle};
int buttonPress = 0;


/////////////////////////////////////////////////////////////////
void setup() {
  pinMode(servoPinA, OUTPUT);  // Set servo pin as an output pin
  pinMode(servoPinB, OUTPUT);  // Set servo pin as an output pin
  pinMode(servoPinC, OUTPUT);  // Set servo pin as an output pin
  pinMode(servoPinD, OUTPUT);  // Set servo pin as an output pin
  pinMode(servoPinE, OUTPUT);  // Set servo pin as an output pin
  pinMode(servoPinF, OUTPUT);  // Set servo pin as an output pin
  pinMode(servoPinG, OUTPUT);  // Set servo pin as an output pin
  pinMode(servoPinH, OUTPUT);  // Set servo pin as an output pin
  pinMode(Led, OUTPUT);
  pinMode(0, INPUT); 
  pinMode(1, INPUT);  
  pinMode(2, INPUT); 
  pinMode(3, INPUT);  
  pinMode(4, INPUT); 
  pinMode(5, INPUT);
  pinMode(6, INPUT);  
  Serial.begin(9600);
  ledflash(3,200);
  Serial.println("I... am... ALIIIIVE!!!!!");
}
/////////////////////////////////////////////////////////////////
void loop() {
  analogWrite(Led,random(1,200));
SetPositions();
ReadInputs();

}
////////////////////////////////////////////////


void ledflash(int number, int duration) {  
  for(;number>0;number--){
    delay(duration/2);
    digitalWrite(Led, HIGH);   // sets the LED on  
    delay(duration/2);                  // waits for a second  
    digitalWrite(Led, LOW);    // sets the LED off  
  }
} 

//////////////////////////////////////////////////////////////
void servo(char pin, int angle){
  // protect the servos from pulses that move them too far
  if(angle>maxangle){angle=maxangle;}
  if(angle<minangle){angle=minangle;}

  // now figure out which one should move, and move it toward the angle specified
  // this will only move the servo by one pulse, 
  // so you will need to call this a few times to get it all the way there
  if(pin=='A' || pin=='a'){digitalWrite(servoPinA, HIGH);}
  if(pin=='B' || pin=='b'){digitalWrite(servoPinB, HIGH);}
  if(pin=='C' || pin=='c'){digitalWrite(servoPinC, HIGH);}
  if(pin=='D' || pin=='d'){digitalWrite(servoPinD, HIGH);}
  if(pin=='E' || pin=='e'){digitalWrite(servoPinE, HIGH);}
  if(pin=='F' || pin=='f'){digitalWrite(servoPinF, HIGH);}
  if(pin=='G' || pin=='g'){digitalWrite(servoPinG, HIGH);}
  if(pin=='H' || pin=='h'){digitalWrite(servoPinH, HIGH);}
 
  delayMicroseconds(angle);       // Length of the pulse sets the motor position

  if(pin=='A' || pin=='a'){digitalWrite(servoPinA, LOW);}
  if(pin=='B' || pin=='b'){digitalWrite(servoPinB, LOW);}
  if(pin=='C' || pin=='c'){digitalWrite(servoPinC, LOW);}
  if(pin=='D' || pin=='d'){digitalWrite(servoPinD, LOW);}
  if(pin=='E' || pin=='e'){digitalWrite(servoPinE, LOW);}
  if(pin=='F' || pin=='f'){digitalWrite(servoPinF, LOW);}
  if(pin=='G' || pin=='g'){digitalWrite(servoPinG, LOW);}
  if(pin=='H' || pin=='h'){digitalWrite(servoPinH, LOW);}
}

void incrementServo(int servoNumber){
  if(servoPositions[servoNumber] <= maxangle) {
    servoPositions[servoNumber] = servoPositions[servoNumber] + 15; 
  }
}

void decrementServo(int servoNumber){
  if(servoPositions[servoNumber] >= minangle) {
    servoPositions[servoNumber] = servoPositions[servoNumber] - 15; 
  }
}



void SetPositions(){
  servo('a',servoPositions[0]);
  servo('b',servoPositions[1]);
  servo('c',servoPositions[2]);
  servo('d',servoPositions[3]);
  servo('e',servoPositions[4]);
  servo('f',servoPositions[5]);
  servo('g',servoPositions[6]);
  servo('h',servoPositions[7]);      

  delay(10);

}

void ReadInputs(){
  if(analogRead(1) > 100){
    buttonPress = 1;
    Serial.println("buttonPress = 1");
  }else{
    if(buttonPress == 1){ 
      buttonPress = 0; 
      if(mode < 3) {mode++;}
      else {mode = 0;}
      Serial.println(" ");
      Serial.print("set mode to ");
      Serial.print(mode);
    }
  }
  if(mode == 0){
   for(int x = 0;x<6;x++){
     if(analogRead(x) > 100){
       Serial.print("input on pin ");
       Serial.print(x);
       if(x == 4){ 
         incrementServo(4);
         incrementServo(5);
       }
       else if(x == 5){ 
         decrementServo(4);
         decrementServo(5);
       }else if(x == 3){ 
         decrementServo(3);
       }else if(x == 2){ 
         incrementServo(3);
       }
     }
   }
  }else if(mode == 1){
   for(int x = 0;x<6;x++){
     if(analogRead(x) > 100){
       Serial.print("input on pin ");
       Serial.print(x);
       if(x == 4){ 
         decrementServo(0);
       }
       else if(x == 5){ 
         incrementServo(0);
       }else if(x == 3){ 
         incrementServo(3);
       }else if(x == 2){ 
         decrementServo(3);
       }
     }
   }
  }else if(mode == 2){
   for(int x = 0;x<6;x++){
     if(analogRead(x) > 100){
       Serial.print("input on pin ");
       Serial.print(x);
       if(x == 4){ 
         incrementServo(1);
       }
       else if(x == 5){ 
         decrementServo(1);
       }else if(x == 3){ 
         incrementServo(3);
       }else if(x == 2){ 
         decrementServo(3);
       }
     }
   }
  }else {
      for(int x = 0;x<6;x++){
     if(analogRead(x) > 100){
       Serial.print("input on pin ");
       Serial.print(x);
       if(x == 4){ 
         decrementServo(6);
       }
       else if(x == 5){
         incrementServo(6); 
       }else if(x == 3){ 
         decrementServo(2);
       }else if(x == 2){ 
         incrementServo(2);
       }
     }
   }
  }
}