/**
 * R2D2 is a mbed robot with the Shadowrobot chassis, two DC motors with feedback control, 
 * IR distance sensor, a speaker and a uLCD 
*/
 
#include "mbed.h"
#include "motordriver.h"
#include "PID.h"
#include "uLCD_4DGL.h"
#include "SongPlayer.h"
//one full on this wheel is ~193 counts

class Counter {     //interrupt driven rotation counter to be used with the feedback control
    public:
    Counter(PinName pin) : _interrupt(pin) {        // create the InterruptIn on the pin specified to Counter
        _interrupt.rise(this, &Counter::increment); // attach increment function of this counter instance
    }
 
    void increment() {
        _count++;
    }
 
    int read() {
        return _count;
    }
 
private:
    InterruptIn _interrupt;
    volatile int _count;
};

int distTransform(float input) {    //stepwise transform the IR output voltage to distance
    if (input>3) return 6;          //IR sensor datasheet: www.sharp-world.com/products/device/lineup/data/pdf/datasheet/gp2y0a21yk_e.pdf
    else if (input>2.5) return 8;
    else if (input>2) return 10;
    else if (input>1.5) return 14;
    else if (input>1.1) return 22;
    else if (input>0.9) return 27;
    else if (input>0.75) return 35;
    else if (input>0.6) return 45;
    else if (input>0.5) return 60;
    else if (input>0.4) return 75;
    else return 99;    
}

Motor leftMotor(p22, p6, p5, 1); // pwm, fwd, rev, can brake 
Motor rightMotor(p21, p7, p8, 1); // pwm, fwd, rev, can brake
Counter leftPulses(p9), rightPulses (p10);
//Tuning parameters calculated from step tests;
//see http://mbed.org/cookbook/PID for examples.
PID leftPid(0.4620, 0.1, 0.0, 0.01);  //Kc, Ti, Td
PID rightPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td
DigitalOut led(LED1), led2(LED2);   //LED feedback
AnalogIn ain(p15);                  //A/D converter for the IR sensor
uLCD_4DGL uLCD(p28,p27,p29); // serial tx, serial rx, reset pin for the uLCD
float note[18]= {3520, 3135.96, 2637.02, 2093, 2349.32, 3951.07, 2793.83, 4186.01, 3520, 3135.96, 2637.02, 2093, 2349.32, 3951.07, 2793.83, 4186.01}; //R2D2 sound effect
float duration[18]= {0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1};   //for a bit of variety, multiple sound samples could be chosen at random

int main() {
    uLCD.printf("\n I am on an\n important\n mission!"); //Initialization
    
    //Tune PID controllers, based on mbed Rover: https://developer.mbed.org/cookbook/mbed-Rover
    leftPid.setInputLimits(0, 3000);    
    leftPid.setOutputLimits(0.0, 0.8);
    leftPid.setMode(AUTO_MODE);
    rightPid.setInputLimits(0, 3000);
    rightPid.setOutputLimits(0.0, 0.8);
    rightPid.setMode(AUTO_MODE);
    
    Serial pc(USBTX, USBRX);
    SongPlayer mySpeaker(p23);
    
    int leftPrevPulses  = 0, leftActPulses=0; //pulses from left motor
    float leftVelocity  = 0.0; //The velocity of the left wheel in pulses per second
    int rightPrevPulses = 0, rightActPulses=0; //pulses from right motor
    float rightVelocity = 0.0; //The velocity of the right wheel in pulses per second
    
    int distance     = 0; //Number of pulses to travel.
    led=0;
    led2=0;
    uLCD.baudrate(3000000); //uLCD baud rate for fast display

    wait(1); //Wait one second before we start moving.
    uLCD.cls();
    uLCD.locate(1,2);
    uLCD.printf("I must find\n Ben Kenobi!");

    //optional motor soft starting to reduce high inrush current
    /*leftMotor.speed(0.1);   
    rightMotor.speed(0.1);
    wait(0.1);*/
    
    leftPid.setSetPoint(1000);  //set velocity goals for PID
    rightPid.setSetPoint(1000);

    while (1) { //start of big while loop
        
        if (distTransform(ain)>50) {    //if no barrier within 50 cm go in a straight line!
            leftActPulses=leftPulses.read();
            leftVelocity = (leftActPulses - leftPrevPulses) / 0.01;
            leftPrevPulses = leftActPulses;
            rightActPulses=rightPulses.read();
            rightVelocity = (rightActPulses - rightPrevPulses) / 0.01;
            rightPrevPulses = rightActPulses;
    
            leftPid.setProcessValue(fabs(leftVelocity));
            leftMotor.speed(leftPid.compute());
            rightPid.setProcessValue(fabs(rightVelocity));
            rightMotor.speed(rightPid.compute());

        } else { //if there is a barrier within 50 cm, don't go straight, turn!
            leftMotor.stop(0.1);
            rightMotor.stop(0.1);
            led2=1;     //turn on LED2 when it is turning
            uLCD.cls();
            mySpeaker.PlaySong(note,duration);  //play R2D2 sound effects
            uLCD.filled_circle(64, 64, 63, RED);    //display R2D2 visual effects
            wait(0.2);
            uLCD.filled_circle(64, 64, 63, 0x0000FF);   //light blue color
            wait(0.5);
            uLCD.filled_circle(64, 64, 63, RED);
            wait(0.3);
            //wait(0.5);     
            
            leftActPulses=leftPulses.read();
            rightActPulses=rightPulses.read();
            distance=leftActPulses+100;     
            while (leftActPulses<distance) { //turn approximately half a revolution
                leftMotor.speed(-0.5);  //rotate to the right
                rightMotor.speed(0.5);  //open loop, because the PID class can't handle negative values
                leftActPulses=leftPulses.read();
                rightActPulses=rightPulses.read();
                
                wait(0.005);
                
            }//Turning while end
            leftMotor.stop(0.1);
            rightMotor.stop(0.1);          
            wait(0.1);
            led2=0;
            uLCD.cls();
            uLCD.locate(1,2);
            uLCD.printf("I must find\n Ben Kenobi!");
            
            leftPid.setSetPoint(1000);  //go straight
            rightPid.setSetPoint(1000);
        
        } //Going straight/turning if end 
        
        //pc.printf("\n%i", distTransform(ain));    //for debugging purposes you can read the distance reading
        //uLCD.locate(1,1);
        //uLCD.printf("Distance: %i cm", distTransform(ain));
        wait(0.01);
        led=!led;   //blink led1 to follow changes
        
    }   //end of big while loop

}