Prana Koirala / Mbed 2 deprecated Wifi_controlled_Robot_ECE4180_Lab4

Wifi Controlled Robot is done as a mini project (Lab4) for ECE 4180 class (Georgia Tech). Robot is assembled in Sparkfun's Shadow chasis robot kit. This robot takes the command from the webpage to move forward, reverse without colliding. The distance sensor is used as a mechanism for collision detection and play a short beep sound. It turn 90 degree with magnetometer when the turn command is sent.

Dependencies:   Motordriver mbed

/media/uploads/pkoirala3/capture.jpg Showing the setup and calibration: Part1 In this part the Wifi setups and attains an IP address so we can control it through a webpage on our phone/PC. After it obtains an IP address it calibrates the IMU so we can have accurate compass headings for precise turns.

The parts that we use are as follows:

  • Mbed
  • 2 DC motors
  • H-Bridge (to drive the DC motors)
  • Speaker
  • Class D Amp (to drive the Speaker)
  • IMU (use the magnometer for compass headings)
  • Wifi card (ESP8266)

Showing webpage functionality and Robot Functionality: In this part we demonstrate the functionality of the robot and the webpage to control it.

Final Code

[Repository '/teams/Prana-Koirala/code/Wifi_controlled_Robot_ECE4180/docs/tip/main_8cpp_source.html' not found]

main.cpp@3:fff9904d2ecb, 2017-03-13 (annotated)

Committer:
pkoirala3
Date:
Mon Mar 13 05:23:32 2017 +0000
Revision:
3:fff9904d2ecb
Parent:
1:bbe16318d747
Child:
4:166570fa7fda
Revised Bug fixed, IMU controlled has issue, instead robot turn implemented with time delay.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pkoirala3 0:553fab92a347 1 #include "mbed.h"
pkoirala3 0:553fab92a347 2 #include "motordriver.h"
pkoirala3 0:553fab92a347 3 #include "ultrasonic.h"
pkoirala3 1:bbe16318d747 4 #include "LSM9DS1.h"
pkoirala3 1:bbe16318d747 5 #define PI 3.14159
pkoirala3 1:bbe16318d747 6 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
pkoirala3 0:553fab92a347 7
pkoirala3 0:553fab92a347 8 DigitalOut ledFwd(p8);
pkoirala3 3:fff9904d2ecb 9 DigitalOut ledRev(p11);
pkoirala3 0:553fab92a347 10
pkoirala3 0:553fab92a347 11 Motor A(p26, p16, p25, 1); // pwm, fwd, rev, can brake
pkoirala3 0:553fab92a347 12 Motor B(p21, p22, p23, 1); // pwm, fwd, rev, can brake
pkoirala3 0:553fab92a347 13
pkoirala3 0:553fab92a347 14 Serial pc(USBTX, USBRX);
pkoirala3 0:553fab92a347 15 Serial esp(p28, p27); // tx, rx
pkoirala3 0:553fab92a347 16
pkoirala3 0:553fab92a347 17 PwmOut mySpeaker(p24);
pkoirala3 0:553fab92a347 18
pkoirala3 3:fff9904d2ecb 19 // LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
pkoirala3 0:553fab92a347 20
pkoirala3 1:bbe16318d747 21 AnalogIn DistSensorFwd(p19);
pkoirala3 1:bbe16318d747 22 AnalogIn DistSensorRev(p20);
pkoirala3 0:553fab92a347 23
pkoirala3 1:bbe16318d747 24 // variables for sending/receiving data over serial
pkoirala3 0:553fab92a347 25 volatile int tx_in=0;
pkoirala3 0:553fab92a347 26 volatile int tx_out=0;
pkoirala3 0:553fab92a347 27 volatile int rx_in=0;
pkoirala3 0:553fab92a347 28 volatile int rx_out=0;
pkoirala3 0:553fab92a347 29 const int buffer_size = 4095;
pkoirala3 0:553fab92a347 30 char tx_buffer[buffer_size+1];
pkoirala3 0:553fab92a347 31 char rx_buffer[buffer_size+1];
pkoirala3 0:553fab92a347 32 void Tx_interrupt();
pkoirala3 0:553fab92a347 33 void Rx_interrupt();
pkoirala3 0:553fab92a347 34 void send_line();
pkoirala3 0:553fab92a347 35 void read_line();
pkoirala3 0:553fab92a347 36 int DataRX;
pkoirala3 0:553fab92a347 37 int update;
pkoirala3 0:553fab92a347 38 char cmdbuff[1024];
pkoirala3 0:553fab92a347 39 char replybuff[4096];
pkoirala3 3:fff9904d2ecb 40 char webdata[4096];
pkoirala3 3:fff9904d2ecb 41 char webbuff[4096];
pkoirala3 3:fff9904d2ecb 42 char stateBuff[50];
pkoirala3 0:553fab92a347 43 void SendCMD(),getreply(),ReadWebData(),startserver();
pkoirala3 3:fff9904d2ecb 44 void ImuCheck(), ObjDetect(float);
pkoirala3 0:553fab92a347 45 char rx_line[1024];
pkoirala3 1:bbe16318d747 46 int port = 80; // set server port
pkoirala3 1:bbe16318d747 47 int SERVtimeout = 5; // set server timeout in seconds in case link breaks.
pkoirala3 0:553fab92a347 48
pkoirala3 0:553fab92a347 49 int main()
pkoirala3 0:553fab92a347 50 {
pkoirala3 0:553fab92a347 51 pc.baud(9600);
pkoirala3 0:553fab92a347 52 esp.baud(9600);
pkoirala3 1:bbe16318d747 53 esp.attach(&Rx_interrupt, Serial::RxIrq); //serial interrupt to receive data
pkoirala3 1:bbe16318d747 54 esp.attach(&Tx_interrupt, Serial::TxIrq); //serial interrupt to transmit data
pkoirala3 0:553fab92a347 55 startserver();
pkoirala3 0:553fab92a347 56 wait(0.01);
pkoirala3 0:553fab92a347 57 DataRX=0;
pkoirala3 1:bbe16318d747 58
pkoirala3 3:fff9904d2ecb 59 mySpeaker.period(1.0/200.0); // 500hz period
pkoirala3 3:fff9904d2ecb 60 A.stop(0.0);
pkoirala3 3:fff9904d2ecb 61 B.stop(0.0);
pkoirala3 3:fff9904d2ecb 62 /*
pkoirala3 3:fff9904d2ecb 63 IMU.begin();
pkoirala3 3:fff9904d2ecb 64 IMU.calibrate(1);
pkoirala3 3:fff9904d2ecb 65 IMU.calibrateMag(0);
pkoirala3 3:fff9904d2ecb 66 pc.printf("Final Calibration done, prana");
pkoirala3 3:fff9904d2ecb 67 */
pkoirala3 0:553fab92a347 68 while(1) {
pkoirala3 0:553fab92a347 69 if(DataRX==1) {
pkoirala3 0:553fab92a347 70 ReadWebData();
pkoirala3 0:553fab92a347 71 esp.attach(&Rx_interrupt, Serial::RxIrq);
pkoirala3 0:553fab92a347 72 }
pkoirala3 3:fff9904d2ecb 73
pkoirala3 0:553fab92a347 74 float stateA = A.state();
pkoirala3 0:553fab92a347 75 float stateB = B.state();
pkoirala3 3:fff9904d2ecb 76 char temp_buff[50];
pkoirala3 3:fff9904d2ecb 77 if(abs(stateA) > 1 && abs(stateB) > 1) strcpy(temp_buff, "Robot Stopped!");
pkoirala3 3:fff9904d2ecb 78 else if((stateA > 0 && stateA <= 1)&&(stateB > 0 && stateB <= 1)) { // obstacle within 10 cm and robot is still moving
pkoirala3 1:bbe16318d747 79 float Dist = DistSensorFwd;
pkoirala3 3:fff9904d2ecb 80 ObjDetect(Dist);
pkoirala3 3:fff9904d2ecb 81 strcpy(temp_buff, "Robot Moving Forward!");
pkoirala3 0:553fab92a347 82 } else if((stateA < 0 && stateA >= -1)&&(stateB < 0 && stateB >= -1)) { // If moving rev
pkoirala3 1:bbe16318d747 83 float Dist = DistSensorRev;
pkoirala3 3:fff9904d2ecb 84 ObjDetect(Dist);
pkoirala3 3:fff9904d2ecb 85 strcpy(temp_buff, "Robot Moving Reverse!");
pkoirala3 3:fff9904d2ecb 86 } else if(abs(stateA) > 0 && abs(stateA) <= 1 && abs(stateB) > 1) {
pkoirala3 3:fff9904d2ecb 87 strcpy(temp_buff, "Robot turning right!");
pkoirala3 3:fff9904d2ecb 88 } else if(abs(stateB) > 0 && abs(stateB) <= 1 && abs(stateA) > 1) {
pkoirala3 3:fff9904d2ecb 89 strcpy(temp_buff, "Robot turning left!");
pkoirala3 3:fff9904d2ecb 90 } else strcpy(temp_buff, "Error getting Robot state");
pkoirala3 3:fff9904d2ecb 91
pkoirala3 3:fff9904d2ecb 92 if(update==1) { // update the state of Robot in webpage
pkoirala3 3:fff9904d2ecb 93 sprintf(stateBuff, "%s",temp_buff);
pkoirala3 3:fff9904d2ecb 94 sprintf(cmdbuff, "BotState=\"%s\"\r\n",stateBuff);
pkoirala3 3:fff9904d2ecb 95 SendCMD();
pkoirala3 3:fff9904d2ecb 96 getreply();
pkoirala3 3:fff9904d2ecb 97 update=0;
pkoirala3 0:553fab92a347 98 }
pkoirala3 0:553fab92a347 99 }
pkoirala3 0:553fab92a347 100 }
pkoirala3 0:553fab92a347 101
pkoirala3 0:553fab92a347 102 // Reads and processes GET and POST web data
pkoirala3 0:553fab92a347 103 void ReadWebData()
pkoirala3 0:553fab92a347 104 {
pkoirala3 0:553fab92a347 105 wait_ms(200);
pkoirala3 0:553fab92a347 106 esp.attach(NULL,Serial::RxIrq);
pkoirala3 0:553fab92a347 107 DataRX=0;
pkoirala3 0:553fab92a347 108 memset(webdata, '\0', sizeof(webdata));
pkoirala3 0:553fab92a347 109 strcpy(webdata, rx_buffer);
pkoirala3 0:553fab92a347 110 memset(rx_buffer, '\0', sizeof(rx_buffer));
pkoirala3 0:553fab92a347 111 rx_in = 0;
pkoirala3 0:553fab92a347 112 rx_out = 0;
pkoirala3 0:553fab92a347 113 // check web data for form information
pkoirala3 0:553fab92a347 114 if( strstr(webdata, "check=BotFwd") != NULL ) {
pkoirala3 3:fff9904d2ecb 115 if(abs(A.state()) <= 1) A.stop(0.0);
pkoirala3 3:fff9904d2ecb 116 if(abs(B.state()) <= 1) B.stop(0.0);
pkoirala3 0:553fab92a347 117 A.speed(0.75);
pkoirala3 0:553fab92a347 118 B.speed(0.75);
pkoirala3 0:553fab92a347 119 ledRev = 0;
pkoirala3 0:553fab92a347 120 ledFwd = 1;
pkoirala3 0:553fab92a347 121 }
pkoirala3 0:553fab92a347 122 if( strstr(webdata, "check=BotRev") != NULL ) {
pkoirala3 3:fff9904d2ecb 123 if(abs(A.state()) <= 1) A.stop(0.0);
pkoirala3 3:fff9904d2ecb 124 if(abs(B.state()) <= 1) B.stop(0.0);
pkoirala3 0:553fab92a347 125 A.speed(-0.75);
pkoirala3 0:553fab92a347 126 B.speed(-0.75);
pkoirala3 0:553fab92a347 127 ledFwd = 0;
pkoirala3 0:553fab92a347 128 ledRev = 1;
pkoirala3 0:553fab92a347 129 }
pkoirala3 0:553fab92a347 130 if( strstr(webdata, "check=BotStop") != NULL ) {
pkoirala3 3:fff9904d2ecb 131 A.stop(0.0);
pkoirala3 3:fff9904d2ecb 132 B.stop(0.0);
pkoirala3 0:553fab92a347 133 ledFwd = 0;
pkoirala3 0:553fab92a347 134 ledRev = 0;
pkoirala3 0:553fab92a347 135 }
pkoirala3 0:553fab92a347 136 if( strstr(webdata, "check=BotLeft") != NULL ) {
pkoirala3 3:fff9904d2ecb 137 B.stop(0.0);
pkoirala3 3:fff9904d2ecb 138 A.speed(0.75);
pkoirala3 3:fff9904d2ecb 139 // ImuCheck();
pkoirala3 3:fff9904d2ecb 140 wait(1.0);
pkoirala3 3:fff9904d2ecb 141 A.stop(0.0);
pkoirala3 0:553fab92a347 142 }
pkoirala3 0:553fab92a347 143 if( strstr(webdata, "check=BotRight") != NULL ) {
pkoirala3 3:fff9904d2ecb 144 A.stop(0.0);
pkoirala3 0:553fab92a347 145 B.speed(0.5);
pkoirala3 3:fff9904d2ecb 146 // ImuCheck();
pkoirala3 3:fff9904d2ecb 147 wait(1.0);
pkoirala3 3:fff9904d2ecb 148 B.stop(0.0);
pkoirala3 0:553fab92a347 149 }
pkoirala3 0:553fab92a347 150 if( strstr(webdata, "POST") != NULL ) { // set update flag if POST request
pkoirala3 0:553fab92a347 151 update=1;
pkoirala3 0:553fab92a347 152 }
pkoirala3 0:553fab92a347 153 if( strstr(webdata, "GET") != NULL && strstr(webdata, "favicon") == NULL ) { // set update flag for GET request but do not want to update for favicon requests
pkoirala3 0:553fab92a347 154 update=1;
pkoirala3 0:553fab92a347 155 }
pkoirala3 0:553fab92a347 156 }
pkoirala3 1:bbe16318d747 157
pkoirala3 0:553fab92a347 158 // Starts webserver
pkoirala3 0:553fab92a347 159 void startserver()
pkoirala3 0:553fab92a347 160 {
pkoirala3 0:553fab92a347 161 pc.printf("Resetting ESP Device....\r\n");
pkoirala3 0:553fab92a347 162 strcpy(cmdbuff,"node.restart()\r\n");
pkoirala3 0:553fab92a347 163 SendCMD();
pkoirala3 0:553fab92a347 164 wait(2);
pkoirala3 0:553fab92a347 165 getreply();
pkoirala3 0:553fab92a347 166
pkoirala3 0:553fab92a347 167 pc.printf("\nStarting Server.....\r\n> ");
pkoirala3 3:fff9904d2ecb 168
pkoirala3 3:fff9904d2ecb 169 // initial values
pkoirala3 3:fff9904d2ecb 170 sprintf(cmdbuff, "BotState=\"%s\"\r\n",stateBuff);
pkoirala3 3:fff9904d2ecb 171 SendCMD();
pkoirala3 3:fff9904d2ecb 172 getreply();
pkoirala3 3:fff9904d2ecb 173 wait(0.5);
pkoirala3 3:fff9904d2ecb 174
pkoirala3 0:553fab92a347 175 //create server
pkoirala3 0:553fab92a347 176 sprintf(cmdbuff, "srv=net.createServer(net.TCP,%d)\r\n",SERVtimeout);
pkoirala3 0:553fab92a347 177 SendCMD();
pkoirala3 0:553fab92a347 178 getreply();
pkoirala3 0:553fab92a347 179 wait(0.5);
pkoirala3 0:553fab92a347 180 strcpy(cmdbuff,"srv:listen(80,function(conn)\r\n");
pkoirala3 0:553fab92a347 181 SendCMD();
pkoirala3 0:553fab92a347 182 getreply();
pkoirala3 0:553fab92a347 183 wait(0.3);
pkoirala3 0:553fab92a347 184 strcpy(cmdbuff,"conn:on(\"receive\",function(conn,payload) \r\n");
pkoirala3 0:553fab92a347 185 SendCMD();
pkoirala3 0:553fab92a347 186 getreply();
pkoirala3 0:553fab92a347 187 wait(0.3);
pkoirala3 0:553fab92a347 188
pkoirala3 0:553fab92a347 189 //print data to mbed
pkoirala3 0:553fab92a347 190 strcpy(cmdbuff,"print(payload)\r\n");
pkoirala3 0:553fab92a347 191 SendCMD();
pkoirala3 0:553fab92a347 192 getreply();
pkoirala3 0:553fab92a347 193 wait(0.2);
pkoirala3 0:553fab92a347 194
pkoirala3 0:553fab92a347 195 //web page data
pkoirala3 0:553fab92a347 196 strcpy(cmdbuff,"conn:send('<!DOCTYPE html><html><body><h1>ESP8266 Mbed IoT Web Controller</h1>')\r\n");
pkoirala3 0:553fab92a347 197 SendCMD();
pkoirala3 0:553fab92a347 198 getreply();
pkoirala3 0:553fab92a347 199 wait(0.4);
pkoirala3 3:fff9904d2ecb 200
pkoirala3 3:fff9904d2ecb 201 strcpy(cmdbuff,"conn:send('<br>Current State of Robot: '..BotState..'<br><hr>')\r\n");
pkoirala3 3:fff9904d2ecb 202 SendCMD();
pkoirala3 3:fff9904d2ecb 203 getreply();
pkoirala3 3:fff9904d2ecb 204 wait(0.2);
pkoirala3 0:553fab92a347 205 strcpy(cmdbuff,"conn:send('<form method=\"POST\"')\r\n");
pkoirala3 0:553fab92a347 206 SendCMD();
pkoirala3 0:553fab92a347 207 getreply();
pkoirala3 0:553fab92a347 208 wait(0.3);
pkoirala3 0:553fab92a347 209 strcpy(cmdbuff, "conn:send('<p><input type=\"radio\" name=\"check\" value=\"BotFwd\"> Move Robot Forward')\r\n");
pkoirala3 0:553fab92a347 210 SendCMD();
pkoirala3 0:553fab92a347 211 getreply();
pkoirala3 0:553fab92a347 212 wait(0.3);
pkoirala3 0:553fab92a347 213 strcpy(cmdbuff, "conn:send('<p><input type=\"radio\" name=\"check\" value=\"BotRev\"> Move Robot Reverse')\r\n");
pkoirala3 0:553fab92a347 214 SendCMD();
pkoirala3 0:553fab92a347 215 getreply();
pkoirala3 0:553fab92a347 216 wait(0.3);
pkoirala3 0:553fab92a347 217 strcpy(cmdbuff, "conn:send('<p><input type=\"radio\" name=\"check\" value=\"BotStop\"> Stop Robot')\r\n");
pkoirala3 0:553fab92a347 218 SendCMD();
pkoirala3 0:553fab92a347 219 getreply();
pkoirala3 0:553fab92a347 220 wait(0.3);
pkoirala3 0:553fab92a347 221 strcpy(cmdbuff, "conn:send('<p><input type=\"radio\" name=\"check\" value=\"BotLeft\"> Turn Robot Left')\r\n");
pkoirala3 0:553fab92a347 222 SendCMD();
pkoirala3 0:553fab92a347 223 getreply();
pkoirala3 0:553fab92a347 224 wait(0.3);
pkoirala3 0:553fab92a347 225 strcpy(cmdbuff, "conn:send('<p><input type=\"radio\" name=\"check\" value=\"BotRight\"> Turn Robot Right')\r\n");
pkoirala3 0:553fab92a347 226 SendCMD();
pkoirala3 0:553fab92a347 227 getreply();
pkoirala3 0:553fab92a347 228 wait(0.3);
pkoirala3 0:553fab92a347 229 strcpy(cmdbuff,"conn:send('<p><input type=\"submit\" value=\"Send to Robot\"></form>')\r\n");
pkoirala3 0:553fab92a347 230 SendCMD();
pkoirala3 0:553fab92a347 231 getreply();
pkoirala3 0:553fab92a347 232 wait(0.3);
pkoirala3 0:553fab92a347 233 strcpy(cmdbuff, "conn:send('<p><h2>How to use:</h2><ul><li>Select a radiobutton </li><li>Click Send to Robot button</li></ul></body></html>')\r\n");
pkoirala3 0:553fab92a347 234 SendCMD();
pkoirala3 0:553fab92a347 235 getreply();
pkoirala3 0:553fab92a347 236 wait(0.5);
pkoirala3 0:553fab92a347 237 // end web page data
pkoirala3 0:553fab92a347 238 strcpy(cmdbuff, "conn:on(\"sent\",function(conn) conn:close() end)\r\n"); // close current connection
pkoirala3 0:553fab92a347 239 SendCMD();
pkoirala3 0:553fab92a347 240 getreply();
pkoirala3 0:553fab92a347 241 wait(0.3);
pkoirala3 0:553fab92a347 242 strcpy(cmdbuff, "end)\r\n");
pkoirala3 0:553fab92a347 243 SendCMD();
pkoirala3 0:553fab92a347 244 getreply();
pkoirala3 0:553fab92a347 245 wait(0.2);
pkoirala3 0:553fab92a347 246 strcpy(cmdbuff, "end)\r\n");
pkoirala3 0:553fab92a347 247 SendCMD();
pkoirala3 0:553fab92a347 248 getreply();
pkoirala3 0:553fab92a347 249 wait(0.2);
pkoirala3 0:553fab92a347 250
pkoirala3 0:553fab92a347 251 strcpy(cmdbuff, "tmr.alarm(0, 1000, 1, function()\r\n");
pkoirala3 0:553fab92a347 252 SendCMD();
pkoirala3 0:553fab92a347 253 getreply();
pkoirala3 0:553fab92a347 254 wait(0.2);
pkoirala3 0:553fab92a347 255 strcpy(cmdbuff, "if wifi.sta.getip() == nil then\r\n");
pkoirala3 0:553fab92a347 256 SendCMD();
pkoirala3 0:553fab92a347 257 getreply();
pkoirala3 0:553fab92a347 258 wait(0.2);
pkoirala3 0:553fab92a347 259 strcpy(cmdbuff, "print(\"Connecting to AP...\\n\")\r\n");
pkoirala3 0:553fab92a347 260 SendCMD();
pkoirala3 0:553fab92a347 261 getreply();
pkoirala3 0:553fab92a347 262 wait(0.2);
pkoirala3 0:553fab92a347 263 strcpy(cmdbuff, "else\r\n");
pkoirala3 0:553fab92a347 264 SendCMD();
pkoirala3 0:553fab92a347 265 getreply();
pkoirala3 0:553fab92a347 266 wait(0.2);
pkoirala3 0:553fab92a347 267 strcpy(cmdbuff, "ip, nm, gw=wifi.sta.getip()\r\n");
pkoirala3 0:553fab92a347 268 SendCMD();
pkoirala3 0:553fab92a347 269 getreply();
pkoirala3 0:553fab92a347 270 wait(0.2);
pkoirala3 0:553fab92a347 271 strcpy(cmdbuff,"print(\"IP Address: \",ip)\r\n");
pkoirala3 0:553fab92a347 272 SendCMD();
pkoirala3 0:553fab92a347 273 getreply();
pkoirala3 0:553fab92a347 274 wait(0.2);
pkoirala3 0:553fab92a347 275 strcpy(cmdbuff,"tmr.stop(0)\r\n");
pkoirala3 0:553fab92a347 276 SendCMD();
pkoirala3 0:553fab92a347 277 getreply();
pkoirala3 0:553fab92a347 278 wait(0.2);
pkoirala3 0:553fab92a347 279 strcpy(cmdbuff,"end\r\n");
pkoirala3 0:553fab92a347 280 SendCMD();
pkoirala3 0:553fab92a347 281 getreply();
pkoirala3 0:553fab92a347 282 wait(0.2);
pkoirala3 0:553fab92a347 283 strcpy(cmdbuff,"end)\r\n");
pkoirala3 0:553fab92a347 284 SendCMD();
pkoirala3 0:553fab92a347 285 getreply();
pkoirala3 0:553fab92a347 286 wait(0.2);
pkoirala3 0:553fab92a347 287 pc.printf("\n\nReady to go .....\r\n\n");
pkoirala3 0:553fab92a347 288 }
pkoirala3 0:553fab92a347 289
pkoirala3 0:553fab92a347 290 // ESP Command data send
pkoirala3 0:553fab92a347 291 void SendCMD()
pkoirala3 0:553fab92a347 292 {
pkoirala3 0:553fab92a347 293 int i;
pkoirala3 0:553fab92a347 294 char temp_char;
pkoirala3 0:553fab92a347 295 bool empty;
pkoirala3 0:553fab92a347 296 i = 0;
pkoirala3 0:553fab92a347 297 // Start Critical Section - don't interrupt while changing global buffer variables
pkoirala3 0:553fab92a347 298 NVIC_DisableIRQ(UART1_IRQn);
pkoirala3 0:553fab92a347 299 empty = (tx_in == tx_out);
pkoirala3 0:553fab92a347 300 while ((i==0) || (cmdbuff[i-1] != '\n')) {
pkoirala3 0:553fab92a347 301 // Wait if buffer full
pkoirala3 0:553fab92a347 302 if (((tx_in + 1) % buffer_size) == tx_out) {
pkoirala3 0:553fab92a347 303 // End Critical Section - need to let interrupt routine empty buffer by sending
pkoirala3 0:553fab92a347 304 NVIC_EnableIRQ(UART1_IRQn);
pkoirala3 0:553fab92a347 305 while (((tx_in + 1) % buffer_size) == tx_out) {
pkoirala3 0:553fab92a347 306 }
pkoirala3 0:553fab92a347 307 // Start Critical Section - don't interrupt while changing global buffer variables
pkoirala3 0:553fab92a347 308 NVIC_DisableIRQ(UART1_IRQn);
pkoirala3 0:553fab92a347 309 }
pkoirala3 0:553fab92a347 310 tx_buffer[tx_in] = cmdbuff[i];
pkoirala3 0:553fab92a347 311 i++;
pkoirala3 0:553fab92a347 312 tx_in = (tx_in + 1) % buffer_size;
pkoirala3 0:553fab92a347 313 }
pkoirala3 0:553fab92a347 314 if (esp.writeable() && (empty)) {
pkoirala3 0:553fab92a347 315 temp_char = tx_buffer[tx_out];
pkoirala3 0:553fab92a347 316 tx_out = (tx_out + 1) % buffer_size;
pkoirala3 0:553fab92a347 317 // Send first character to start tx interrupts, if stopped
pkoirala3 0:553fab92a347 318 esp.putc(temp_char);
pkoirala3 0:553fab92a347 319 }
pkoirala3 0:553fab92a347 320 // End Critical Section
pkoirala3 0:553fab92a347 321 NVIC_EnableIRQ(UART1_IRQn);
pkoirala3 0:553fab92a347 322 return;
pkoirala3 0:553fab92a347 323 }
pkoirala3 0:553fab92a347 324
pkoirala3 0:553fab92a347 325 // Get Command and ESP status replies
pkoirala3 0:553fab92a347 326 void getreply()
pkoirala3 0:553fab92a347 327 {
pkoirala3 0:553fab92a347 328 read_line();
pkoirala3 0:553fab92a347 329 sscanf(rx_line,replybuff);
pkoirala3 0:553fab92a347 330 }
pkoirala3 0:553fab92a347 331
pkoirala3 0:553fab92a347 332 // Read a line from the large rx buffer from rx interrupt routine
pkoirala3 0:553fab92a347 333 void read_line()
pkoirala3 0:553fab92a347 334 {
pkoirala3 0:553fab92a347 335 int i;
pkoirala3 0:553fab92a347 336 i = 0;
pkoirala3 0:553fab92a347 337 // Start Critical Section - don't interrupt while changing global buffer variables
pkoirala3 0:553fab92a347 338 NVIC_DisableIRQ(UART1_IRQn);
pkoirala3 0:553fab92a347 339 // Loop reading rx buffer characters until end of line character
pkoirala3 0:553fab92a347 340 while ((i==0) || (rx_line[i-1] != '\r')) {
pkoirala3 0:553fab92a347 341 // Wait if buffer empty
pkoirala3 0:553fab92a347 342 if (rx_in == rx_out) {
pkoirala3 0:553fab92a347 343 // End Critical Section - need to allow rx interrupt to get new characters for buffer
pkoirala3 0:553fab92a347 344 NVIC_EnableIRQ(UART1_IRQn);
pkoirala3 0:553fab92a347 345 while (rx_in == rx_out) {
pkoirala3 0:553fab92a347 346 }
pkoirala3 0:553fab92a347 347 // Start Critical Section - don't interrupt while changing global buffer variables
pkoirala3 0:553fab92a347 348 NVIC_DisableIRQ(UART1_IRQn);
pkoirala3 0:553fab92a347 349 }
pkoirala3 0:553fab92a347 350 rx_line[i] = rx_buffer[rx_out];
pkoirala3 0:553fab92a347 351 i++;
pkoirala3 0:553fab92a347 352 rx_out = (rx_out + 1) % buffer_size;
pkoirala3 0:553fab92a347 353 }
pkoirala3 0:553fab92a347 354 // End Critical Section
pkoirala3 0:553fab92a347 355 NVIC_EnableIRQ(UART1_IRQn);
pkoirala3 0:553fab92a347 356 rx_line[i-1] = 0;
pkoirala3 0:553fab92a347 357 return;
pkoirala3 0:553fab92a347 358 }
pkoirala3 0:553fab92a347 359
pkoirala3 0:553fab92a347 360 // Interupt Routine to read in data from serial port
pkoirala3 0:553fab92a347 361 void Rx_interrupt()
pkoirala3 0:553fab92a347 362 {
pkoirala3 0:553fab92a347 363 DataRX=1;
pkoirala3 0:553fab92a347 364 // Loop just in case more than one character is in UART's receive FIFO buffer
pkoirala3 0:553fab92a347 365 // Stop if buffer full
pkoirala3 0:553fab92a347 366 while ((esp.readable()) && (((rx_in + 1) % buffer_size) != rx_out)) {
pkoirala3 0:553fab92a347 367 rx_buffer[rx_in] = esp.getc();
pkoirala3 0:553fab92a347 368 // Uncomment to Echo to USB serial to watch data flow
pkoirala3 0:553fab92a347 369 pc.putc(rx_buffer[rx_in]);
pkoirala3 0:553fab92a347 370 rx_in = (rx_in + 1) % buffer_size;
pkoirala3 0:553fab92a347 371 }
pkoirala3 0:553fab92a347 372 return;
pkoirala3 0:553fab92a347 373 }
pkoirala3 0:553fab92a347 374
pkoirala3 0:553fab92a347 375 // Interupt Routine to write out data to serial port
pkoirala3 0:553fab92a347 376 void Tx_interrupt()
pkoirala3 0:553fab92a347 377 {
pkoirala3 0:553fab92a347 378 // Loop to fill more than one character in UART's transmit FIFO buffer
pkoirala3 0:553fab92a347 379 // Stop if buffer empty
pkoirala3 0:553fab92a347 380 while ((esp.writeable()) && (tx_in != tx_out)) {
pkoirala3 0:553fab92a347 381 esp.putc(tx_buffer[tx_out]);
pkoirala3 0:553fab92a347 382 tx_out = (tx_out + 1) % buffer_size;
pkoirala3 0:553fab92a347 383 }
pkoirala3 0:553fab92a347 384 return;
pkoirala3 1:bbe16318d747 385 }
pkoirala3 1:bbe16318d747 386
pkoirala3 1:bbe16318d747 387 // To make Robot Turn approximately 90 degree using IMU mag
pkoirala3 1:bbe16318d747 388 void ImuCheck()
pkoirala3 1:bbe16318d747 389 {
pkoirala3 1:bbe16318d747 390 bool rotFlag = true;
pkoirala3 1:bbe16318d747 391 IMU.readMag();
pkoirala3 1:bbe16318d747 392 float initial_heading;
pkoirala3 3:fff9904d2ecb 393 float mx = -1*IMU.mx;
pkoirala3 3:fff9904d2ecb 394 float my = IMU.my;
pkoirala3 3:fff9904d2ecb 395 if(my == 0.0) initial_heading = (mx < 0.0) ? 180.0 : 0.0;
pkoirala3 3:fff9904d2ecb 396 else initial_heading = atan2(mx,my)*360.0/(2.0*PI);
pkoirala3 1:bbe16318d747 397 initial_heading -= DECLINATION;
pkoirala3 1:bbe16318d747 398 if(initial_heading>180.0) initial_heading = initial_heading - 360.0;
pkoirala3 1:bbe16318d747 399 else if(initial_heading<0) initial_heading += 360.0;
pkoirala3 1:bbe16318d747 400
pkoirala3 1:bbe16318d747 401 while(rotFlag) {
pkoirala3 1:bbe16318d747 402 IMU.readMag();
pkoirala3 3:fff9904d2ecb 403 float mx = -1*IMU.mx;
pkoirala3 3:fff9904d2ecb 404 float my = IMU.my;
pkoirala3 1:bbe16318d747 405 float heading;
pkoirala3 3:fff9904d2ecb 406 if (my == 0.0) heading = (mx < 0.0) ? 180.0 : 0.0;
pkoirala3 3:fff9904d2ecb 407 else heading = atan2(mx, my)*360.0/(2.0*PI);
pkoirala3 1:bbe16318d747 408 heading -= DECLINATION; //correct for geo location
pkoirala3 1:bbe16318d747 409 if(heading>180.0) heading = heading - 360.0;
pkoirala3 1:bbe16318d747 410 else if(heading<0.0) heading += 360.0;
pkoirala3 1:bbe16318d747 411 if(abs(heading - initial_heading) > 90) rotFlag = 1;
pkoirala3 1:bbe16318d747 412 }
pkoirala3 1:bbe16318d747 413 }
pkoirala3 1:bbe16318d747 414
pkoirala3 3:fff9904d2ecb 415 // Checking for obstacles in the way while moving Fwd and Rev
pkoirala3 3:fff9904d2ecb 416 void ObjDetect(float currDist)
pkoirala3 1:bbe16318d747 417 {
pkoirala3 1:bbe16318d747 418 if(currDist <= 0.5f) {
pkoirala3 3:fff9904d2ecb 419 A.stop(0.0);
pkoirala3 3:fff9904d2ecb 420 B.stop(0.0);
pkoirala3 1:bbe16318d747 421 ledFwd = 0;
pkoirala3 1:bbe16318d747 422 ledRev = 0;
pkoirala3 3:fff9904d2ecb 423 mySpeaker = 0.5; // Play Beep sound
pkoirala3 3:fff9904d2ecb 424 wait(1.0);
pkoirala3 3:fff9904d2ecb 425 mySpeaker=0.0; // turn off audio
pkoirala3 1:bbe16318d747 426 }
pkoirala3 0:553fab92a347 427 }