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@5:e9f1030a5bbb, 2017-03-13 (annotated)

Committer:
pkoirala3
Date:
Mon Mar 13 21:54:22 2017 +0000
Revision:
5:e9f1030a5bbb
Parent:
4:166570fa7fda 
ECE4180_ROBOT_CONTROLLEDBY_WiFI

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