The IoTBot is a WiFi-enabled rover built from the Shadow Robot kit. It is controlled from a web interface running on the Adafruit Huzzah ESP8266 WiFi module and implements a pair of Hall-effect sensors on the motors to keep the wheels spinning at the same speed. Additionally, it uses a Sharp IR sensor to detect walls in front of the robot and prevent it from crashing.
Dependencies: mbed-dev mbed-rtos
Fork of huzzah_helloWorld by
Diff: main.cpp
- Revision:
- 2:2d87957b577b
- Parent:
- 1:046dd94c57ce
--- 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