/* Balancing robot
   - uses 2 modified servos for balancing
   - uses a potentiometer to adjust the perfect balance point
   - uses one Ping))) ultrasonic sensor to read the distance to the surface
   - uses a PID control algorithm to adjust the servo pulses so the Ping)))
     sensor reading will match the value of the perfect balance point
*/ 

//Input/outputs
#define PingPin 7 // digital pin 7
#define PotPin 5  // analog pin 5

#define Lservo  4 // digital pin 4
#define Rservo 5  // digital pin 5

#define Led 13     //digital pin 13                          // I use a jumper to switch
#define Speaker 13                                           // between Led and Speaker

//Variables
unsigned int Ptime = 0;                                      // raw value from sensors
int Drive = 0;                                               // PWM value sent to Servos
int Error[5];                                       // array of 5 Error elements
int P = 0;                                                   // proportional term
int I = 0;                                                   // integral term
int D = 0;                                                   // derivative term
int SetPoint = 307;                                            // perfect balance point value
byte Count = 0;                                              // counter
int Period = 0;                                              // period for generating sounds
int Lwheel = 0;                                              // left wheel variable
int Rwheel = 0;                                              // right wheel variable
int Pot = 0;                                                 // potentiometer variable

//tests should be made to determine acurate Min, Mid and Max values for the servos
#define Midl 1460                                            // center for servos, they should be stoped
#define Midr 1460


//Ping PID constants
#define Kp 2                                                
#define Ki 3                                                
#define Kd 4                                                

//Meaningful names for error array index:
#define Current 0
#define Sum 1
#define Last 2
#define SecondToLast 3
#define Delta 4


void setup() { 
   
  pinMode(PingPin, OUTPUT);
  digitalWrite(PingPin, LOW);

  pinMode(Speaker, OUTPUT);
  digitalWrite(Speaker, LOW);
  
  pinMode(Lwheel, OUTPUT);
  digitalWrite(Lwheel, LOW);
  pinMode(Rwheel, OUTPUT);
  digitalWrite(Rwheel, LOW);
  
  Serial.begin (19200);
  Serial.println("start");                
  
  //delay(2000);    //wait 2 seconds before start for debug purposes
} 

void loop(){
  Read_Pot_Sensor();
  Read_Ping_Sensor();
  PID();
  Drive_Motors();
  delay(7);     //wait 7 miliseconds, adjust this value for a 18 to 20 ms loop
}  

int Read_Pot_Sensor() {
  Pot = analogRead(PotPin);
  //Serial.print ("Pot = ");                  // debug - remember to comment out
  //Serial.println (Pot, DEC);              // debug - remember to comment out
  return Pot;
}  

int Read_Ping_Sensor() {
  //trigger the sensor
  pinMode(PingPin, OUTPUT);
  digitalWrite(PingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(PingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(PingPin, LOW);
  //receive the echo
  pinMode(PingPin, INPUT);
  digitalWrite(PingPin, HIGH);              //turn on pull up resistor
  Ptime = pulseIn(PingPin, HIGH);
  //print out the value for fine tuning of SetPoint constant
  //Serial.print ("Ping time = ");            // debug - remember to comment out
  //Serial.println (Ptime, DEC);              // debug - remember to comment out
  return Ptime;
}

int PID() {
  //Error[Current] = SetPoint - Ptime;
  Error[Current] = Pot - Ptime;
  P = Error[Current] * Kp;
  Error[Sum] = Error[Current] + Error[Last] + Error[SecondToLast];
  I = Error[Sum] * Ki;
  Error[Delta] = Error[Current] - Error[Last];
  D = Error[Delta] * Kd;
  Drive = P + I + D;
  Error[SecondToLast] = Error[Last];
  Error[Last] = Error[Current];
  Drive = Drive * 4;                       // we have to multiply the result to get in the 1000-2000 interval
  Serial.print ("PID = ");                 // debug - remember to comment out
  Serial.println (Drive, DEC);             // debug - remember to comment out
  return Drive;
}

void Drive_Motors() {
  Lwheel = Midl + Drive;
  digitalWrite(Lservo, HIGH);           
  delayMicroseconds(Lwheel);        
  digitalWrite(Lservo, LOW);    
  //Serial.print ("Left = ");                 // debug - remember to comment out
  //Serial.println (Lwheel, DEC);             // debug - remember to comment out
   
  Rwheel = Midr - Drive;
  digitalWrite(Rservo, HIGH);   
  delayMicroseconds(Rwheel);
  digitalWrite(Rservo, LOW);    
  //Serial.print ("Right = ");                // debug - remember to comment out
  //Serial.println (Rwheel, DEC);             // debug - remember to comment out
}