main.cpp

00001 /**
00002  * R2D2 is a mbed robot with the Shadowrobot chassis, two DC motors with feedback control, 
00003  * IR distance sensor, a speaker and a uLCD 
00004 */
00005  
00006 #include "mbed.h"
00007 #include "motordriver.h"
00008 #include "PID.h"
00009 #include "uLCD_4DGL.h"
00010 #include "SongPlayer.h"
00011 //one full on this wheel is ~193 counts
00012 
00013 class Counter {     //interrupt driven rotation counter to be used with the feedback control
00014     public:
00015     Counter(PinName pin) : _interrupt(pin) {        // create the InterruptIn on the pin specified to Counter
00016         _interrupt.rise(this, &Counter::increment); // attach increment function of this counter instance
00017     }
00018  
00019     void increment() {
00020         _count++;
00021     }
00022  
00023     int read() {
00024         return _count;
00025     }
00026  
00027 private:
00028     InterruptIn _interrupt;
00029     volatile int _count;
00030 };
00031 
00032 int distTransform(float input) {    //stepwise transform the IR output voltage to distance
00033     if (input>3) return 6;          //IR sensor datasheet: www.sharp-world.com/products/device/lineup/data/pdf/datasheet/gp2y0a21yk_e.pdf
00034     else if (input>2.5) return 8;
00035     else if (input>2) return 10;
00036     else if (input>1.5) return 14;
00037     else if (input>1.1) return 22;
00038     else if (input>0.9) return 27;
00039     else if (input>0.75) return 35;
00040     else if (input>0.6) return 45;
00041     else if (input>0.5) return 60;
00042     else if (input>0.4) return 75;
00043     else return 99;    
00044 }
00045 
00046 Motor leftMotor(p22, p6, p5, 1); // pwm, fwd, rev, can brake 
00047 Motor rightMotor(p21, p7, p8, 1); // pwm, fwd, rev, can brake
00048 Counter leftPulses(p9), rightPulses (p10);
00049 //Tuning parameters calculated from step tests;
00050 //see http://mbed.org/cookbook/PID for examples.
00051 PID leftPid(0.4620, 0.1, 0.0, 0.01);  //Kc, Ti, Td
00052 PID rightPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td
00053 DigitalOut led(LED1), led2(LED2);   //LED feedback
00054 AnalogIn ain(p15);                  //A/D converter for the IR sensor
00055 uLCD_4DGL uLCD(p28,p27,p29); // serial tx, serial rx, reset pin for the uLCD
00056 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
00057 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
00058 
00059 int main() {
00060     uLCD.printf("\n I am on an\n important\n mission!"); //Initialization
00061     
00062     //Tune PID controllers, based on mbed Rover: https://developer.mbed.org/cookbook/mbed-Rover
00063     leftPid.setInputLimits(0, 3000);    
00064     leftPid.setOutputLimits(0.0, 0.8);
00065     leftPid.setMode(AUTO_MODE);
00066     rightPid.setInputLimits(0, 3000);
00067     rightPid.setOutputLimits(0.0, 0.8);
00068     rightPid.setMode(AUTO_MODE);
00069     
00070     Serial pc(USBTX, USBRX);
00071     SongPlayer mySpeaker(p23);
00072     
00073     int leftPrevPulses  = 0, leftActPulses=0; //pulses from left motor
00074     float leftVelocity  = 0.0; //The velocity of the left wheel in pulses per second
00075     int rightPrevPulses = 0, rightActPulses=0; //pulses from right motor
00076     float rightVelocity = 0.0; //The velocity of the right wheel in pulses per second
00077     
00078     int distance     = 0; //Number of pulses to travel.
00079     led=0;
00080     led2=0;
00081     uLCD.baudrate(3000000); //uLCD baud rate for fast display
00082 
00083     wait(1); //Wait one second before we start moving.
00084     uLCD.cls();
00085     uLCD.locate(1,2);
00086     uLCD.printf("I must find\n Ben Kenobi!");
00087 
00088     //optional motor soft starting to reduce high inrush current
00089     /*leftMotor.speed(0.1);   
00090     rightMotor.speed(0.1);
00091     wait(0.1);*/
00092     
00093     leftPid.setSetPoint(1000);  //set velocity goals for PID
00094     rightPid.setSetPoint(1000);
00095 
00096     while (1) { //start of big while loop
00097         
00098         if (distTransform(ain)>50) {    //if no barrier within 50 cm go in a straight line!
00099             leftActPulses=leftPulses.read();
00100             leftVelocity = (leftActPulses - leftPrevPulses) / 0.01;
00101             leftPrevPulses = leftActPulses;
00102             rightActPulses=rightPulses.read();
00103             rightVelocity = (rightActPulses - rightPrevPulses) / 0.01;
00104             rightPrevPulses = rightActPulses;
00105     
00106             leftPid.setProcessValue(fabs(leftVelocity));
00107             leftMotor.speed(leftPid.compute());
00108             rightPid.setProcessValue(fabs(rightVelocity));
00109             rightMotor.speed(rightPid.compute());
00110 
00111         } else { //if there is a barrier within 50 cm, don't go straight, turn!
00112             leftMotor.stop(0.1);
00113             rightMotor.stop(0.1);
00114             led2=1;     //turn on LED2 when it is turning
00115             uLCD.cls();
00116             mySpeaker.PlaySong(note,duration);  //play R2D2 sound effects
00117             uLCD.filled_circle(64, 64, 63, RED);    //display R2D2 visual effects
00118             wait(0.2);
00119             uLCD.filled_circle(64, 64, 63, 0x0000FF);   //light blue color
00120             wait(0.5);
00121             uLCD.filled_circle(64, 64, 63, RED);
00122             wait(0.3);
00123             //wait(0.5);     
00124             
00125             leftActPulses=leftPulses.read();
00126             rightActPulses=rightPulses.read();
00127             distance=leftActPulses+100;     
00128             while (leftActPulses<distance) { //turn approximately half a revolution
00129                 leftMotor.speed(-0.5);  //rotate to the right
00130                 rightMotor.speed(0.5);  //open loop, because the PID class can't handle negative values
00131                 leftActPulses=leftPulses.read();
00132                 rightActPulses=rightPulses.read();
00133                 
00134                 wait(0.005);
00135                 
00136             }//Turning while end
00137             leftMotor.stop(0.1);
00138             rightMotor.stop(0.1);          
00139             wait(0.1);
00140             led2=0;
00141             uLCD.cls();
00142             uLCD.locate(1,2);
00143             uLCD.printf("I must find\n Ben Kenobi!");
00144             
00145             leftPid.setSetPoint(1000);  //go straight
00146             rightPid.setSetPoint(1000);
00147         
00148         } //Going straight/turning if end 
00149         
00150         //pc.printf("\n%i", distTransform(ain));    //for debugging purposes you can read the distance reading
00151         //uLCD.locate(1,1);
00152         //uLCD.printf("Distance: %i cm", distTransform(ain));
00153         wait(0.01);
00154         led=!led;   //blink led1 to follow changes
00155         
00156     }   //end of big while loop
00157 
00158 }