/*******************************************
 *
 *  CMR Robot Arm control
 *
 *  Scott Warren, Caroline Chang
 *
 *******************************************/

#include <PID_v1.h>
#include <Servo.h>
#include <QueueArray.h>

#define BASE_P 20
#define BASE_I 0
#define BASE_D 0

#define SHOULDER_P 4
#define SHOULDER_I 0
#define SHOULDER_D 0

#define ELBOW_P 4
#define ELBOW_I 0
#define ELBOW_D 0

#define BASE_SAMPLE_TIME     100
#define SHOULDER_SAMPLE_TIME 100
#define ELBOW_SAMPLE_TIME    100

//#define BASE_OUT_MIN -108
//#define BASE_OUT_MAX 60
#define BASE_OUT_MIN -84
#define BASE_OUT_MAX 84

#define SHOULDER_OUT_MIN -150
#define SHOULDER_OUT_MAX 150

#define ELBOW_OUT_MIN -200
#define ELBOW_OUT_MAX 200

//#define WRIST_OUT_MIN 1200
//#define WRIST_OUT_MAX 1800

#define MAX_BASE_POSE 700
#define MIN_BASE_POSE 300

#define MAX_SHOULDER_POSE 600
#define MIN_SHOULDER_POSE 400

#define MAX_ELBOW_POSE 915
#define MIN_ELBOW_POSE 200

#define BASE_SETPOINT_START     512
#define SHOULDER_SETPOINT_START 512
#define ELBOW_SETPOINT_START    512

//#define BASE_ERROR_RANGE     .01
//#define SHOULDER_ERROR_RANGE .01
//#define ELBOW_ERROR_RANGE    .01

#define BASE_INPUT_PIN  2
#define BASE_OUTPUT_PIN 2

#define SHOULDER_INPUT_PIN  3
#define SHOULDER_OUTPUT_PIN 3

#define ELBOW_INPUT_PIN  5
#define ELBOW_OUTPUT_PIN 5

#define WRIST_OUTPUT_PIN 6

#define SCOOP_OUTPUT_PIN 7
#define SCOOP_OPEN	1200
#define SCOOP_CLOSE	1800

#define DC_MOTOR_PIN 10

#define CMD_TYPE_BIT 0x8000
#define JOINT_TYPE_BITS 0x0c00
#define DATA_BITS 0x03ff

#define BASE_JOINT_CODE 0x0000
#define SHOULDER_JOINT_CODE 0x0400
#define ELBOW_JOINT_CODE 0x0800
#define WRIST_JOINT_CODE 0x0c00

#define QUEUE_FULL_ERROR 'Q'
#define BAD_VALUE_ERROR 'V'

Servo Base_Servo, Shoulder_Servo, Elbow_Servo, Wrist_Servo, Scoop_Servo;

//Define Variables we'll be connecting to
double Base_Setpoint,     Base_Input,     Base_Output;
double Shoulder_Setpoint, Shoulder_Input, Shoulder_Output;
double Elbow_Setpoint,    Elbow_Input,    Elbow_Output;
int Wrist_Output, Scoop_Output;

//Specify the links and initial tuning parameters
PID Base_PID(      &Base_Input,     &Base_Output,     &Base_Setpoint,      BASE_P,     BASE_I,     BASE_D,     DIRECT);
PID Shoulder_PID(  &Shoulder_Input, &Shoulder_Output, &Shoulder_Setpoint,  SHOULDER_P, SHOULDER_I, SHOULDER_D, DIRECT);
PID Elbow_PID(     &Elbow_Input,    &Elbow_Output,    &Elbow_Setpoint,     ELBOW_P,    ELBOW_I,    ELBOW_D,    DIRECT);

QueueArray <int> queue;
int tmp;
boolean goal_reached;

int i = 0;
unsigned char temp_char;

void setup()
{
  // Init everything
  Base_Setpoint =     BASE_SETPOINT_START;
  Shoulder_Setpoint = SHOULDER_SETPOINT_START;
  Elbow_Setpoint =    ELBOW_SETPOINT_START;
  
  Wrist_Output = 1500;
  Scoop_Output = SCOOP_OPEN;
  
  Base_PID.SetSampleTime(    BASE_SAMPLE_TIME);
  Shoulder_PID.SetSampleTime(SHOULDER_SAMPLE_TIME);
  Elbow_PID.SetSampleTime(   ELBOW_SAMPLE_TIME);
  
  Base_PID.SetOutputLimits(BASE_OUT_MIN,BASE_OUT_MAX);
  Shoulder_PID.SetOutputLimits(SHOULDER_OUT_MIN,SHOULDER_OUT_MAX);
  Elbow_PID.SetOutputLimits(ELBOW_OUT_MIN,ELBOW_OUT_MAX);
  
  //turn the PID on
  Base_PID.SetMode(      AUTOMATIC);
  Shoulder_PID.SetMode(  AUTOMATIC);
  Elbow_PID.SetMode(     AUTOMATIC);
  
  // attach servos
  Base_Servo.attach(BASE_OUTPUT_PIN);
  Shoulder_Servo.attach(SHOULDER_OUTPUT_PIN);
  Elbow_Servo.attach(ELBOW_OUTPUT_PIN);
  Wrist_Servo.attach(WRIST_OUTPUT_PIN);
  Scoop_Servo.attach(SCOOP_OUTPUT_PIN);
  
  pinMode(DC_MOTOR_PIN, OUTPUT);
  
  goal_reached = false;
  
  // initialize serial:
  Serial.begin(9600);
}

void loop()
{
  Base_Input =     analogRead(BASE_INPUT_PIN);
  Shoulder_Input = analogRead(SHOULDER_INPUT_PIN);
  Elbow_Input =    analogRead(ELBOW_INPUT_PIN);

//  if (Serial.available() >= 2) {
//    tmp = Serial.read();
//    tmp -= 'm';
//    Base_Setpoint = tmp * 31 + 512;
//    
//    tmp = Serial.read();
//    tmp -= 'm';
//    Shoulder_Setpoint = tmp * 31 + 512;
//    
//    tmp = Serial.read();
//    tmp -= 'm';
//    Elbow_Setpoint = tmp * 31 + 512;
//    
//    tmp = Serial.read();
//    tmp -= 'm';
//    tmp *= 30;
//    Wrist_Output = tmp + 1500;
//  }
  
  Base_PID.Compute();
  Shoulder_PID.Compute();
  Elbow_PID.Compute();
  
  Base_Servo.writeMicroseconds((int)Base_Output + 1476);
  Shoulder_Servo.writeMicroseconds((int)Shoulder_Output + 1500);
  Elbow_Servo.writeMicroseconds((int)Elbow_Output + 1500);
  Wrist_Servo.writeMicroseconds(Wrist_Output);
  Scoop_Servo.writeMicroseconds(Scoop_Output);
  
//  Base_Setpoint = Base_Input;
//  Shoulder_Setpoint = Shoulder_Input;
  
  if ((Base_Input - Base_Setpoint) == 0  &&
      (Shoulder_Input - Shoulder_Setpoint) == 0  &&
      (Elbow_Input - Elbow_Setpoint) == 0     )
  {
    goal_reached = true;
  }
  else {
    goal_reached = false;
  }
  
  if (Serial.available() >= 2) {
    if (queue.count() >= 255) {
      // throw away the int
      Serial.read();
      Serial.read();
      Serial.write(QUEUE_FULL_ERROR);
    } else {
//      temp_char = Serial.read() - 109;
//      tmp = temp_char;
      tmp = Serial.read();
      tmp <<= 8;
      tmp += Serial.read();
//      temp_char = Serial.read() - 109;
//      tmp += temp_char;
      
      if ((tmp & 0x7000) != 0) {
        Serial.write(BAD_VALUE_ERROR);
      }
      else {
        queue.push(tmp);
      }
    }
    Serial.println(queue.count());
  }
  
  if (queue.count() >= 1 && goal_reached) {
    tmp = queue.pop();
    
    if (tmp & CMD_TYPE_BIT) {
      // turn on/off end attachment
      if (tmp & 0x0001) {
        digitalWrite(DC_MOTOR_PIN,HIGH);
        Scoop_Output = SCOOP_CLOSE;
      } else {
        digitalWrite(DC_MOTOR_PIN,LOW);
        Scoop_Output = SCOOP_OPEN;
      }
    } else {
      if ((tmp & JOINT_TYPE_BITS) == BASE_JOINT_CODE) {
        Base_Setpoint = (tmp & DATA_BITS);
        // constrain goal
        if (Base_Setpoint > MAX_BASE_POSE) {
          Base_Setpoint = MAX_BASE_POSE;
        } else if (Base_Setpoint < MIN_BASE_POSE) {
          Base_Setpoint = MIN_BASE_POSE;
        }
      } else if ((tmp & 0x0c00) == SHOULDER_JOINT_CODE) {
        Shoulder_Setpoint = (tmp & DATA_BITS);
        // constrain goal
        if (Shoulder_Setpoint > MAX_SHOULDER_POSE) {
          Shoulder_Setpoint = MAX_SHOULDER_POSE;
        } else if (Shoulder_Setpoint < MIN_SHOULDER_POSE) {
          Shoulder_Setpoint = MIN_SHOULDER_POSE;
        }
      } else if ((tmp & 0x0c00) == ELBOW_JOINT_CODE) {
        Elbow_Setpoint = (tmp & DATA_BITS);
        // constrain goal
        if (Elbow_Setpoint > MAX_ELBOW_POSE) {
          Elbow_Setpoint = MAX_ELBOW_POSE;
        } else if (Elbow_Setpoint < MIN_ELBOW_POSE) {
          Elbow_Setpoint = MIN_ELBOW_POSE;
        }
      } else if ((tmp & 0x0c00) == WRIST_JOINT_CODE) {
        Wrist_Output = (tmp & DATA_BITS) + 1000;
      }
    }
    goal_reached = false;
  }
  
  if (i < 7000) {
    i++;
  } else {
    i = 0;
    Serial.print("\nWrist Output = ");
    Serial.print(Wrist_Output);
    Serial.print("\tWrist_PWM = ");
    Serial.println(Wrist_Servo.readMicroseconds());
    Serial.print("Elbow Input = ");
    Serial.print(Elbow_Input);
    Serial.print("\tElbow Goal = ");
    Serial.print(Elbow_Setpoint);
    Serial.print("\t\tElbow Output = ");
    Serial.println(Elbow_Servo.readMicroseconds());
    Serial.print("Shoulder Input = ");
    Serial.print(Shoulder_Input);
    Serial.print("\tShoulder Goal = ");
    Serial.print(Shoulder_Setpoint);
    Serial.print("\t\tShoulder Output = ");
    Serial.println(Shoulder_Servo.readMicroseconds());
    Serial.print("Base Input = ");
    Serial.print(Base_Input);
    Serial.print("\tBase Goal = ");
    Serial.print(Base_Setpoint);
    Serial.print("\t\tBase Output = ");
    Serial.println(Base_Servo.readMicroseconds());
  }
}