Revision 6:9dc165a89453, committed 2016-03-16

Comitter:

Szilard

Date:

Wed Mar 16 17:37:23 2016 +0000

Parent:

5:a8f6ac485b5d

Commit message:

published version

Changed in this revision

diff -r a8f6ac485b5d -r 9dc165a89453 main.cpp
--- a/main.cpp	Wed Mar 16 07:24:04 2016 +0000
+++ b/main.cpp	Wed Mar 16 17:37:23 2016 +0000
@@ -1,21 +1,16 @@
 /**
- * Drive a robot forwards or backwards by using a PID controller to vary
- * the PWM signal to H-bridges connected to the motors to attempt to maintain
- * a constant velocity.
- */
- /*Sources:     mbed Rover cookbook page: https://developer.mbed.org/cookbook/mbed-Rover
-                InterruptIn handbook page: https://developer.mbed.org/handbook/InterruptIn 
- */
+ * 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 revolution=193 counts
+//one full on this wheel is ~193 counts
 
-
-class Counter {
+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
@@ -34,8 +29,8 @@
     volatile int _count;
 };
 
-int distTransform(float input) {
-    if (input>3) return 6;
+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;
@@ -53,55 +48,54 @@
 Counter leftPulses(p9), rightPulses (p10);
 //Tuning parameters calculated from step tests;
 //see http://mbed.org/cookbook/PID for examples.
-//PID leftPid(0.4312, 0.1, 0.0, 0.01);  //Kc, Ti, Td old
 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);
-AnalogIn ain(p15);
-uLCD_4DGL uLCD(p28,p27,p29); // serial tx, serial rx, reset pin;
-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};
-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};
+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!"); //Default Green on black text
+    uLCD.printf("\n I am on an\n important\n mission!"); //Initialization
     
-    //Input and output limits have been determined
-    //empirically with the specific motors being used.
-    //Change appropriately for different motors.
-    //Input  units: counts per second.
-    //Output units: PwmOut duty cycle as %.
-    //Default limits are for moving forward.
-    leftPid.setInputLimits(0, 3000);
+    //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; //The previous reading of how far the left wheel
-    float leftVelocity  = 0.0; //The velocity of the left wheel in pulses per
-    int rightPrevPulses = 0, rightActPulses=0; //The previous reading of how far the right wheel
-    float rightVelocity = 0.0; //The velocity of the right wheel in pulses per
+    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.baudrate(3000000); //uLCD baud rate for fast display
 
-    wait(1); //Wait a few seconds before we start moving.
+    wait(1); //Wait one second before we start moving.
     uLCD.cls();
     uLCD.locate(1,2);
     uLCD.printf("I must find\n Ben Kenobi!");
 
-    //Velocity to mantain in pulses per second.
-    leftPid.setSetPoint(1000);
+    //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) {
+    while (1) { //start of big while loop
         
-        if (distTransform(ain)>50) {    //going straight line
+        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;
@@ -114,54 +108,50 @@
             rightPid.setProcessValue(fabs(rightVelocity));
             rightMotor.speed(rightPid.compute());
 
-        } else { //Don't go straight, turn!
-            leftMotor.stop(0.5);
-            rightMotor.stop(0.5);
-            led2=1;
+        } 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();
-            uLCD.locate(1,2);
-            //uLCD.printf("He is not here!");
-            mySpeaker.PlaySong(note,duration);
-            uLCD.filled_circle(64, 64, 63, RED);
+            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);
+            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);
-            leftPid.setSetPoint(-500);
-            rightPid.setSetPoint(500);
+            //wait(0.5);     
             
             leftActPulses=leftPulses.read();
             rightActPulses=rightPulses.read();
-            distance=leftActPulses+100;
-            while (leftActPulses<distance) { //I'm turning!
-                leftMotor.speed(-0.5);
-                rightMotor.speed(0.5);
+            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.5);
-            rightMotor.stop(0.5);          
+            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);
+            leftPid.setSetPoint(1000);  //go straight
             rightPid.setSetPoint(1000);
         
         } //Going straight/turning if end 
         
-        //pc.printf("\n%i", distTransform(ain));
+        //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;
+        led=!led;   //blink led1 to follow changes
         
     }   //end of big while loop