//Title: gimbal_code
//
//Programmer: Joe Morrow
//Contact: jmmorrow@mit.edu
//Date: Nov 12, 2011
//
//Description: Operates in two modes. Mode one takes analog inputs from three pots
//and commands three servos accordingly. Angles etc are also printed to an lcd 
//screen via serialsoft. Mode two does the same thing, but replaces the input
//from the pots with external commands via the hardware serial port. 

#include <SoftwareSerial.h>

#define innerPot A0
#define middlePot A1
#define outerPot A2
#define txPin 2 //to lcd
#define modeSelect 3//panel or serial
#define unitSelect 4 //deg or rad switch 
#define innerServo 5
#define middleServo 6
#define outerServo 7
#define ledPin 8 //gimbal lock led
#define timeFlag 19
#define timeLimit 22

char buffer[5]; //make one larger than number of digits expecting to receive
int received = 0;

SoftwareSerial LCD = SoftwareSerial(0,txPin);
const int LCDdelay = 1;

float dT0;
float dT;

int LCDflag;
const int LCDupdate = 10; //how many 25ms cycles before LCD update

void setup(){

  pinMode(innerPot, INPUT);
  pinMode(middlePot, INPUT);
  pinMode(outerPot, INPUT);
  pinMode(txPin, OUTPUT);
  pinMode(modeSelect, INPUT);
  pinMode(unitSelect, INPUT);
  pinMode(innerServo, OUTPUT);
  pinMode(middleServo, OUTPUT);
  pinMode(outerServo, OUTPUT);
  pinMode(ledPin, OUTPUT);
  
  digitalWrite(unitSelect,HIGH); //pull up resistor 

  Serial.begin(115200);

  LCD.begin(19300);  
  delay(500);
  clearLCD();
  delay(100);
  goTo(0,0);
  LCD.print("Setup Complete");
  backlightOn();

  delay(500);

}

void loop(){

  dT0 = millis();

  int modeSelectVal = digitalRead(modeSelect);
  int unitSelectVal = digitalRead(unitSelect);

  int innerPw = 1500;
  int middlePw = 1500;
  int outerPw = 1500;
  int innerAngle = 0.0;
  int middleAngle = 0.0;
  int outerAngle = 0.0;

  if (modeSelectVal == 0) { //panel mode

    int innerPotVal = analogRead(innerPot);
    int middlePotVal = analogRead(middlePot);
    int outerPotVal = analogRead(outerPot);

    innerPotVal = map(innerPotVal,0,1023,1000,2000);
    middlePotVal = map(middlePotVal,0,1023,1000,2000);
    outerPotVal = map(outerPotVal,0,1023,1000,2000);

    innerPw = constrain(innerPotVal,1000,2000);
    middlePw = constrain(middlePotVal,1000,2000);
    outerPw = constrain(outerPotVal,1000,2000);

  } 
  else { //serial mode

      if(Serial.available()){

      buffer[received++] = Serial.read();

      if (received >= (sizeof(buffer)-1)) {

        innerPw = atoi(buffer);
        innerPw = constrain(innerPw,1000,2000);
        middlePw = innerPw;
        outerPw = outerPw;
        received = 0;
        Serial.flush();

        dT = millis();
        if ((dT - dT0) > timeFlag) {
          goto PULSE;
        }

      }
      
    }
    
    dT = millis();
    if ((dT - dT0) > timeFlag) {
      goto PULSE;
    }
    
  }

  if(LCDflag >= LCDupdate) {
    
    LCDflag = 0;

    clearLCD();
  
    if (unitSelectVal == 1) { //deg
  
      innerAngle = map(innerPw,1000,2000,-90,90);
      middleAngle = map(middlePw,1000,2000,-90,90);
      outerAngle = map(outerPw,1000,2000,-90,90);
      
      if(innerAngle >= 0) {
        goTo(0,1);
      } else {
        goTo(0,0);
      }
      LCD.print((int)innerAngle);
      dT = millis();
      if ((dT - dT0) > timeFlag) {
        goto PULSE;
      }
      if(middleAngle >= 0) {
        goTo(0,7);
      } else {
        goTo(0,6);
      }
      LCD.print((int)middleAngle);
      dT = millis();
      if ((dT - dT0) > timeFlag) {
        goto PULSE;
      }
      if(outerAngle >= 0) {
        goTo(0,13);
      } else {
        goTo(0,12);
      }
      LCD.print((int)outerAngle);
      dT = millis();
      if ((dT - dT0) > timeFlag) {
        goto PULSE;
      }
      goTo(1,0);
      LCD.print("Degrees*1");
      dT = millis();
      if ((dT - dT0) > timeFlag) {
        goto PULSE;
      }
  
    } else { //rad
  
      innerAngle = map(innerPw,1000,2000,-157,157);
      middleAngle = map(middlePw,1000,2000,-157,157);
      outerAngle = map(outerPw,1000,2000,-157,157);
  
      if(innerAngle >= 0) {
        goTo(0,1);
      } else {
        goTo(0,0);
      }
      LCD.print((int)innerAngle);
      dT = millis();
      if ((dT - dT0) > timeFlag) {
        goto PULSE;
      }
      if(middleAngle >= 0) {
        goTo(0,7);
      } else {
        goTo(0,6);
      }
      LCD.print((int)middleAngle);
      dT = millis();
      if ((dT - dT0) > timeFlag) {
        goto PULSE;
      }
      if(outerAngle >= 0) {
        goTo(0,13);
      } else {
        goTo(0,12);
      }
      LCD.print((int)outerAngle);
      dT = millis();
      if ((dT - dT0) > timeFlag) {
        goto PULSE;
      }
      goTo(1,0);
      LCD.print("Radians*100");
      dT = millis();
      if ((dT - dT0) > timeFlag) {
        goto PULSE;
      }
  
    }
    
  }

  dT = millis();
  if ((dT-dT0) >= timeFlag) {
    goto PULSE;
  } else {
    delay(timeLimit-(dT-dT0));
  }

PULSE:

  digitalWrite(innerServo, HIGH);
  delayMicroseconds(innerPw);
  digitalWrite(innerServo, LOW);
  digitalWrite(middleServo, HIGH);
  delayMicroseconds(middlePw);
  digitalWrite(middleServo, LOW);
  digitalWrite(outerServo, HIGH);
  delayMicroseconds(outerPw);
  digitalWrite(outerServo, LOW);
  
  Serial.println(LCDflag);
  
  LCDflag = LCDflag + 1;

}


//////////////////////////////////////////////
float mapf(float x, float in_min, float in_max, float out_min, float out_max)
{
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

void goTo(int row, int col) {
  LCD.print(0xFE,BYTE);
  LCD.print((col+row*64+128),BYTE);
  delay(LCDdelay);
}
void clearLCD(){
  LCD.print(0xFE, BYTE);
  LCD.print(0x01,BYTE);
  delay(LCDdelay);
}
void backlightOn() {
  LCD.print(0x7C,BYTE);
  LCD.print(157,BYTE); //light level (128 to 157)
}
void backlightOff() {
  LCD.print(0x7C,BYTE);
  LCD.print(128,BYTE);
  delay(LCDdelay);
}
void serCommand() {
  LCD.print(0xFE,BYTE);
}
//void setRate() {
//  LCD.print(0x7C,BYTE);
//  Serial.print(0x0A, BYTE);  //1200 bps
//  Serial.print(0x0B, BYTE);  //2400 bps
//  Serial.print(0x0C, BYTE);  //4800 bps
//  Serial.print(0x0D, BYTE);  //9600 bps
//  Serial.print(0x0E, BYTE);  //14400 bps
//  Serial.print(0x0F, BYTE);  //19200 bps
//  Serial.print(0x10, BYTE);  //38400 bps
//}