Revision 2:2d87957b577b, committed 2016-11-02

Comitter:

tanmaybangalore

Date:

Wed Nov 02 17:15:46 2016 +0000

Parent:

1:046dd94c57ce

Commit message:

Commit for code checkoff

Changed in this revision

diff -r 046dd94c57ce -r 2d87957b577b SongPlayer.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SongPlayer.h	Wed Nov 02 17:15:46 2016 +0000
@@ -0,0 +1,41 @@
+#include "mbed.h"
+// new class to play a note on Speaker based on PwmOut class
+class SongPlayer
+{
+public:
+    SongPlayer(PinName pin) : _pin(pin) {
+// _pin(pin) means pass pin to the constructor
+    }
+// class method to play a note based on PwmOut class
+    void PlaySong(float frequency[], float duration[], float volume=1.0) {
+        vol = volume;
+        notecount = 0;
+        _pin.period(1.0/frequency[notecount]);
+        _pin = volume/2.0;
+        noteduration.attach(this,&SongPlayer::nextnote, duration[notecount]);
+        // setup timer to interrupt for next note to play
+        frequencyptr = frequency;
+        durationptr = duration;
+        //returns after first note starts to play
+    }
+    void nextnote();
+private:
+    Timeout noteduration;
+    PwmOut _pin;
+    int notecount;
+    float vol;
+    float * frequencyptr;
+    float * durationptr;
+};
+//Interrupt Routine to play next note
+void SongPlayer::nextnote()
+{
+    _pin = 0.0;
+    notecount++; //setup next note in song
+    if (durationptr[notecount]!=0.0) {
+        _pin.period(1.0/frequencyptr[notecount]);
+        noteduration.attach(this,&SongPlayer::nextnote, durationptr[notecount]);
+        _pin = vol/2.0;
+    } else
+        _pin = 0.0; //turn off on last note
+}
\ No newline at end of file

diff -r 046dd94c57ce -r 2d87957b577b main.cpp
--- a/main.cpp	Mon Oct 31 07:33:45 2016 +0000
+++ b/main.cpp	Wed Nov 02 17:15:46 2016 +0000
@@ -1,5 +1,6 @@
 #include "mbed.h"
 #include "rtos.h"
+#include "SongPlayer.h"
  
 RawSerial  pc(USBTX, USBRX);
 RawSerial  dev(p28,p27);
@@ -15,12 +16,27 @@
 DigitalOut fwdB(p17);
 DigitalOut backB(p16);
 Timer t;
-float speed = 0.9f;
+
+// Set up Hall-effect control
+DigitalIn EncR(p13); // right motor
+DigitalIn EncL(p14); // left motor
+int oldEncR = 0; // Previous encoder values
+int oldEncL = 0;
+int ticksR = 0; // Encoder wheel state change counts
+int ticksL = 0;
+int E; // Error  between the 2 wheel speeds
+float speedL = 0; //PWM speed setting for left wheel (scaled between 0 and 1)
+float speedR = 0; //Same for right wheel
+Ticker Sampler; //Interrupt Routine to sample encoder ticks.
+
+// Distance sensor stuff
+AnalogIn irSensor(p15);         // Sensor
+bool emergencyStop = false;
 
 float timeout = 0.05f;
 int  count,ended;
 char buf[256];
-char motorCmd[] = "fwrd";
+char motorCmd[] = "stop";
  
 /**
  *  Get the reply string from the WiFi module
@@ -73,32 +89,32 @@
            fwdB = 1;
            backA = 0;
            backB = 0;
-           motorACtrl = speed;
-           motorBCtrl = speed;
+           motorACtrl = speedL;
+           motorBCtrl = speedR;
     } else if (strcmp(motorCmd, (char *)"back") == 0) {
            stby = 1;        // Start H-bridge driver
            fwdA = 0;
            fwdB = 0;
            backA = 1;
            backB = 1;
-           motorACtrl = speed;
-           motorBCtrl = speed;
+           motorACtrl = speedL;
+           motorBCtrl = speedR;
     } else if (strcmp(motorCmd, (char *)"left") == 0) {
            stby = 1;        // Start H-bridge driver
            fwdA = 1;
            fwdB = 0;
            backA = 0;
            backB = 1;
-           motorACtrl = speed;
-           motorBCtrl = speed;
+           motorACtrl = speedL;
+           motorBCtrl = speedR;
     } else if (strcmp(motorCmd, (char *)"rght") == 0) {
            stby = 1;        // Start H-bridge driver
            fwdA = 0;
            fwdB = 1;
            backA = 1;
            backB = 0;
-           motorACtrl = speed;
-           motorBCtrl = speed;
+           motorACtrl = speedL;
+           motorBCtrl = speedR;
     } else if (strcmp(motorCmd, (char *)"stop") == 0) {
            stby = 1;        // Start H-bridge driver
            fwdA = 0;
@@ -111,11 +127,36 @@
     }
 }
 
+/**
+ * Sample encoders and find error on right wheel. Assume Left wheel is always 
+ * correct speed.
+ */
+void sampleEncoder()
+{
+    // Make sure the robot is moving forward or reversed and that left wheel hasn't stopped. 
+    if(((strcmp(motorCmd, (char *)"fwrd") == 0) || 
+            (strcmp(motorCmd, (char *)"back") == 0)) && (ticksL != 0 || ticksR != 0)) { 
+        E = ticksL - ticksR; // Find error
+        pc.printf("Error = %d\n\r", E);
+        speedR = speedR + float(E) / 255.0f; // Assign a scaled increment to the right wheel based on error
+        pc.printf("Updating right motor speed to %f\n\r", speedR);
+        runMotor();
+    }
+    ticksR = 0; //Restart the counters
+    ticksL = 0;
+}
+
 void dev_recv()
 {
     led1 = !led1;
     getreply();         // Get the serial message
     getMotorCommand();  // Extract the motor command from the message
+    pc.printf(buf);
+    // Check whether we are in emergency stop mode (robot is too close to a wall)
+    if (emergencyStop == true && strcmp(motorCmd, (char *)"back") != 0) {
+        // If so, only allow a "back" command
+        strcpy(motorCmd, "stop");
+    }
     runMotor();         // Run the motor based on the command
 }
  
@@ -126,6 +167,44 @@
         dev.putc(pc.getc());
     }
 }
+
+/**
+ *  Do distance measurement
+ */
+void distance_thread(void const *args) {
+    SongPlayer speaker(p23);
+    float alert1[2] = {440.0, 0.0};
+    float alert1duration[2] = {0.05, 0.0};
+    float alert2[2] = {880.0, 0.0};
+    float alert2duration[2] = {0.05, 0.0};
+    float alert3[2] = {1760.0, 0.0};
+    float alert3duration[2] = {0.05, 0.0};
+    while(1){
+        float reading = irSensor;
+        pc.printf("Distance: %f\n\r", reading);
+        if (reading > 0.7f) {
+            if (strcmp(motorCmd, (char *)"fwrd") == 0){
+                strcpy(motorCmd, "stop");
+            }
+            emergencyStop = true;
+            runMotor();
+        } else if (reading > 0.65f) {
+            speaker.PlaySong(alert3, alert3duration);
+            emergencyStop = false;
+            Thread::wait(60);
+        } else if (reading > 0.55f) {
+            speaker.PlaySong(alert2, alert2duration);
+            emergencyStop = false;
+            Thread::wait(65);
+        } else if (reading > 0.45f) {
+            speaker.PlaySong(alert1, alert1duration);
+            emergencyStop = false;
+            Thread::wait(70);
+        } else {
+            Thread::wait(20);       // Wait 20 ms
+        }
+    }
+}
  
 int main()
 {
@@ -138,8 +217,31 @@
     // Initialize the motors
     motorACtrl.period(0.01f);
     motorBCtrl.period(0.01f);
+    speedL = 0.9f;
+    speedR = 0.9f;
+    
+    // Initialize hall-effect control
+    Sampler.attach(&sampleEncoder, .1); //Sampler uses sampleEncoder function every 20ms
+    EncL.mode(PullUp); // Use internal pullups
+    EncR.mode(PullUp);
+    
+    Thread t0(distance_thread);
  
     while(1) {
-        sleep();
+        if((strcmp(motorCmd, (char *)"fwrd") == 0) || 
+            (strcmp(motorCmd, (char *)"back") == 0)) { // Only increment tick values if moving forward or reverse
+            int tmpL = EncL;    // Sample the current value of the encoders
+            int tmpR = EncR;
+            pc.printf("Encoder values %d, %d\n\r", tmpL, tmpR);
+            if(tmpL != oldEncL) { // Increment ticks every time the state has switched. 
+                ticksL++;
+                oldEncL = tmpL;
+            }
+            if(tmpR != oldEncR) {
+                ticksR++;
+                oldEncR = tmpR;
+            }
+        }
+        wait(0.02f);        // Sleep 20 ms
     }
 }
\ No newline at end of file

diff -r 046dd94c57ce -r 2d87957b577b mbed-dev.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-dev.lib	Wed Nov 02 17:15:46 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-dev/#156823d33999

diff -r 046dd94c57ce -r 2d87957b577b mbed.bld
--- a/mbed.bld	Mon Oct 31 07:33:45 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/9bcdf88f62b0
\ No newline at end of file