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
main.cpp
- Committer:
- tanmaybangalore
- Date:
- 2016-11-02
- Revision:
- 2:2d87957b577b
- Parent:
- 1:046dd94c57ce
File content as of revision 2:2d87957b577b:
#include "mbed.h" #include "rtos.h" #include "SongPlayer.h" RawSerial pc(USBTX, USBRX); RawSerial dev(p28,p27); DigitalOut led1(LED1); DigitalOut led4(LED4); // Set up the motor pins PwmOut motorACtrl(p21); PwmOut motorBCtrl(p22); DigitalOut backA(p20); DigitalOut fwdA(p19); DigitalOut stby(p18); DigitalOut fwdB(p17); DigitalOut backB(p16); Timer t; // 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[] = "stop"; /** * Get the reply string from the WiFi module */ void getreply() { memset(buf, '\0', sizeof(buf)); t.start(); ended=0; count=0; while(!ended) { if(dev.readable()) { buf[count] = dev.getc(); count++; } if(t.read() > timeout) { ended = 1; t.stop(); t.reset(); } } } /** * */ void getMotorCommand() { int index = 3; // iterate backwards through the message buffer for (int i = sizeof(buf) - 1; i > 2; i--){ // Check if the current character is a null if (buf[i] != '\0') { motorCmd[index] = buf[i - 2]; // Copy the character if (index == 0){ break; // Break if we have completed getting the command } else { index--; // Decrement otherwise } } } } /** * Run the motor based on the command passed through the serial interface. */ void runMotor(){ if (strcmp(motorCmd, (char *)"fwrd") == 0) { stby = 1; // Start H-bridge driver fwdA = 1; fwdB = 1; backA = 0; backB = 0; 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 = 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 = 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 = speedL; motorBCtrl = speedR; } else if (strcmp(motorCmd, (char *)"stop") == 0) { stby = 1; // Start H-bridge driver fwdA = 0; fwdB = 0; backA = 0; backB = 0; motorACtrl = 0.0f; // Set speed to 0 motorBCtrl = 0.0f; stby = 0; // Turn off H-bridge driver } } /** * 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 } void pc_recv() { led4 = !led4; while(pc.readable()) { 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() { pc.baud(115200); dev.baud(115200); pc.attach(&pc_recv, Serial::RxIrq); dev.attach(&dev_recv, Serial::RxIrq); // 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) { 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 } }