// this sketch is for the new miniboard.
// it differs from the version 2 in that the pins are now the other
// way around. (pinMapping is now "old")

#define LEFT true // sides outa synch, set false for right legs

#define SHOULDER 0 // give servo array indexes nice names
#define ELBOW 1

const char legNum='3';

const int rxPin=7; // physical pin 10
const int txPin=8; // physical pin 11
const int bluLED=10; // physical pin 13
const int redLED=9; // physical pin 12
const int yelLED=8; // physical pin 11
const int grnLED=7; // physical pin 10
const int servoPin0=3; // physical pin 6
const int servoPin1=2; // physical pin 5

#include <SoftwareSerial.h>
SoftwareSerial mySerial(rxPin, txPin);
String inMessage;
String inCommand="";

#include <Servo.h>
Servo myservo1;
Servo myservo2;

const byte servoPin[2] = {servoPin0,servoPin1}; // place your servo pin numbers here
byte rest[] = {100,20,0};

void moveServo(int which, int pos)
{
  switch(which)
  {
    case SHOULDER:
      myservo1.write(pos);
      break;
    case ELBOW:
      myservo2.write(pos);
      break;
  }
}

// ================================================================================================================================================
void setupServos()
{
  myservo1.attach(servoPin[0]);
  myservo2.attach(servoPin[1]);
  moveServo(ELBOW,120);
  moveServo(SHOULDER,140);
}


// ===================================================================================================================================
void setup() 
{
  mySerial.begin(1200);
  
  // initialise the LEDs
    for (byte i=9; i<= 10; i++)
      {
        pinMode(i,OUTPUT);
        digitalWrite(i,LOW);
        delay(1000);
        digitalWrite(i,HIGH);
        }

  // initialise the sensors
  //   pinMode(8,INPUT); // microswitch

 setupServos(); // It's not necessary to set the servo pins as OUTPUT, that's taken care of for you


}

 
// ===================================================================================================================================
void loop() { // run over and over
  while (mySerial.available()) {
    String inString = String(mySerial.read());
    long var=inString.toInt();
    if (var!=13) {
      if (var==10) {
        if(inMessage[1]==legNum || inMessage[1]=='7') {
          digitalWrite(redLED,LOW);
          delay(100);
          digitalWrite(redLED,HIGH);
          delay(50);
          inCommand = inMessage;
        }
        inMessage="";
       
      } else {                    // var!=10
        inMessage+=char(var);
      }
    }

  }

    if(inCommand[1]==legNum) {
      char action=inCommand[3];
      int angle=inCommand.substring(9).toInt();
      if (angle > 20) {
        switch(action) {
          case 'f':
            digitalWrite(bluLED,LOW);
            digitalWrite(redLED,LOW);
            delay(100);
            digitalWrite(bluLED,HIGH);
            digitalWrite(redLED,HIGH);
            delay(50);
            break;
          case 's':
            digitalWrite(redLED,LOW);
            delay(100);
            digitalWrite(redLED,HIGH);
            delay(50);
            if(legNum=='4' || legNum=='5' || legNum=='6'){
              angle = 180-angle;
            }
            moveServo(SHOULDER,angle);
            break;      
          case 'h':
            digitalWrite(bluLED,LOW);
            delay(100);
            digitalWrite(bluLED,HIGH);
            delay(50);
            moveServo(ELBOW,angle);
            break;
        }      
      }
    }
    if(inCommand[1]=='7') {
      char action=inCommand[3];
      int angle=inCommand.substring(9).toInt();
      if (angle > 20) {
        switch(action) {
          case 'f':
            digitalWrite(bluLED,LOW);
            digitalWrite(redLED,LOW);
            delay(100);
            digitalWrite(bluLED,HIGH);
            digitalWrite(redLED,HIGH);
            delay(50);
            break;
          case 's':
            digitalWrite(bluLED,LOW);
            delay(100);
            digitalWrite(bluLED,HIGH);
            delay(50);
            if(legNum=='4' || legNum=='5' || legNum=='6'){
              angle = 180-angle;
            }
            moveServo(SHOULDER,angle);
            break;      
          case 'h':
            digitalWrite(redLED,LOW);
            delay(100);
            digitalWrite(redLED,HIGH);
            delay(50);
            moveServo(ELBOW,angle);
            break;  
        }    
      }
    }
    inCommand = "";
}