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@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?
User | Revision | Line number | New 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 | } |