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@1:bbe16318d747, 2017-03-12 (annotated)

Committer:
pkoirala3
Date:
Sun Mar 12 21:12:08 2017 +0000
Revision:
1:bbe16318d747
Parent:
0:553fab92a347
Child:
3:fff9904d2ecb
Removed Sonar and added IR distance sensor. Added IMU to turn the robot.

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 0:553fab92a347 9 DigitalOut ledRev(p9);
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 1:bbe16318d747 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 0:553fab92a347 40 char webdata[4096]; // This may need to be bigger depending on WEB browser used
pkoirala3 0:553fab92a347 41 char webbuff[4096]; // Currently using 1986 characters, Increase this if more web page data added
pkoirala3 0:553fab92a347 42 void SendCMD(),getreply(),ReadWebData(),startserver();
pkoirala3 1:bbe16318d747 43 void ImuCheck(), ObsDetected(float);
pkoirala3 0:553fab92a347 44 char rx_line[1024];
pkoirala3 1:bbe16318d747 45 int port = 80; // set server port
pkoirala3 1:bbe16318d747 46 int SERVtimeout = 5; // set server timeout in seconds in case link breaks.
pkoirala3 0:553fab92a347 47 struct tm t;
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 0:553fab92a347 59 mySpeaker.period(1.0/500.0); // 500hz period
pkoirala3 1:bbe16318d747 60
pkoirala3 1:bbe16318d747 61 IMU.begin();
pkoirala3 1:bbe16318d747 62 IMU.calibrate(1);
pkoirala3 1:bbe16318d747 63 IMU.calibrateMag(0);
pkoirala3 1:bbe16318d747 64
pkoirala3 0:553fab92a347 65 while(1) {
pkoirala3 0:553fab92a347 66 if(DataRX==1) {
pkoirala3 0:553fab92a347 67 ReadWebData();
pkoirala3 0:553fab92a347 68 esp.attach(&Rx_interrupt, Serial::RxIrq);
pkoirala3 0:553fab92a347 69 }
pkoirala3 0:553fab92a347 70 float stateA = A.state();
pkoirala3 0:553fab92a347 71 float stateB = B.state();
pkoirala3 0:553fab92a347 72 if((stateA > 0 && stateA <= 1)&&(stateB > 0 && stateB <= 1)) { // obstacle within 10 cm and robot is still moving
pkoirala3 1:bbe16318d747 73 // Add second distance Sensor code
pkoirala3 1:bbe16318d747 74 float Dist = DistSensorFwd;
pkoirala3 1:bbe16318d747 75 pc.printf("currDist: %f",Dist);
pkoirala3 1:bbe16318d747 76 ObsDectected(Dist);
pkoirala3 0:553fab92a347 77 } else if((stateA < 0 && stateA >= -1)&&(stateB < 0 && stateB >= -1)) { // If moving rev
pkoirala3 1:bbe16318d747 78 float Dist = DistSensorRev;
pkoirala3 1:bbe16318d747 79 pc.printf("currDist: %f",Dist);
pkoirala3 1:bbe16318d747 80 ObsDectected(Dist);
pkoirala3 0:553fab92a347 81 }
pkoirala3 0:553fab92a347 82 }
pkoirala3 0:553fab92a347 83 }
pkoirala3 0:553fab92a347 84
pkoirala3 0:553fab92a347 85 // Reads and processes GET and POST web data
pkoirala3 0:553fab92a347 86 void ReadWebData()
pkoirala3 0:553fab92a347 87 {
pkoirala3 0:553fab92a347 88 wait_ms(200);
pkoirala3 0:553fab92a347 89 esp.attach(NULL,Serial::RxIrq);
pkoirala3 0:553fab92a347 90 DataRX=0;
pkoirala3 0:553fab92a347 91 memset(webdata, '\0', sizeof(webdata));
pkoirala3 0:553fab92a347 92 strcpy(webdata, rx_buffer);
pkoirala3 0:553fab92a347 93 memset(rx_buffer, '\0', sizeof(rx_buffer));
pkoirala3 0:553fab92a347 94 rx_in = 0;
pkoirala3 0:553fab92a347 95 rx_out = 0;
pkoirala3 0:553fab92a347 96 // check web data for form information
pkoirala3 0:553fab92a347 97 if( strstr(webdata, "check=BotFwd") != NULL ) {
pkoirala3 0:553fab92a347 98 A.speed(0.75);
pkoirala3 0:553fab92a347 99 B.speed(0.75);
pkoirala3 0:553fab92a347 100 ledRev = 0;
pkoirala3 0:553fab92a347 101 ledFwd = 1;
pkoirala3 0:553fab92a347 102 }
pkoirala3 0:553fab92a347 103 if( strstr(webdata, "check=BotRev") != NULL ) {
pkoirala3 0:553fab92a347 104 A.speed(-0.75);
pkoirala3 0:553fab92a347 105 B.speed(-0.75);
pkoirala3 0:553fab92a347 106 ledFwd = 0;
pkoirala3 0:553fab92a347 107 ledRev = 1;
pkoirala3 0:553fab92a347 108 }
pkoirala3 0:553fab92a347 109 if( strstr(webdata, "check=BotStop") != NULL ) {
pkoirala3 0:553fab92a347 110 A.stop(0.5);
pkoirala3 0:553fab92a347 111 B.stop(0.5);
pkoirala3 0:553fab92a347 112 ledFwd = 0;
pkoirala3 0:553fab92a347 113 ledRev = 0;
pkoirala3 0:553fab92a347 114 }
pkoirala3 0:553fab92a347 115 if( strstr(webdata, "check=BotLeft") != NULL ) {
pkoirala3 0:553fab92a347 116 B.stop(0.5);
pkoirala3 0:553fab92a347 117 A.speed(0.5);
pkoirala3 1:bbe16318d747 118 ImuCheck();
pkoirala3 1:bbe16318d747 119 A.stop(0.5);
pkoirala3 0:553fab92a347 120 }
pkoirala3 0:553fab92a347 121 if( strstr(webdata, "check=BotRight") != NULL ) {
pkoirala3 0:553fab92a347 122 A.stop(0.5);
pkoirala3 0:553fab92a347 123 B.speed(0.5);
pkoirala3 1:bbe16318d747 124 ImuCheck();
pkoirala3 1:bbe16318d747 125 B.stop(0.5);
pkoirala3 0:553fab92a347 126 }
pkoirala3 0:553fab92a347 127 if( strstr(webdata, "POST") != NULL ) { // set update flag if POST request
pkoirala3 0:553fab92a347 128 update=1;
pkoirala3 0:553fab92a347 129 }
pkoirala3 0:553fab92a347 130 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 131 update=1;
pkoirala3 0:553fab92a347 132 }
pkoirala3 0:553fab92a347 133 }
pkoirala3 1:bbe16318d747 134
pkoirala3 0:553fab92a347 135 // Starts webserver
pkoirala3 0:553fab92a347 136 void startserver()
pkoirala3 0:553fab92a347 137 {
pkoirala3 0:553fab92a347 138 pc.printf("Resetting ESP Device....\r\n");
pkoirala3 0:553fab92a347 139 strcpy(cmdbuff,"node.restart()\r\n");
pkoirala3 0:553fab92a347 140 SendCMD();
pkoirala3 0:553fab92a347 141 wait(2);
pkoirala3 0:553fab92a347 142 getreply();
pkoirala3 0:553fab92a347 143
pkoirala3 0:553fab92a347 144 pc.printf("\nStarting Server.....\r\n> ");
pkoirala3 0:553fab92a347 145 //create server
pkoirala3 0:553fab92a347 146 sprintf(cmdbuff, "srv=net.createServer(net.TCP,%d)\r\n",SERVtimeout);
pkoirala3 0:553fab92a347 147 SendCMD();
pkoirala3 0:553fab92a347 148 getreply();
pkoirala3 0:553fab92a347 149 wait(0.5);
pkoirala3 0:553fab92a347 150 strcpy(cmdbuff,"srv:listen(80,function(conn)\r\n");
pkoirala3 0:553fab92a347 151 SendCMD();
pkoirala3 0:553fab92a347 152 getreply();
pkoirala3 0:553fab92a347 153 wait(0.3);
pkoirala3 0:553fab92a347 154 strcpy(cmdbuff,"conn:on(\"receive\",function(conn,payload) \r\n");
pkoirala3 0:553fab92a347 155 SendCMD();
pkoirala3 0:553fab92a347 156 getreply();
pkoirala3 0:553fab92a347 157 wait(0.3);
pkoirala3 0:553fab92a347 158
pkoirala3 0:553fab92a347 159 //print data to mbed
pkoirala3 0:553fab92a347 160 strcpy(cmdbuff,"print(payload)\r\n");
pkoirala3 0:553fab92a347 161 SendCMD();
pkoirala3 0:553fab92a347 162 getreply();
pkoirala3 0:553fab92a347 163 wait(0.2);
pkoirala3 0:553fab92a347 164
pkoirala3 0:553fab92a347 165 //web page data
pkoirala3 0:553fab92a347 166 strcpy(cmdbuff,"conn:send('<!DOCTYPE html><html><body><h1>ESP8266 Mbed IoT Web Controller</h1>')\r\n");
pkoirala3 0:553fab92a347 167 SendCMD();
pkoirala3 0:553fab92a347 168 getreply();
pkoirala3 0:553fab92a347 169 wait(0.4);
pkoirala3 0:553fab92a347 170 strcpy(cmdbuff,"conn:send('<form method=\"POST\"')\r\n");
pkoirala3 0:553fab92a347 171 SendCMD();
pkoirala3 0:553fab92a347 172 getreply();
pkoirala3 0:553fab92a347 173 wait(0.3);
pkoirala3 0:553fab92a347 174 strcpy(cmdbuff, "conn:send('<p><input type=\"radio\" name=\"check\" value=\"BotFwd\"> Move Robot Forward')\r\n");
pkoirala3 0:553fab92a347 175 SendCMD();
pkoirala3 0:553fab92a347 176 getreply();
pkoirala3 0:553fab92a347 177 wait(0.3);
pkoirala3 0:553fab92a347 178 strcpy(cmdbuff, "conn:send('<p><input type=\"radio\" name=\"check\" value=\"BotRev\"> Move Robot Reverse')\r\n");
pkoirala3 0:553fab92a347 179 SendCMD();
pkoirala3 0:553fab92a347 180 getreply();
pkoirala3 0:553fab92a347 181 wait(0.3);
pkoirala3 0:553fab92a347 182 strcpy(cmdbuff, "conn:send('<p><input type=\"radio\" name=\"check\" value=\"BotStop\"> Stop Robot')\r\n");
pkoirala3 0:553fab92a347 183 SendCMD();
pkoirala3 0:553fab92a347 184 getreply();
pkoirala3 0:553fab92a347 185 wait(0.3);
pkoirala3 0:553fab92a347 186 strcpy(cmdbuff, "conn:send('<p><input type=\"radio\" name=\"check\" value=\"BotLeft\"> Turn Robot Left')\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:send('<p><input type=\"radio\" name=\"check\" value=\"BotRight\"> Turn Robot Right')\r\n");
pkoirala3 0:553fab92a347 191 SendCMD();
pkoirala3 0:553fab92a347 192 getreply();
pkoirala3 0:553fab92a347 193 wait(0.3);
pkoirala3 0:553fab92a347 194 strcpy(cmdbuff,"conn:send('<p><input type=\"submit\" value=\"Send to Robot\"></form>')\r\n");
pkoirala3 0:553fab92a347 195 SendCMD();
pkoirala3 0:553fab92a347 196 getreply();
pkoirala3 0:553fab92a347 197 wait(0.3);
pkoirala3 0:553fab92a347 198 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 199 SendCMD();
pkoirala3 0:553fab92a347 200 getreply();
pkoirala3 0:553fab92a347 201 wait(0.5);
pkoirala3 0:553fab92a347 202 // end web page data
pkoirala3 0:553fab92a347 203 strcpy(cmdbuff, "conn:on(\"sent\",function(conn) conn:close() end)\r\n"); // close current connection
pkoirala3 0:553fab92a347 204 SendCMD();
pkoirala3 0:553fab92a347 205 getreply();
pkoirala3 0:553fab92a347 206 wait(0.3);
pkoirala3 0:553fab92a347 207 strcpy(cmdbuff, "end)\r\n");
pkoirala3 0:553fab92a347 208 SendCMD();
pkoirala3 0:553fab92a347 209 getreply();
pkoirala3 0:553fab92a347 210 wait(0.2);
pkoirala3 0:553fab92a347 211 strcpy(cmdbuff, "end)\r\n");
pkoirala3 0:553fab92a347 212 SendCMD();
pkoirala3 0:553fab92a347 213 getreply();
pkoirala3 0:553fab92a347 214 wait(0.2);
pkoirala3 0:553fab92a347 215
pkoirala3 0:553fab92a347 216 strcpy(cmdbuff, "tmr.alarm(0, 1000, 1, function()\r\n");
pkoirala3 0:553fab92a347 217 SendCMD();
pkoirala3 0:553fab92a347 218 getreply();
pkoirala3 0:553fab92a347 219 wait(0.2);
pkoirala3 0:553fab92a347 220 strcpy(cmdbuff, "if wifi.sta.getip() == nil then\r\n");
pkoirala3 0:553fab92a347 221 SendCMD();
pkoirala3 0:553fab92a347 222 getreply();
pkoirala3 0:553fab92a347 223 wait(0.2);
pkoirala3 0:553fab92a347 224 strcpy(cmdbuff, "print(\"Connecting to AP...\\n\")\r\n");
pkoirala3 0:553fab92a347 225 SendCMD();
pkoirala3 0:553fab92a347 226 getreply();
pkoirala3 0:553fab92a347 227 wait(0.2);
pkoirala3 0:553fab92a347 228 strcpy(cmdbuff, "else\r\n");
pkoirala3 0:553fab92a347 229 SendCMD();
pkoirala3 0:553fab92a347 230 getreply();
pkoirala3 0:553fab92a347 231 wait(0.2);
pkoirala3 0:553fab92a347 232 strcpy(cmdbuff, "ip, nm, gw=wifi.sta.getip()\r\n");
pkoirala3 0:553fab92a347 233 SendCMD();
pkoirala3 0:553fab92a347 234 getreply();
pkoirala3 0:553fab92a347 235 wait(0.2);
pkoirala3 0:553fab92a347 236 strcpy(cmdbuff,"print(\"IP Address: \",ip)\r\n");
pkoirala3 0:553fab92a347 237 SendCMD();
pkoirala3 0:553fab92a347 238 getreply();
pkoirala3 0:553fab92a347 239 wait(0.2);
pkoirala3 0:553fab92a347 240 strcpy(cmdbuff,"tmr.stop(0)\r\n");
pkoirala3 0:553fab92a347 241 SendCMD();
pkoirala3 0:553fab92a347 242 getreply();
pkoirala3 0:553fab92a347 243 wait(0.2);
pkoirala3 0:553fab92a347 244 strcpy(cmdbuff,"end\r\n");
pkoirala3 0:553fab92a347 245 SendCMD();
pkoirala3 0:553fab92a347 246 getreply();
pkoirala3 0:553fab92a347 247 wait(0.2);
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 pc.printf("\n\nReady to go .....\r\n\n");
pkoirala3 0:553fab92a347 253 }
pkoirala3 0:553fab92a347 254
pkoirala3 0:553fab92a347 255 // ESP Command data send
pkoirala3 0:553fab92a347 256 void SendCMD()
pkoirala3 0:553fab92a347 257 {
pkoirala3 0:553fab92a347 258 int i;
pkoirala3 0:553fab92a347 259 char temp_char;
pkoirala3 0:553fab92a347 260 bool empty;
pkoirala3 0:553fab92a347 261 i = 0;
pkoirala3 0:553fab92a347 262 // Start Critical Section - don't interrupt while changing global buffer variables
pkoirala3 0:553fab92a347 263 NVIC_DisableIRQ(UART1_IRQn);
pkoirala3 0:553fab92a347 264 empty = (tx_in == tx_out);
pkoirala3 0:553fab92a347 265 while ((i==0) || (cmdbuff[i-1] != '\n')) {
pkoirala3 0:553fab92a347 266 // Wait if buffer full
pkoirala3 0:553fab92a347 267 if (((tx_in + 1) % buffer_size) == tx_out) {
pkoirala3 0:553fab92a347 268 // End Critical Section - need to let interrupt routine empty buffer by sending
pkoirala3 0:553fab92a347 269 NVIC_EnableIRQ(UART1_IRQn);
pkoirala3 0:553fab92a347 270 while (((tx_in + 1) % buffer_size) == tx_out) {
pkoirala3 0:553fab92a347 271 }
pkoirala3 0:553fab92a347 272 // Start Critical Section - don't interrupt while changing global buffer variables
pkoirala3 0:553fab92a347 273 NVIC_DisableIRQ(UART1_IRQn);
pkoirala3 0:553fab92a347 274 }
pkoirala3 0:553fab92a347 275 tx_buffer[tx_in] = cmdbuff[i];
pkoirala3 0:553fab92a347 276 i++;
pkoirala3 0:553fab92a347 277 tx_in = (tx_in + 1) % buffer_size;
pkoirala3 0:553fab92a347 278 }
pkoirala3 0:553fab92a347 279 if (esp.writeable() && (empty)) {
pkoirala3 0:553fab92a347 280 temp_char = tx_buffer[tx_out];
pkoirala3 0:553fab92a347 281 tx_out = (tx_out + 1) % buffer_size;
pkoirala3 0:553fab92a347 282 // Send first character to start tx interrupts, if stopped
pkoirala3 0:553fab92a347 283 esp.putc(temp_char);
pkoirala3 0:553fab92a347 284 }
pkoirala3 0:553fab92a347 285 // End Critical Section
pkoirala3 0:553fab92a347 286 NVIC_EnableIRQ(UART1_IRQn);
pkoirala3 0:553fab92a347 287 return;
pkoirala3 0:553fab92a347 288 }
pkoirala3 0:553fab92a347 289
pkoirala3 0:553fab92a347 290 // Get Command and ESP status replies
pkoirala3 0:553fab92a347 291 void getreply()
pkoirala3 0:553fab92a347 292 {
pkoirala3 0:553fab92a347 293 read_line();
pkoirala3 0:553fab92a347 294 sscanf(rx_line,replybuff);
pkoirala3 0:553fab92a347 295 }
pkoirala3 0:553fab92a347 296
pkoirala3 0:553fab92a347 297 // Read a line from the large rx buffer from rx interrupt routine
pkoirala3 0:553fab92a347 298 void read_line()
pkoirala3 0:553fab92a347 299 {
pkoirala3 0:553fab92a347 300 int i;
pkoirala3 0:553fab92a347 301 i = 0;
pkoirala3 0:553fab92a347 302 // Start Critical Section - don't interrupt while changing global buffer variables
pkoirala3 0:553fab92a347 303 NVIC_DisableIRQ(UART1_IRQn);
pkoirala3 0:553fab92a347 304 // Loop reading rx buffer characters until end of line character
pkoirala3 0:553fab92a347 305 while ((i==0) || (rx_line[i-1] != '\r')) {
pkoirala3 0:553fab92a347 306 // Wait if buffer empty
pkoirala3 0:553fab92a347 307 if (rx_in == rx_out) {
pkoirala3 0:553fab92a347 308 // End Critical Section - need to allow rx interrupt to get new characters for buffer
pkoirala3 0:553fab92a347 309 NVIC_EnableIRQ(UART1_IRQn);
pkoirala3 0:553fab92a347 310 while (rx_in == rx_out) {
pkoirala3 0:553fab92a347 311 }
pkoirala3 0:553fab92a347 312 // Start Critical Section - don't interrupt while changing global buffer variables
pkoirala3 0:553fab92a347 313 NVIC_DisableIRQ(UART1_IRQn);
pkoirala3 0:553fab92a347 314 }
pkoirala3 0:553fab92a347 315 rx_line[i] = rx_buffer[rx_out];
pkoirala3 0:553fab92a347 316 i++;
pkoirala3 0:553fab92a347 317 rx_out = (rx_out + 1) % buffer_size;
pkoirala3 0:553fab92a347 318 }
pkoirala3 0:553fab92a347 319 // End Critical Section
pkoirala3 0:553fab92a347 320 NVIC_EnableIRQ(UART1_IRQn);
pkoirala3 0:553fab92a347 321 rx_line[i-1] = 0;
pkoirala3 0:553fab92a347 322 return;
pkoirala3 0:553fab92a347 323 }
pkoirala3 0:553fab92a347 324
pkoirala3 0:553fab92a347 325 // Interupt Routine to read in data from serial port
pkoirala3 0:553fab92a347 326 void Rx_interrupt()
pkoirala3 0:553fab92a347 327 {
pkoirala3 0:553fab92a347 328 DataRX=1;
pkoirala3 0:553fab92a347 329 // Loop just in case more than one character is in UART's receive FIFO buffer
pkoirala3 0:553fab92a347 330 // Stop if buffer full
pkoirala3 0:553fab92a347 331 while ((esp.readable()) && (((rx_in + 1) % buffer_size) != rx_out)) {
pkoirala3 0:553fab92a347 332 rx_buffer[rx_in] = esp.getc();
pkoirala3 0:553fab92a347 333 // Uncomment to Echo to USB serial to watch data flow
pkoirala3 0:553fab92a347 334 pc.putc(rx_buffer[rx_in]);
pkoirala3 0:553fab92a347 335 rx_in = (rx_in + 1) % buffer_size;
pkoirala3 0:553fab92a347 336 }
pkoirala3 0:553fab92a347 337 return;
pkoirala3 0:553fab92a347 338 }
pkoirala3 0:553fab92a347 339
pkoirala3 0:553fab92a347 340 // Interupt Routine to write out data to serial port
pkoirala3 0:553fab92a347 341 void Tx_interrupt()
pkoirala3 0:553fab92a347 342 {
pkoirala3 0:553fab92a347 343 // Loop to fill more than one character in UART's transmit FIFO buffer
pkoirala3 0:553fab92a347 344 // Stop if buffer empty
pkoirala3 0:553fab92a347 345 while ((esp.writeable()) && (tx_in != tx_out)) {
pkoirala3 0:553fab92a347 346 esp.putc(tx_buffer[tx_out]);
pkoirala3 0:553fab92a347 347 tx_out = (tx_out + 1) % buffer_size;
pkoirala3 0:553fab92a347 348 }
pkoirala3 0:553fab92a347 349 return;
pkoirala3 1:bbe16318d747 350 }
pkoirala3 1:bbe16318d747 351
pkoirala3 1:bbe16318d747 352 // To make Robot Turn approximately 90 degree using IMU mag
pkoirala3 1:bbe16318d747 353 void ImuCheck()
pkoirala3 1:bbe16318d747 354 {
pkoirala3 1:bbe16318d747 355 bool rotFlag = true;
pkoirala3 1:bbe16318d747 356 IMU.readMag();
pkoirala3 1:bbe16318d747 357 float initial_heading;
pkoirala3 1:bbe16318d747 358 if(my == 0.0) initial_heading = (-1*mx < 0.0) ? 180.0 : 0.0;
pkoirala3 1:bbe16318d747 359 else initial_heading = atan2(-1*mx,my)*360.0/(2.0*PI);
pkoirala3 1:bbe16318d747 360 initial_heading -= DECLINATION;
pkoirala3 1:bbe16318d747 361 if(initial_heading>180.0) initial_heading = initial_heading - 360.0;
pkoirala3 1:bbe16318d747 362 else if(initial_heading<0) initial_heading += 360.0;
pkoirala3 1:bbe16318d747 363
pkoirala3 1:bbe16318d747 364 while(rotFlag) {
pkoirala3 1:bbe16318d747 365 IMU.readMag();
pkoirala3 1:bbe16318d747 366 float mx = -1*(imu.mx);
pkoirala3 1:bbe16318d747 367 float heading;
pkoirala3 1:bbe16318d747 368 if (my == 0.0) heading = (-1*mx < 0.0) ? 180.0 : 0.0;
pkoirala3 1:bbe16318d747 369 else heading = atan2(-1*mx, my)*360.0/(2.0*PI);
pkoirala3 1:bbe16318d747 370 heading -= DECLINATION; //correct for geo location
pkoirala3 1:bbe16318d747 371 if(heading>180.0) heading = heading - 360.0;
pkoirala3 1:bbe16318d747 372 else if(heading<0.0) heading += 360.0;
pkoirala3 1:bbe16318d747 373 if(abs(heading - initial_heading) > 90) rotFlag = 1;
pkoirala3 1:bbe16318d747 374 }
pkoirala3 1:bbe16318d747 375 }
pkoirala3 1:bbe16318d747 376
pkoirala3 1:bbe16318d747 377 // Checking if object there is obstacles in the way( Fwd and Rev )
pkoirala3 1:bbe16318d747 378 void ObsDetected(float currDist)
pkoirala3 1:bbe16318d747 379 {
pkoirala3 1:bbe16318d747 380 if(currDist <= 0.5f) {
pkoirala3 1:bbe16318d747 381 A.stop(0.5);
pkoirala3 1:bbe16318d747 382 B.stop(0.5);
pkoirala3 1:bbe16318d747 383 ledFwd = 0;
pkoirala3 1:bbe16318d747 384 ledRev = 0;
pkoirala3 1:bbe16318d747 385 mySpeaker = 0.5; // Play Beep sound
pkoirala3 1:bbe16318d747 386 }
pkoirala3 0:553fab92a347 387 }