Tanmay Bangalore / 4180_L4_IoTBot

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 ECE 4180 Team Who

main.cpp@2:2d87957b577b, 2016-11-02 (annotated)

Committer:
tanmaybangalore
Date:
Wed Nov 02 17:15:46 2016 +0000
Revision:
2:2d87957b577b
Parent:
1:046dd94c57ce 
Commit for code checkoff

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jhunter029 0:57cec3469a80 1 #include "mbed.h"
tanmaybangalore 1:046dd94c57ce 2 #include "rtos.h"
tanmaybangalore 2:2d87957b577b 3 #include "SongPlayer.h"
jhunter029 0:57cec3469a80 4
tanmaybangalore 1:046dd94c57ce 5 RawSerial pc(USBTX, USBRX);
tanmaybangalore 1:046dd94c57ce 6 RawSerial dev(p28,p27);
jhunter029 0:57cec3469a80 7 DigitalOut led1(LED1);
jhunter029 0:57cec3469a80 8 DigitalOut led4(LED4);
tanmaybangalore 1:046dd94c57ce 9
tanmaybangalore 1:046dd94c57ce 10 // Set up the motor pins
tanmaybangalore 1:046dd94c57ce 11 PwmOut motorACtrl(p21);
tanmaybangalore 1:046dd94c57ce 12 PwmOut motorBCtrl(p22);
tanmaybangalore 1:046dd94c57ce 13 DigitalOut backA(p20);
tanmaybangalore 1:046dd94c57ce 14 DigitalOut fwdA(p19);
tanmaybangalore 1:046dd94c57ce 15 DigitalOut stby(p18);
tanmaybangalore 1:046dd94c57ce 16 DigitalOut fwdB(p17);
tanmaybangalore 1:046dd94c57ce 17 DigitalOut backB(p16);
jhunter029 0:57cec3469a80 18 Timer t;
tanmaybangalore 2:2d87957b577b 19
tanmaybangalore 2:2d87957b577b 20 // Set up Hall-effect control
tanmaybangalore 2:2d87957b577b 21 DigitalIn EncR(p13); // right motor
tanmaybangalore 2:2d87957b577b 22 DigitalIn EncL(p14); // left motor
tanmaybangalore 2:2d87957b577b 23 int oldEncR = 0; // Previous encoder values
tanmaybangalore 2:2d87957b577b 24 int oldEncL = 0;
tanmaybangalore 2:2d87957b577b 25 int ticksR = 0; // Encoder wheel state change counts
tanmaybangalore 2:2d87957b577b 26 int ticksL = 0;
tanmaybangalore 2:2d87957b577b 27 int E; // Error between the 2 wheel speeds
tanmaybangalore 2:2d87957b577b 28 float speedL = 0; //PWM speed setting for left wheel (scaled between 0 and 1)
tanmaybangalore 2:2d87957b577b 29 float speedR = 0; //Same for right wheel
tanmaybangalore 2:2d87957b577b 30 Ticker Sampler; //Interrupt Routine to sample encoder ticks.
tanmaybangalore 2:2d87957b577b 31
tanmaybangalore 2:2d87957b577b 32 // Distance sensor stuff
tanmaybangalore 2:2d87957b577b 33 AnalogIn irSensor(p15); // Sensor
tanmaybangalore 2:2d87957b577b 34 bool emergencyStop = false;
jhunter029 0:57cec3469a80 35
tanmaybangalore 1:046dd94c57ce 36 float timeout = 0.05f;
tanmaybangalore 1:046dd94c57ce 37 int count,ended;
tanmaybangalore 1:046dd94c57ce 38 char buf[256];
tanmaybangalore 2:2d87957b577b 39 char motorCmd[] = "stop";
jhunter029 0:57cec3469a80 40
tanmaybangalore 1:046dd94c57ce 41 /**
tanmaybangalore 1:046dd94c57ce 42 * Get the reply string from the WiFi module
tanmaybangalore 1:046dd94c57ce 43 */
jhunter029 0:57cec3469a80 44 void getreply()
jhunter029 0:57cec3469a80 45 {
jhunter029 0:57cec3469a80 46 memset(buf, '\0', sizeof(buf));
jhunter029 0:57cec3469a80 47 t.start();
jhunter029 0:57cec3469a80 48 ended=0;
jhunter029 0:57cec3469a80 49 count=0;
jhunter029 0:57cec3469a80 50 while(!ended) {
tanmaybangalore 1:046dd94c57ce 51 if(dev.readable()) {
tanmaybangalore 1:046dd94c57ce 52 buf[count] = dev.getc();
jhunter029 0:57cec3469a80 53 count++;
jhunter029 0:57cec3469a80 54 }
jhunter029 0:57cec3469a80 55 if(t.read() > timeout) {
jhunter029 0:57cec3469a80 56 ended = 1;
jhunter029 0:57cec3469a80 57 t.stop();
jhunter029 0:57cec3469a80 58 t.reset();
jhunter029 0:57cec3469a80 59 }
jhunter029 0:57cec3469a80 60 }
jhunter029 0:57cec3469a80 61 }
tanmaybangalore 1:046dd94c57ce 62
tanmaybangalore 1:046dd94c57ce 63 /**
tanmaybangalore 1:046dd94c57ce 64 *
tanmaybangalore 1:046dd94c57ce 65 */
tanmaybangalore 1:046dd94c57ce 66 void getMotorCommand() {
tanmaybangalore 1:046dd94c57ce 67 int index = 3;
tanmaybangalore 1:046dd94c57ce 68 // iterate backwards through the message buffer
tanmaybangalore 1:046dd94c57ce 69 for (int i = sizeof(buf) - 1; i > 2; i--){
tanmaybangalore 1:046dd94c57ce 70 // Check if the current character is a null
tanmaybangalore 1:046dd94c57ce 71 if (buf[i] != '\0') {
tanmaybangalore 1:046dd94c57ce 72 motorCmd[index] = buf[i - 2]; // Copy the character
tanmaybangalore 1:046dd94c57ce 73 if (index == 0){
tanmaybangalore 1:046dd94c57ce 74 break; // Break if we have completed getting the command
tanmaybangalore 1:046dd94c57ce 75 } else {
tanmaybangalore 1:046dd94c57ce 76 index--; // Decrement otherwise
tanmaybangalore 1:046dd94c57ce 77 }
tanmaybangalore 1:046dd94c57ce 78 }
tanmaybangalore 1:046dd94c57ce 79 }
tanmaybangalore 1:046dd94c57ce 80 }
tanmaybangalore 1:046dd94c57ce 81
tanmaybangalore 1:046dd94c57ce 82 /**
tanmaybangalore 1:046dd94c57ce 83 * Run the motor based on the command passed through the serial interface.
tanmaybangalore 1:046dd94c57ce 84 */
tanmaybangalore 1:046dd94c57ce 85 void runMotor(){
tanmaybangalore 1:046dd94c57ce 86 if (strcmp(motorCmd, (char *)"fwrd") == 0) {
tanmaybangalore 1:046dd94c57ce 87 stby = 1; // Start H-bridge driver
tanmaybangalore 1:046dd94c57ce 88 fwdA = 1;
tanmaybangalore 1:046dd94c57ce 89 fwdB = 1;
tanmaybangalore 1:046dd94c57ce 90 backA = 0;
tanmaybangalore 1:046dd94c57ce 91 backB = 0;
tanmaybangalore 2:2d87957b577b 92 motorACtrl = speedL;
tanmaybangalore 2:2d87957b577b 93 motorBCtrl = speedR;
tanmaybangalore 1:046dd94c57ce 94 } else if (strcmp(motorCmd, (char *)"back") == 0) {
tanmaybangalore 1:046dd94c57ce 95 stby = 1; // Start H-bridge driver
tanmaybangalore 1:046dd94c57ce 96 fwdA = 0;
tanmaybangalore 1:046dd94c57ce 97 fwdB = 0;
tanmaybangalore 1:046dd94c57ce 98 backA = 1;
tanmaybangalore 1:046dd94c57ce 99 backB = 1;
tanmaybangalore 2:2d87957b577b 100 motorACtrl = speedL;
tanmaybangalore 2:2d87957b577b 101 motorBCtrl = speedR;
tanmaybangalore 1:046dd94c57ce 102 } else if (strcmp(motorCmd, (char *)"left") == 0) {
tanmaybangalore 1:046dd94c57ce 103 stby = 1; // Start H-bridge driver
tanmaybangalore 1:046dd94c57ce 104 fwdA = 1;
tanmaybangalore 1:046dd94c57ce 105 fwdB = 0;
tanmaybangalore 1:046dd94c57ce 106 backA = 0;
tanmaybangalore 1:046dd94c57ce 107 backB = 1;
tanmaybangalore 2:2d87957b577b 108 motorACtrl = speedL;
tanmaybangalore 2:2d87957b577b 109 motorBCtrl = speedR;
tanmaybangalore 1:046dd94c57ce 110 } else if (strcmp(motorCmd, (char *)"rght") == 0) {
tanmaybangalore 1:046dd94c57ce 111 stby = 1; // Start H-bridge driver
tanmaybangalore 1:046dd94c57ce 112 fwdA = 0;
tanmaybangalore 1:046dd94c57ce 113 fwdB = 1;
tanmaybangalore 1:046dd94c57ce 114 backA = 1;
tanmaybangalore 1:046dd94c57ce 115 backB = 0;
tanmaybangalore 2:2d87957b577b 116 motorACtrl = speedL;
tanmaybangalore 2:2d87957b577b 117 motorBCtrl = speedR;
tanmaybangalore 1:046dd94c57ce 118 } else if (strcmp(motorCmd, (char *)"stop") == 0) {
tanmaybangalore 1:046dd94c57ce 119 stby = 1; // Start H-bridge driver
tanmaybangalore 1:046dd94c57ce 120 fwdA = 0;
tanmaybangalore 1:046dd94c57ce 121 fwdB = 0;
tanmaybangalore 1:046dd94c57ce 122 backA = 0;
tanmaybangalore 1:046dd94c57ce 123 backB = 0;
tanmaybangalore 1:046dd94c57ce 124 motorACtrl = 0.0f; // Set speed to 0
tanmaybangalore 1:046dd94c57ce 125 motorBCtrl = 0.0f;
tanmaybangalore 1:046dd94c57ce 126 stby = 0; // Turn off H-bridge driver
tanmaybangalore 1:046dd94c57ce 127 }
tanmaybangalore 1:046dd94c57ce 128 }
tanmaybangalore 1:046dd94c57ce 129
tanmaybangalore 2:2d87957b577b 130 /**
tanmaybangalore 2:2d87957b577b 131 * Sample encoders and find error on right wheel. Assume Left wheel is always
tanmaybangalore 2:2d87957b577b 132 * correct speed.
tanmaybangalore 2:2d87957b577b 133 */
tanmaybangalore 2:2d87957b577b 134 void sampleEncoder()
tanmaybangalore 2:2d87957b577b 135 {
tanmaybangalore 2:2d87957b577b 136 // Make sure the robot is moving forward or reversed and that left wheel hasn't stopped.
tanmaybangalore 2:2d87957b577b 137 if(((strcmp(motorCmd, (char *)"fwrd") == 0) ||
tanmaybangalore 2:2d87957b577b 138 (strcmp(motorCmd, (char *)"back") == 0)) && (ticksL != 0 || ticksR != 0)) {
tanmaybangalore 2:2d87957b577b 139 E = ticksL - ticksR; // Find error
tanmaybangalore 2:2d87957b577b 140 pc.printf("Error = %d\n\r", E);
tanmaybangalore 2:2d87957b577b 141 speedR = speedR + float(E) / 255.0f; // Assign a scaled increment to the right wheel based on error
tanmaybangalore 2:2d87957b577b 142 pc.printf("Updating right motor speed to %f\n\r", speedR);
tanmaybangalore 2:2d87957b577b 143 runMotor();
tanmaybangalore 2:2d87957b577b 144 }
tanmaybangalore 2:2d87957b577b 145 ticksR = 0; //Restart the counters
tanmaybangalore 2:2d87957b577b 146 ticksL = 0;
tanmaybangalore 2:2d87957b577b 147 }
tanmaybangalore 2:2d87957b577b 148
tanmaybangalore 1:046dd94c57ce 149 void dev_recv()
tanmaybangalore 1:046dd94c57ce 150 {
tanmaybangalore 1:046dd94c57ce 151 led1 = !led1;
tanmaybangalore 1:046dd94c57ce 152 getreply(); // Get the serial message
tanmaybangalore 1:046dd94c57ce 153 getMotorCommand(); // Extract the motor command from the message
tanmaybangalore 2:2d87957b577b 154 pc.printf(buf);
tanmaybangalore 2:2d87957b577b 155 // Check whether we are in emergency stop mode (robot is too close to a wall)
tanmaybangalore 2:2d87957b577b 156 if (emergencyStop == true && strcmp(motorCmd, (char *)"back") != 0) {
tanmaybangalore 2:2d87957b577b 157 // If so, only allow a "back" command
tanmaybangalore 2:2d87957b577b 158 strcpy(motorCmd, "stop");
tanmaybangalore 2:2d87957b577b 159 }
tanmaybangalore 1:046dd94c57ce 160 runMotor(); // Run the motor based on the command
tanmaybangalore 1:046dd94c57ce 161 }
jhunter029 0:57cec3469a80 162
tanmaybangalore 1:046dd94c57ce 163 void pc_recv()
tanmaybangalore 1:046dd94c57ce 164 {
tanmaybangalore 1:046dd94c57ce 165 led4 = !led4;
tanmaybangalore 1:046dd94c57ce 166 while(pc.readable()) {
tanmaybangalore 1:046dd94c57ce 167 dev.putc(pc.getc());
tanmaybangalore 1:046dd94c57ce 168 }
tanmaybangalore 1:046dd94c57ce 169 }
tanmaybangalore 2:2d87957b577b 170
tanmaybangalore 2:2d87957b577b 171 /**
tanmaybangalore 2:2d87957b577b 172 * Do distance measurement
tanmaybangalore 2:2d87957b577b 173 */
tanmaybangalore 2:2d87957b577b 174 void distance_thread(void const *args) {
tanmaybangalore 2:2d87957b577b 175 SongPlayer speaker(p23);
tanmaybangalore 2:2d87957b577b 176 float alert1[2] = {440.0, 0.0};
tanmaybangalore 2:2d87957b577b 177 float alert1duration[2] = {0.05, 0.0};
tanmaybangalore 2:2d87957b577b 178 float alert2[2] = {880.0, 0.0};
tanmaybangalore 2:2d87957b577b 179 float alert2duration[2] = {0.05, 0.0};
tanmaybangalore 2:2d87957b577b 180 float alert3[2] = {1760.0, 0.0};
tanmaybangalore 2:2d87957b577b 181 float alert3duration[2] = {0.05, 0.0};
tanmaybangalore 2:2d87957b577b 182 while(1){
tanmaybangalore 2:2d87957b577b 183 float reading = irSensor;
tanmaybangalore 2:2d87957b577b 184 pc.printf("Distance: %f\n\r", reading);
tanmaybangalore 2:2d87957b577b 185 if (reading > 0.7f) {
tanmaybangalore 2:2d87957b577b 186 if (strcmp(motorCmd, (char *)"fwrd") == 0){
tanmaybangalore 2:2d87957b577b 187 strcpy(motorCmd, "stop");
tanmaybangalore 2:2d87957b577b 188 }
tanmaybangalore 2:2d87957b577b 189 emergencyStop = true;
tanmaybangalore 2:2d87957b577b 190 runMotor();
tanmaybangalore 2:2d87957b577b 191 } else if (reading > 0.65f) {
tanmaybangalore 2:2d87957b577b 192 speaker.PlaySong(alert3, alert3duration);
tanmaybangalore 2:2d87957b577b 193 emergencyStop = false;
tanmaybangalore 2:2d87957b577b 194 Thread::wait(60);
tanmaybangalore 2:2d87957b577b 195 } else if (reading > 0.55f) {
tanmaybangalore 2:2d87957b577b 196 speaker.PlaySong(alert2, alert2duration);
tanmaybangalore 2:2d87957b577b 197 emergencyStop = false;
tanmaybangalore 2:2d87957b577b 198 Thread::wait(65);
tanmaybangalore 2:2d87957b577b 199 } else if (reading > 0.45f) {
tanmaybangalore 2:2d87957b577b 200 speaker.PlaySong(alert1, alert1duration);
tanmaybangalore 2:2d87957b577b 201 emergencyStop = false;
tanmaybangalore 2:2d87957b577b 202 Thread::wait(70);
tanmaybangalore 2:2d87957b577b 203 } else {
tanmaybangalore 2:2d87957b577b 204 Thread::wait(20); // Wait 20 ms
tanmaybangalore 2:2d87957b577b 205 }
tanmaybangalore 2:2d87957b577b 206 }
tanmaybangalore 2:2d87957b577b 207 }
tanmaybangalore 1:046dd94c57ce 208
tanmaybangalore 1:046dd94c57ce 209 int main()
tanmaybangalore 1:046dd94c57ce 210 {
tanmaybangalore 1:046dd94c57ce 211 pc.baud(115200);
tanmaybangalore 1:046dd94c57ce 212 dev.baud(115200);
tanmaybangalore 1:046dd94c57ce 213
tanmaybangalore 1:046dd94c57ce 214 pc.attach(&pc_recv, Serial::RxIrq);
tanmaybangalore 1:046dd94c57ce 215 dev.attach(&dev_recv, Serial::RxIrq);
tanmaybangalore 1:046dd94c57ce 216
tanmaybangalore 1:046dd94c57ce 217 // Initialize the motors
tanmaybangalore 1:046dd94c57ce 218 motorACtrl.period(0.01f);
tanmaybangalore 1:046dd94c57ce 219 motorBCtrl.period(0.01f);
tanmaybangalore 2:2d87957b577b 220 speedL = 0.9f;
tanmaybangalore 2:2d87957b577b 221 speedR = 0.9f;
tanmaybangalore 2:2d87957b577b 222
tanmaybangalore 2:2d87957b577b 223 // Initialize hall-effect control
tanmaybangalore 2:2d87957b577b 224 Sampler.attach(&sampleEncoder, .1); //Sampler uses sampleEncoder function every 20ms
tanmaybangalore 2:2d87957b577b 225 EncL.mode(PullUp); // Use internal pullups
tanmaybangalore 2:2d87957b577b 226 EncR.mode(PullUp);
tanmaybangalore 2:2d87957b577b 227
tanmaybangalore 2:2d87957b577b 228 Thread t0(distance_thread);
tanmaybangalore 1:046dd94c57ce 229
tanmaybangalore 1:046dd94c57ce 230 while(1) {
tanmaybangalore 2:2d87957b577b 231 if((strcmp(motorCmd, (char *)"fwrd") == 0) ||
tanmaybangalore 2:2d87957b577b 232 (strcmp(motorCmd, (char *)"back") == 0)) { // Only increment tick values if moving forward or reverse
tanmaybangalore 2:2d87957b577b 233 int tmpL = EncL; // Sample the current value of the encoders
tanmaybangalore 2:2d87957b577b 234 int tmpR = EncR;
tanmaybangalore 2:2d87957b577b 235 pc.printf("Encoder values %d, %d\n\r", tmpL, tmpR);
tanmaybangalore 2:2d87957b577b 236 if(tmpL != oldEncL) { // Increment ticks every time the state has switched.
tanmaybangalore 2:2d87957b577b 237 ticksL++;
tanmaybangalore 2:2d87957b577b 238 oldEncL = tmpL;
tanmaybangalore 2:2d87957b577b 239 }
tanmaybangalore 2:2d87957b577b 240 if(tmpR != oldEncR) {
tanmaybangalore 2:2d87957b577b 241 ticksR++;
tanmaybangalore 2:2d87957b577b 242 oldEncR = tmpR;
tanmaybangalore 2:2d87957b577b 243 }
tanmaybangalore 2:2d87957b577b 244 }
tanmaybangalore 2:2d87957b577b 245 wait(0.02f); // Sleep 20 ms
tanmaybangalore 1:046dd94c57ce 246 }
tanmaybangalore 1:046dd94c57ce 247 }