Cameron Causer / Mbed 2 deprecated 4180_Project_WiFi_Communication_Controller

Final version

Dependencies:   mbed SparkfunAnalogJoystick LSM9DS1_Library_cal PinDetect

main.cpp@2:da3e288789a4, 2018-12-11 (annotated)

Committer:
kzar
Date:
Tue Dec 11 21:22:01 2018 +0000
Revision:
2:da3e288789a4
Parent:
1:0ec1b59239f2 
ye

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kzar 1:0ec1b59239f2 1 // This mbed code creates a connection with the car mbeds server and reads from it
kzar 0:5ad3d5f99603 2 // The car will host the server, the remote controll will send data to the server
kzar 0:5ad3d5f99603 3
kzar 0:5ad3d5f99603 4 #include "mbed.h"
kzar 0:5ad3d5f99603 5 #include "PinDetect.h"
kzar 2:da3e288789a4 6 #include "SparkfunAnalogJoystick.h"
kzar 2:da3e288789a4 7 #include "LSM9DS1.h"
kzar 2:da3e288789a4 8 #define PI 3.14159
kzar 0:5ad3d5f99603 9
kzar 2:da3e288789a4 10 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
kzar 2:da3e288789a4 11 // Construct serial objects
kzar 0:5ad3d5f99603 12 Serial pc(USBTX, USBRX);
kzar 0:5ad3d5f99603 13 Serial esp(p28, p27); // tx, rx
kzar 0:5ad3d5f99603 14 // Standard Mbed LED definitions
kzar 0:5ad3d5f99603 15 DigitalOut led1(LED1);
kzar 0:5ad3d5f99603 16 DigitalOut led2(LED2);
kzar 2:da3e288789a4 17 // Joystick for control
kzar 2:da3e288789a4 18 SparkfunAnalogJoystick joystick(p18, p19, p20);
kzar 2:da3e288789a4 19 // Construct imu object
kzar 2:da3e288789a4 20 LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);//sda, scl
kzar 2:da3e288789a4 21 // function for geting mag heading
kzar 2:da3e288789a4 22 float calc_heading(float mx, float my);
kzar 0:5ad3d5f99603 23
kzar 0:5ad3d5f99603 24 // things for sending/receiving data over serial
kzar 0:5ad3d5f99603 25 volatile int tx_in=0;
kzar 0:5ad3d5f99603 26 volatile int tx_out=0;
kzar 0:5ad3d5f99603 27 volatile int rx_in=0;
kzar 0:5ad3d5f99603 28 volatile int rx_out=0;
kzar 0:5ad3d5f99603 29 const int buffer_size = 4095;
kzar 0:5ad3d5f99603 30 char tx_buffer[buffer_size+1];
kzar 0:5ad3d5f99603 31 char rx_buffer[buffer_size+1];
kzar 0:5ad3d5f99603 32 void Tx_interrupt();
kzar 0:5ad3d5f99603 33 void Rx_interrupt();
kzar 2:da3e288789a4 34 //void read_line();
kzar 0:5ad3d5f99603 35
kzar 0:5ad3d5f99603 36 int DataRX;
kzar 0:5ad3d5f99603 37 int update;
kzar 0:5ad3d5f99603 38 char cmdbuff[1024];
kzar 0:5ad3d5f99603 39 char replybuff[4096];
kzar 2:da3e288789a4 40 char webdata[4096]; // This may need to be bigger depending on WEB browser used
kzar 0:5ad3d5f99603 41 char webbuff[4096]; // Currently using 1986 characters, Increase this if more web page data added
kzar 2:da3e288789a4 42 void SendCMD();
kzar 2:da3e288789a4 43 //void getreply();
kzar 0:5ad3d5f99603 44 void getConnection(); // Sets up a connection with the car server
kzar 0:5ad3d5f99603 45 char rx_line[1024];
kzar 2:da3e288789a4 46 int port = 80; // set server port
kzar 2:da3e288789a4 47 int SERVtimeout = 5; // set server timeout in seconds in case link breaks.
kzar 0:5ad3d5f99603 48
kzar 0:5ad3d5f99603 49 int main()
kzar 0:5ad3d5f99603 50 {
kzar 0:5ad3d5f99603 51 pc.baud(9600);
kzar 0:5ad3d5f99603 52 esp.baud(9600);
kzar 2:da3e288789a4 53
kzar 2:da3e288789a4 54 // Init leds
kzar 2:da3e288789a4 55 led1=0,led2=0;
kzar 2:da3e288789a4 56
kzar 0:5ad3d5f99603 57 // Setup a serial interrupt function to receive data
kzar 0:5ad3d5f99603 58 esp.attach(&Rx_interrupt, Serial::RxIrq);
kzar 0:5ad3d5f99603 59 // Setup a serial interrupt function to transmit data
kzar 0:5ad3d5f99603 60 esp.attach(&Tx_interrupt, Serial::TxIrq);
kzar 2:da3e288789a4 61
kzar 0:5ad3d5f99603 62 // Get connection to server
kzar 0:5ad3d5f99603 63 getConnection();
kzar 0:5ad3d5f99603 64
kzar 2:da3e288789a4 65 // Set up the imu
kzar 2:da3e288789a4 66 IMU.begin();
kzar 2:da3e288789a4 67 if (!IMU.begin()) {
kzar 2:da3e288789a4 68 pc.printf("Failed to communicate with LSM9DS1.\n");
kzar 2:da3e288789a4 69 }
kzar 2:da3e288789a4 70 IMU.calibrate(1);
kzar 2:da3e288789a4 71 IMU.calibrateMag(0);
kzar 2:da3e288789a4 72 float forward, current, diff;
kzar 2:da3e288789a4 73 float servo_x;
kzar 2:da3e288789a4 74 //initial heading calc, need to rotate by 360 degrees.
kzar 2:da3e288789a4 75 while(!IMU.magAvailable(X_AXIS));
kzar 2:da3e288789a4 76 IMU.readMag();
kzar 2:da3e288789a4 77 forward = calc_heading(IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my));
kzar 2:da3e288789a4 78
kzar 2:da3e288789a4 79 // Variables used for joystick
kzar 2:da3e288789a4 80 float lm, rm;
kzar 2:da3e288789a4 81 float x, y;
kzar 2:da3e288789a4 82
kzar 2:da3e288789a4 83 while (1) {
kzar 2:da3e288789a4 84 // Update the motor speeds
kzar 2:da3e288789a4 85 // Get x and y
kzar 2:da3e288789a4 86 x = joystick.xAxis();
kzar 2:da3e288789a4 87 y = joystick.yAxis();
kzar 2:da3e288789a4 88 // Get the angle of direction
kzar 2:da3e288789a4 89 // Determine which direction (straight, complete turn, or in a coordinate quadrant)
kzar 2:da3e288789a4 90 // straight (set motor speed directly to y
kzar 2:da3e288789a4 91 if (x < .1 && x > -.1) {
kzar 2:da3e288789a4 92 lm = (y);
kzar 2:da3e288789a4 93 rm = (-1*y);\
kzar 2:da3e288789a4 94 }
kzar 2:da3e288789a4 95 // Complete turn (set outside motor directly to x)
kzar 2:da3e288789a4 96 else if ( y < .1 && y > -.1) {
kzar 2:da3e288789a4 97 if (x > 0) {
kzar 2:da3e288789a4 98 lm = (x);
kzar 2:da3e288789a4 99 rm = (0);
kzar 2:da3e288789a4 100 } else {
kzar 2:da3e288789a4 101 lm = (0);
kzar 2:da3e288789a4 102 rm = (x);
kzar 2:da3e288789a4 103 }
kzar 0:5ad3d5f99603 104 }
kzar 0:5ad3d5f99603 105
kzar 2:da3e288789a4 106 // Update the servo
kzar 2:da3e288789a4 107 while(!IMU.magAvailable(X_AXIS));
kzar 2:da3e288789a4 108 IMU.readMag();
kzar 2:da3e288789a4 109 current = calc_heading(IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my));
kzar 2:da3e288789a4 110 // diff is the final difference from forward direction in [-180, 180] range
kzar 2:da3e288789a4 111 // this needs to be normalized to [0, 1] while only keeping [-45, 45]
kzar 2:da3e288789a4 112 diff = fmod((forward - current + 180), 360) - 180;
kzar 2:da3e288789a4 113 diff = (diff < -180) ? (diff + 360) : diff;
kzar 2:da3e288789a4 114 servo_x = (diff / 90.0) + 0.45;
kzar 2:da3e288789a4 115
kzar 2:da3e288789a4 116 // Send the updated value of the motors and servos
kzar 2:da3e288789a4 117 sprintf(cmdbuff, "cl:send(\"<l=%f,r=%f,x=%f>\")\r\n)", lm, rm, servo_x);
kzar 2:da3e288789a4 118
kzar 2:da3e288789a4 119 SendCMD();
kzar 2:da3e288789a4 120 wait(0.18);
kzar 2:da3e288789a4 121 led1=!led1;
kzar 0:5ad3d5f99603 122 }
kzar 0:5ad3d5f99603 123 }
kzar 0:5ad3d5f99603 124
kzar 2:da3e288789a4 125 // Gets magnetic heading
kzar 2:da3e288789a4 126 float calc_heading(float mx, float my)
kzar 0:5ad3d5f99603 127 {
kzar 2:da3e288789a4 128 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
kzar 2:da3e288789a4 129 mx = -mx;
kzar 2:da3e288789a4 130 float heading;
kzar 2:da3e288789a4 131 if (my == 0.0)
kzar 2:da3e288789a4 132 heading = (mx < 0.0) ? 180.0 : 0.0;
kzar 2:da3e288789a4 133 else
kzar 2:da3e288789a4 134 heading = atan2(mx, my)*360.0/(2.0*PI);
kzar 2:da3e288789a4 135 heading -= DECLINATION; //correct for geo location
kzar 2:da3e288789a4 136 if(heading>180.0) heading = heading - 360.0;
kzar 2:da3e288789a4 137 else if(heading<-180.0) heading = 360.0 + heading;
kzar 2:da3e288789a4 138 else if(heading<0.0) heading = 360.0 + heading;
kzar 2:da3e288789a4 139
kzar 2:da3e288789a4 140 // pc.printf("Magnetic Heading: %f degrees\n\r",heading);
kzar 2:da3e288789a4 141 return heading;
kzar 0:5ad3d5f99603 142 }
kzar 1:0ec1b59239f2 143
kzar 2:da3e288789a4 144
kzar 0:5ad3d5f99603 145 // Sets up connection with car server
kzar 0:5ad3d5f99603 146 void getConnection()
kzar 2:da3e288789a4 147 {
kzar 1:0ec1b59239f2 148 // Reset the ESP8266
kzar 1:0ec1b59239f2 149 pc.printf("++++++++++ Resetting ESP ++++++++++\r\n");
kzar 1:0ec1b59239f2 150 strcpy(cmdbuff,"node.restart()\r\n");
kzar 1:0ec1b59239f2 151 SendCMD();
kzar 2:da3e288789a4 152 wait(1);
kzar 1:0ec1b59239f2 153
kzar 0:5ad3d5f99603 154 // Disconncet from any potential connections
kzar 0:5ad3d5f99603 155 strcpy(cmdbuff,"wifi.sta.disconnect()\r\n");
kzar 0:5ad3d5f99603 156 SendCMD();
kzar 2:da3e288789a4 157 wait(1);
kzar 0:5ad3d5f99603 158 // Set mode (STATION)
kzar 0:5ad3d5f99603 159 strcpy(cmdbuff,"wifi.setmode(wifi.STATION)\r\n");
kzar 0:5ad3d5f99603 160 SendCMD();
kzar 2:da3e288789a4 161 //getreply();
kzar 2:da3e288789a4 162 wait(1);
kzar 0:5ad3d5f99603 163 // Connect to car server
kzar 1:0ec1b59239f2 164 strcpy(cmdbuff,"wifi.sta.config(\"Marlon's iPhone\", \"feelsbadman\")\r\n");
kzar 0:5ad3d5f99603 165 SendCMD();
kzar 2:da3e288789a4 166 wait(1);
kzar 0:5ad3d5f99603 167 // Connect to server
kzar 0:5ad3d5f99603 168 strcpy(cmdbuff,"wifi.sta.connect()\r\n");
kzar 0:5ad3d5f99603 169 SendCMD();
kzar 2:da3e288789a4 170 wait(1);
kzar 1:0ec1b59239f2 171
kzar 1:0ec1b59239f2 172 strcpy(cmdbuff,"print(\"Looking for a connection\")\r\n");
kzar 1:0ec1b59239f2 173 SendCMD();
kzar 2:da3e288789a4 174 wait(1);
kzar 0:5ad3d5f99603 175
kzar 0:5ad3d5f99603 176 strcpy(cmdbuff,"tmr.alarm(1,2000,1, function()\r\n");
kzar 0:5ad3d5f99603 177 SendCMD();
kzar 2:da3e288789a4 178 wait(1);
kzar 0:5ad3d5f99603 179
kzar 1:0ec1b59239f2 180 strcpy(cmdbuff,"if(wifi.sta.getip()~=nil) then\r\n");
kzar 0:5ad3d5f99603 181 SendCMD();
kzar 2:da3e288789a4 182 wait(1);
kzar 0:5ad3d5f99603 183
kzar 0:5ad3d5f99603 184 strcpy(cmdbuff,"tmr.stop(1)\r\n");
kzar 0:5ad3d5f99603 185 SendCMD();
kzar 2:da3e288789a4 186 wait(1);
kzar 0:5ad3d5f99603 187 // Print out controller ip address
kzar 0:5ad3d5f99603 188 strcpy(cmdbuff,"print(\"Connected!\")\r\n");
kzar 0:5ad3d5f99603 189 SendCMD();
kzar 2:da3e288789a4 190 wait(1);
kzar 0:5ad3d5f99603 191 // Print out controller ip address
kzar 1:0ec1b59239f2 192 strcpy(cmdbuff,"print(\"Client IP Address: \",wifi.sta.getip())\r\n");
kzar 1:0ec1b59239f2 193 SendCMD();
kzar 2:da3e288789a4 194 wait(1);
kzar 1:0ec1b59239f2 195 // Create connetion
kzar 1:0ec1b59239f2 196 strcpy(cmdbuff,"cl=net.createConnection(net.TCP, 0)\r\n");
kzar 1:0ec1b59239f2 197 SendCMD();
kzar 2:da3e288789a4 198 wait(1);
kzar 1:0ec1b59239f2 199 // Connect the connection
kzar 1:0ec1b59239f2 200 strcpy(cmdbuff,"cl:connect(80, \"192.168.4.1\")\r\n");
kzar 0:5ad3d5f99603 201 SendCMD();
kzar 2:da3e288789a4 202 wait(1);
kzar 2:da3e288789a4 203
kzar 0:5ad3d5f99603 204 strcpy(cmdbuff,"else\r\n");
kzar 0:5ad3d5f99603 205 SendCMD();
kzar 2:da3e288789a4 206 wait(1);
kzar 2:da3e288789a4 207
kzar 0:5ad3d5f99603 208 strcpy(cmdbuff,"print(\"Connecting...\")\r\n");
kzar 0:5ad3d5f99603 209 SendCMD();
kzar 2:da3e288789a4 210 wait(1);
kzar 2:da3e288789a4 211
kzar 0:5ad3d5f99603 212 strcpy(cmdbuff,"end\r\n");
kzar 0:5ad3d5f99603 213 SendCMD();
kzar 2:da3e288789a4 214 wait(1);
kzar 2:da3e288789a4 215
kzar 0:5ad3d5f99603 216 strcpy(cmdbuff,"end)\r\n");
kzar 0:5ad3d5f99603 217 SendCMD();
kzar 2:da3e288789a4 218 //getreply();
kzar 2:da3e288789a4 219 wait(5);
kzar 0:5ad3d5f99603 220 }
kzar 0:5ad3d5f99603 221
kzar 0:5ad3d5f99603 222
kzar 0:5ad3d5f99603 223 // ESP Command data send
kzar 0:5ad3d5f99603 224 void SendCMD()
kzar 0:5ad3d5f99603 225 {
kzar 0:5ad3d5f99603 226 int i;
kzar 0:5ad3d5f99603 227 char temp_char;
kzar 0:5ad3d5f99603 228 bool empty;
kzar 0:5ad3d5f99603 229 i = 0;
kzar 0:5ad3d5f99603 230 // Start Critical Section - don't interrupt while changing global buffer variables
kzar 0:5ad3d5f99603 231 NVIC_DisableIRQ(UART1_IRQn);
kzar 0:5ad3d5f99603 232 empty = (tx_in == tx_out);
kzar 0:5ad3d5f99603 233 while ((i==0) || (cmdbuff[i-1] != '\n')) {
kzar 0:5ad3d5f99603 234 // Wait if buffer full
kzar 0:5ad3d5f99603 235 if (((tx_in + 1) % buffer_size) == tx_out) {
kzar 0:5ad3d5f99603 236 // End Critical Section - need to let interrupt routine empty buffer by sending
kzar 0:5ad3d5f99603 237 NVIC_EnableIRQ(UART1_IRQn);
kzar 0:5ad3d5f99603 238 while (((tx_in + 1) % buffer_size) == tx_out) {
kzar 0:5ad3d5f99603 239 }
kzar 0:5ad3d5f99603 240 // Start Critical Section - don't interrupt while changing global buffer variables
kzar 0:5ad3d5f99603 241 NVIC_DisableIRQ(UART1_IRQn);
kzar 0:5ad3d5f99603 242 }
kzar 0:5ad3d5f99603 243 tx_buffer[tx_in] = cmdbuff[i];
kzar 0:5ad3d5f99603 244 i++;
kzar 0:5ad3d5f99603 245 tx_in = (tx_in + 1) % buffer_size;
kzar 0:5ad3d5f99603 246 }
kzar 0:5ad3d5f99603 247 if (esp.writeable() && (empty)) {
kzar 0:5ad3d5f99603 248 temp_char = tx_buffer[tx_out];
kzar 0:5ad3d5f99603 249 tx_out = (tx_out + 1) % buffer_size;
kzar 0:5ad3d5f99603 250 // Send first character to start tx interrupts, if stopped
kzar 0:5ad3d5f99603 251 esp.putc(temp_char);
kzar 0:5ad3d5f99603 252 }
kzar 0:5ad3d5f99603 253 // End Critical Section
kzar 0:5ad3d5f99603 254 NVIC_EnableIRQ(UART1_IRQn);
kzar 0:5ad3d5f99603 255 return;
kzar 0:5ad3d5f99603 256 }
kzar 0:5ad3d5f99603 257
kzar 0:5ad3d5f99603 258
kzar 0:5ad3d5f99603 259 // Get Command and ESP status replies
kzar 2:da3e288789a4 260 //void getreply()
kzar 2:da3e288789a4 261 //{
kzar 2:da3e288789a4 262 // read_line();
kzar 2:da3e288789a4 263 // sscanf(rx_line,replybuff);
kzar 2:da3e288789a4 264 //}
kzar 0:5ad3d5f99603 265
kzar 0:5ad3d5f99603 266 // FUNCTIONS BELOW ARE FOR RX AND TX INTERUPTS (NOT WEB STUFF)
kzar 0:5ad3d5f99603 267
kzar 0:5ad3d5f99603 268 // Read a line from the large rx buffer from rx interrupt routine
kzar 2:da3e288789a4 269 //void read_line() {
kzar 2:da3e288789a4 270 // int i;
kzar 2:da3e288789a4 271 // i = 0;
kzar 2:da3e288789a4 272 //// Start Critical Section - don't interrupt while changing global buffer variables
kzar 2:da3e288789a4 273 // NVIC_DisableIRQ(UART1_IRQn);
kzar 2:da3e288789a4 274 //// Loop reading rx buffer characters until end of line character
kzar 2:da3e288789a4 275 // while ((i==0) || (rx_line[i-1] != '\r')) {
kzar 2:da3e288789a4 276 //// Wait if buffer empty
kzar 2:da3e288789a4 277 // if (rx_in == rx_out) {
kzar 2:da3e288789a4 278 //// End Critical Section - need to allow rx interrupt to get new characters for buffer
kzar 2:da3e288789a4 279 // NVIC_EnableIRQ(UART1_IRQn);
kzar 2:da3e288789a4 280 // while (rx_in == rx_out) {
kzar 2:da3e288789a4 281 // }
kzar 2:da3e288789a4 282 //// Start Critical Section - don't interrupt while changing global buffer variables
kzar 2:da3e288789a4 283 // NVIC_DisableIRQ(UART1_IRQn);
kzar 2:da3e288789a4 284 // }
kzar 2:da3e288789a4 285 // rx_line[i] = rx_buffer[rx_out];
kzar 2:da3e288789a4 286 // i++;
kzar 2:da3e288789a4 287 // rx_out = (rx_out + 1) % buffer_size;
kzar 2:da3e288789a4 288 // }
kzar 2:da3e288789a4 289 //// End Critical Section
kzar 2:da3e288789a4 290 // NVIC_EnableIRQ(UART1_IRQn);
kzar 2:da3e288789a4 291 // rx_line[i-1] = 0;
kzar 2:da3e288789a4 292 // return;
kzar 2:da3e288789a4 293 //}
kzar 0:5ad3d5f99603 294
kzar 0:5ad3d5f99603 295
kzar 0:5ad3d5f99603 296 // Interupt Routine to read in data from serial port
kzar 0:5ad3d5f99603 297 void Rx_interrupt() {
kzar 0:5ad3d5f99603 298 DataRX=1;
kzar 0:5ad3d5f99603 299 //led3=1;
kzar 0:5ad3d5f99603 300 // Loop just in case more than one character is in UART's receive FIFO buffer
kzar 0:5ad3d5f99603 301 // Stop if buffer full
kzar 0:5ad3d5f99603 302 while ((esp.readable()) && (((rx_in + 1) % buffer_size) != rx_out)) {
kzar 0:5ad3d5f99603 303 rx_buffer[rx_in] = esp.getc();
kzar 0:5ad3d5f99603 304 // Uncomment to Echo to USB serial to watch data flow
kzar 2:da3e288789a4 305 // pc.putc(rx_buffer[rx_in]);
kzar 2:da3e288789a4 306 // rx_in = (rx_in + 1) % buffer_size;
kzar 0:5ad3d5f99603 307 }
kzar 0:5ad3d5f99603 308 //led3=0;
kzar 0:5ad3d5f99603 309 return;
kzar 0:5ad3d5f99603 310 }
kzar 0:5ad3d5f99603 311
kzar 0:5ad3d5f99603 312
kzar 0:5ad3d5f99603 313 // Interupt Routine to write out data to serial port
kzar 0:5ad3d5f99603 314 void Tx_interrupt() {
kzar 0:5ad3d5f99603 315 //led2=1;
kzar 0:5ad3d5f99603 316 // Loop to fill more than one character in UART's transmit FIFO buffer
kzar 0:5ad3d5f99603 317 // Stop if buffer empty
kzar 0:5ad3d5f99603 318 while ((esp.writeable()) && (tx_in != tx_out)) {
kzar 0:5ad3d5f99603 319 esp.putc(tx_buffer[tx_out]);
kzar 0:5ad3d5f99603 320 tx_out = (tx_out + 1) % buffer_size;
kzar 0:5ad3d5f99603 321 }
kzar 0:5ad3d5f99603 322 //led2=0;
kzar 0:5ad3d5f99603 323 return;
kzar 0:5ad3d5f99603 324 }