Isaac Lee / Mbed 2 deprecated 4180_lab4_project

Bluetooth app controlled robot

Dependencies:   ESP8266 HALLFX_ENCODER LSM9DS1_Library_cal MotorDriver PID mbed

Fork of ESP8266_pid_redbot_webserver by Bryce Williams

main.cpp@2:3466e9e16c99, 2016-03-16 (annotated)

Committer:
ihl396
Date:
Wed Mar 16 16:03:44 2016 +0000
Revision:
2:3466e9e16c99
Parent:
1:d4a95e3a8aeb
Child:
3:bf6e71964ceb
4180 robot code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
electromotivated 0:11bc7a815367 1 #include "mbed.h"
electromotivated 0:11bc7a815367 2 #include "PID.h"
electromotivated 1:d4a95e3a8aeb 3 #include "HALLFX_ENCODER.h"
electromotivated 1:d4a95e3a8aeb 4 #include "MotorDriver.h"
ihl396 2:3466e9e16c99 5 #include "LSM9DS1.h"
electromotivated 0:11bc7a815367 6 #include <algorithm>
ihl396 2:3466e9e16c99 7 #define PI 3.14159
ihl396 2:3466e9e16c99 8 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
ihl396 2:3466e9e16c99 9 #define REFRESH_TIME_MS 1
ihl396 2:3466e9e16c99 10 #define DEBUG
electromotivated 0:11bc7a815367 11
ihl396 2:3466e9e16c99 12 Serial blue(p28,p27);
ihl396 2:3466e9e16c99 13 BusOut myled(LED1,LED2,LED3,LED4);
ihl396 2:3466e9e16c99 14 Serial pc(USBTX, USBRX);
electromotivated 1:d4a95e3a8aeb 15 float kp, ki, kd; // Working gain vars
electromotivated 1:d4a95e3a8aeb 16 float working_setpoint; // Used for web parsing and updating
electromotivated 1:d4a95e3a8aeb 17 float setpoint; // This is used by PID objects, because
electromotivated 1:d4a95e3a8aeb 18 // encoders are not quadrature and
electromotivated 1:d4a95e3a8aeb 19 // do not have direction information
electromotivated 1:d4a95e3a8aeb 20 // we use this to store the absolute
electromotivated 1:d4a95e3a8aeb 21 // value of the working_setpoint.
electromotivated 1:d4a95e3a8aeb 22 // Specifically setpoint is used only
electromotivated 1:d4a95e3a8aeb 23 // for PID objects. Refer to
electromotivated 1:d4a95e3a8aeb 24 // pid_callback() function
electromotivated 1:d4a95e3a8aeb 25 float feedbackL, outputL; // Should these be volatile?
electromotivated 1:d4a95e3a8aeb 26 float feedbackR, outputR; // Should these be volatile?
electromotivated 1:d4a95e3a8aeb 27 const float output_lower_limit = 0.0;
electromotivated 0:11bc7a815367 28 const float output_upper_limit = 1.0;
electromotivated 1:d4a95e3a8aeb 29 const float FEEDBACK_SCALE = 1.0/384.0; // Scale feedback to 1rev/3000cnts
electromotivated 0:11bc7a815367 30 // this is encoder specific.
electromotivated 0:11bc7a815367 31 const float Ts = 0.04; // 25Hz Sample Freq (40ms Sample Time)
electromotivated 0:11bc7a815367 32 const float Ts_PID_CALLBACK = Ts/2.0; // Update Motors and sensers twice as
electromotivated 0:11bc7a815367 33 // fast as PID sample rate, ensures
electromotivated 0:11bc7a815367 34 // PID feedback is upto date every
electromotivated 0:11bc7a815367 35 // time PID calculations run
electromotivated 0:11bc7a815367 36
electromotivated 1:d4a95e3a8aeb 37 const float kp_init = 0.01; // Good Kp for Speed Control; Start with this
electromotivated 1:d4a95e3a8aeb 38 const float ki_init= 0.015; // Good Ki for Speed Control; Start with this
electromotivated 1:d4a95e3a8aeb 39 const float kd_init = 0.0001; // Good Kd for Speed Control; Start with this
electromotivated 0:11bc7a815367 40
electromotivated 1:d4a95e3a8aeb 41 PID pidL(&setpoint, &feedbackL, &outputL,
electromotivated 0:11bc7a815367 42 output_lower_limit, output_upper_limit,
electromotivated 1:d4a95e3a8aeb 43 kp_init, ki_init, kd_init, Ts);
electromotivated 1:d4a95e3a8aeb 44 PID pidR(&setpoint, &feedbackR, &outputR,
electromotivated 1:d4a95e3a8aeb 45 output_lower_limit, output_upper_limit,
electromotivated 1:d4a95e3a8aeb 46 kp_init, ki_init, kd_init, Ts);
electromotivated 1:d4a95e3a8aeb 47 MotorDriver mtrR(p20, p19, p25, 10000.0, true); // in1, in2, pwm, pwmFreq, isBrakeable
electromotivated 1:d4a95e3a8aeb 48 MotorDriver mtrL(p18, p17, p24, 10000.0, true); // in1, in2, pwm, pwmFreq, isBrakeable
electromotivated 1:d4a95e3a8aeb 49 HALLFX_ENCODER encR(p21);
electromotivated 1:d4a95e3a8aeb 50 HALLFX_ENCODER encL(p22);
electromotivated 0:11bc7a815367 51 void pid_callback(); // Updates encoder feedback and motor output
ihl396 2:3466e9e16c99 52
electromotivated 1:d4a95e3a8aeb 53 Ticker motor; // Interrupt for feedback and motor updates
ihl396 2:3466e9e16c99 54 //global variables for main and interrupt routine
electromotivated 0:11bc7a815367 55
ihl396 2:3466e9e16c99 56 char state_num = 0, bnum =0 ;
ihl396 2:3466e9e16c99 57 int dirL = 0, dirR = 0;
electromotivated 0:11bc7a815367 58
ihl396 2:3466e9e16c99 59 //Interrupt routine to parse message with one new character per serial RX interrupt
electromotivated 0:11bc7a815367 60
electromotivated 0:11bc7a815367 61 float clip(float value, float lower, float upper);
electromotivated 0:11bc7a815367 62
ihl396 2:3466e9e16c99 63 LSM9DS1 imu(p9, p10, 0xD6, 0x3C);
ihl396 2:3466e9e16c99 64 void calculate_heading(float mx, float my, float mz);
ihl396 2:3466e9e16c99 65 float oldX = 0;
ihl396 2:3466e9e16c99 66 float oldY = 0;
ihl396 2:3466e9e16c99 67 float oldZ = 0;
ihl396 2:3466e9e16c99 68 float x= 0;
ihl396 2:3466e9e16c99 69 float y = 0;
ihl396 2:3466e9e16c99 70 float z = 0;
ihl396 2:3466e9e16c99 71
ihl396 2:3466e9e16c99 72 float posx = 0;
ihl396 2:3466e9e16c99 73 float velx = 0;
ihl396 2:3466e9e16c99 74 float oldPosx = 0;
ihl396 2:3466e9e16c99 75 float oldVelx = 0;
ihl396 2:3466e9e16c99 76
ihl396 2:3466e9e16c99 77 float posy = 0;
ihl396 2:3466e9e16c99 78 float vely = 0;
ihl396 2:3466e9e16c99 79 float oldPosy = 0;
ihl396 2:3466e9e16c99 80 float oldVely = 0;
ihl396 2:3466e9e16c99 81
ihl396 2:3466e9e16c99 82 float posz = 0;
ihl396 2:3466e9e16c99 83 float velz = 0;
ihl396 2:3466e9e16c99 84 float oldPosz = 0;
ihl396 2:3466e9e16c99 85 float oldVelz = 0;
ihl396 2:3466e9e16c99 86 int sample_time = 0;
ihl396 2:3466e9e16c99 87
ihl396 2:3466e9e16c99 88 float accelx = 0, accely = 0, accelz = 0;
ihl396 2:3466e9e16c99 89 Serial esp(p13, p14); // tx, rx
ihl396 2:3466e9e16c99 90 DigitalOut reset(p26);
ihl396 2:3466e9e16c99 91
ihl396 2:3466e9e16c99 92 Timer t;
ihl396 2:3466e9e16c99 93 void send_data(float *accelx, float *accely, float *accelz, float *velx, float *vely, float *velz);
ihl396 2:3466e9e16c99 94 int count,ended,timeout;
ihl396 2:3466e9e16c99 95 float heading = 0.0;
ihl396 2:3466e9e16c99 96 char buf[1024];
ihl396 2:3466e9e16c99 97 char snd[1024];
ihl396 2:3466e9e16c99 98 char str_buf[80];
ihl396 2:3466e9e16c99 99
ihl396 2:3466e9e16c99 100 char ssid[32] = "4180_test"; // enter WiFi router ssid inside the quotes
ihl396 2:3466e9e16c99 101 char pwd [32] = "4180isawesome"; // enter WiFi router password inside the quotes
ihl396 2:3466e9e16c99 102
ihl396 2:3466e9e16c99 103 void SendCMD(),getreply(),ESPconfig(),ESPsetbaudrate();
electromotivated 0:11bc7a815367 104 int main()
electromotivated 0:11bc7a815367 105 {
ihl396 2:3466e9e16c99 106 pc.printf("\f---------- Program Start ----------\r\n\n");
ihl396 2:3466e9e16c99 107 reset=0; //hardware reset for 8266
ihl396 2:3466e9e16c99 108 pc.baud(9600); // set what you want here depending on your terminal program speed
ihl396 2:3466e9e16c99 109 pc.printf("\f\n\r-------------ESP8266 Hardware Reset-------------\n\r");
ihl396 2:3466e9e16c99 110 wait(0.5);
ihl396 2:3466e9e16c99 111 reset=1;
ihl396 2:3466e9e16c99 112 timeout=2;
ihl396 2:3466e9e16c99 113 getreply();
electromotivated 0:11bc7a815367 114
ihl396 2:3466e9e16c99 115 ESPconfig(); //****************** include Config to set the ESP8266 configuration ***********************
electromotivated 0:11bc7a815367 116
ihl396 2:3466e9e16c99 117 imu.begin();
ihl396 2:3466e9e16c99 118 if (!imu.begin()) {
ihl396 2:3466e9e16c99 119 pc.printf("Failed to communicate with LSM9DS1.\n");
electromotivated 0:11bc7a815367 120 }
ihl396 2:3466e9e16c99 121 imu.calibrate(1);
ihl396 2:3466e9e16c99 122 imu.calibrateMag(0);
ihl396 2:3466e9e16c99 123
ihl396 2:3466e9e16c99 124 working_setpoint = 0.0;
electromotivated 0:11bc7a815367 125
electromotivated 1:d4a95e3a8aeb 126 encL.reset();
electromotivated 1:d4a95e3a8aeb 127 encR.reset();
electromotivated 1:d4a95e3a8aeb 128 feedbackL = encL.read();
electromotivated 1:d4a95e3a8aeb 129 feedbackR = encR.read();
electromotivated 0:11bc7a815367 130
electromotivated 0:11bc7a815367 131 // Update sensors and feedback twice as fast as PID sample time
electromotivated 0:11bc7a815367 132 // this makes pid react in real-time avoiding errors due to
electromotivated 0:11bc7a815367 133 // missing counts etc.
ihl396 2:3466e9e16c99 134
electromotivated 0:11bc7a815367 135 motor.attach(&pid_callback, Ts_PID_CALLBACK);
ihl396 2:3466e9e16c99 136
electromotivated 0:11bc7a815367 137 // Start PID sampling
electromotivated 1:d4a95e3a8aeb 138 pidL.start();
electromotivated 1:d4a95e3a8aeb 139 pidR.start();
ihl396 2:3466e9e16c99 140 working_setpoint = 0;
ihl396 2:3466e9e16c99 141 dirL = 1;
ihl396 2:3466e9e16c99 142 dirR = 1;
ihl396 2:3466e9e16c99 143 while(1)
ihl396 2:3466e9e16c99 144 {
ihl396 2:3466e9e16c99 145 if(blue.readable())
ihl396 2:3466e9e16c99 146 {
ihl396 2:3466e9e16c99 147 if (blue.getc()=='!')
ihl396 2:3466e9e16c99 148 {
ihl396 2:3466e9e16c99 149 if (blue.getc()=='B') //button data
ihl396 2:3466e9e16c99 150 {
ihl396 2:3466e9e16c99 151 bnum = blue.getc(); //button number
ihl396 2:3466e9e16c99 152 if(bnum=='1') {
ihl396 2:3466e9e16c99 153 working_setpoint = 50;
ihl396 2:3466e9e16c99 154 sample_time = 1;
ihl396 2:3466e9e16c99 155 pc.printf("setpoint: %2.0f\r\n", working_setpoint);
electromotivated 0:11bc7a815367 156 }
ihl396 2:3466e9e16c99 157 }
electromotivated 0:11bc7a815367 158 }
ihl396 2:3466e9e16c99 159 }
ihl396 2:3466e9e16c99 160 while(!imu.accelAvailable());
ihl396 2:3466e9e16c99 161 imu.readAccel();
ihl396 2:3466e9e16c99 162 while(!imu.magAvailable());
ihl396 2:3466e9e16c99 163 imu.readMag();
ihl396 2:3466e9e16c99 164
ihl396 2:3466e9e16c99 165 if( sample_time % 800 == 0)
ihl396 2:3466e9e16c99 166 {
ihl396 2:3466e9e16c99 167 velx = oldVelx + REFRESH_TIME_MS * oldX/1000;
ihl396 2:3466e9e16c99 168 posx = oldPosx + REFRESH_TIME_MS * oldVelx/1000;
ihl396 2:3466e9e16c99 169
ihl396 2:3466e9e16c99 170 vely = oldVely + REFRESH_TIME_MS * oldY/1000;
ihl396 2:3466e9e16c99 171 posy = oldPosy + REFRESH_TIME_MS * oldVely/1000;
ihl396 2:3466e9e16c99 172
ihl396 2:3466e9e16c99 173 accelx = imu.calcAccel(imu.ax);
ihl396 2:3466e9e16c99 174 accely = imu.calcAccel(imu.ay);
ihl396 2:3466e9e16c99 175 accelz = imu.calcAccel(imu.az);
ihl396 2:3466e9e16c99 176 calculate_heading(imu.calcMag(imu.mx), imu.calcMag(imu.my), imu.calcMag(imu.mz));
ihl396 2:3466e9e16c99 177
ihl396 2:3466e9e16c99 178 #ifdef DEBUG
ihl396 2:3466e9e16c99 179 pc.printf(" X axis Y axis Z axis\n\r");
ihl396 2:3466e9e16c99 180 pc.printf("accel: %2.2f %2.2f %2.2f in Gs\r\n", imu.calcAccel(imu.ax), imu.calcAccel(imu.ay), imu.calcAccel(imu.az));
ihl396 2:3466e9e16c99 181 pc.printf("Veloc: %2.2f %2.2f %2.2f in G*s\r\n", velx, vely, velz);
ihl396 2:3466e9e16c99 182 pc.printf("Magnetic heading: %2.2f", heading);
ihl396 2:3466e9e16c99 183 #endif
ihl396 2:3466e9e16c99 184
ihl396 2:3466e9e16c99 185 oldVelx = velx;
ihl396 2:3466e9e16c99 186 oldPosx = posx;
ihl396 2:3466e9e16c99 187 oldVely = vely;
ihl396 2:3466e9e16c99 188 oldPosy = posy;
ihl396 2:3466e9e16c99 189 oldX = imu.ax;
ihl396 2:3466e9e16c99 190 oldY = imu.ay;
electromotivated 0:11bc7a815367 191 }
ihl396 2:3466e9e16c99 192 if(working_setpoint > 0)
ihl396 2:3466e9e16c99 193 {
ihl396 2:3466e9e16c99 194 if(sample_time % 1500 == 0)
ihl396 2:3466e9e16c99 195 {
ihl396 2:3466e9e16c99 196 #ifdef DEBUG
ihl396 2:3466e9e16c99 197 pc.printf("Pushing data to server\r\n");
ihl396 2:3466e9e16c99 198 #endif
ihl396 2:3466e9e16c99 199
ihl396 2:3466e9e16c99 200 myled[0] = !myled[0];
ihl396 2:3466e9e16c99 201 working_setpoint = 0;
ihl396 2:3466e9e16c99 202
ihl396 2:3466e9e16c99 203 strcpy(snd, "srv:listen(80,function(conn)\r\n");
ihl396 2:3466e9e16c99 204 SendCMD();
ihl396 2:3466e9e16c99 205 wait(1);
ihl396 2:3466e9e16c99 206 strcpy(snd, "conn:on(\"receive\",function(conn,payload)\r\n");
ihl396 2:3466e9e16c99 207 SendCMD();
ihl396 2:3466e9e16c99 208 wait(1);
ihl396 2:3466e9e16c99 209 strcpy(snd, "print(payload)\r\n");
ihl396 2:3466e9e16c99 210 SendCMD();
ihl396 2:3466e9e16c99 211 wait(1);
ihl396 2:3466e9e16c99 212 strcpy(snd, "conn:send(\"<!DOCTYPE html>\")\r\n");
ihl396 2:3466e9e16c99 213 SendCMD();
ihl396 2:3466e9e16c99 214 wait(1);
ihl396 2:3466e9e16c99 215
ihl396 2:3466e9e16c99 216 strcpy(snd, "conn:send(\"<html>\")\r\n");
ihl396 2:3466e9e16c99 217 SendCMD();
ihl396 2:3466e9e16c99 218 wait(1);
ihl396 2:3466e9e16c99 219 sprintf(str_buf,"%2.2f",velx);
ihl396 2:3466e9e16c99 220 strcpy(snd, "conn:send(\"<h1>Velocity x: ");
ihl396 2:3466e9e16c99 221 strcat(snd, str_buf);
ihl396 2:3466e9e16c99 222 strcat(snd, "</h1>\")\r\n");
ihl396 2:3466e9e16c99 223 SendCMD();
ihl396 2:3466e9e16c99 224 wait(1);
ihl396 2:3466e9e16c99 225
ihl396 2:3466e9e16c99 226 sprintf(str_buf,"%2.2f",vely);
ihl396 2:3466e9e16c99 227 strcpy(snd, "conn:send(\"<h1>Velocity y: ");
ihl396 2:3466e9e16c99 228 strcat(snd, str_buf);
ihl396 2:3466e9e16c99 229 strcat(snd, "</h1>\")\r\n");
ihl396 2:3466e9e16c99 230 SendCMD();
ihl396 2:3466e9e16c99 231 wait(1);
ihl396 2:3466e9e16c99 232
ihl396 2:3466e9e16c99 233 sprintf(str_buf,"%2.2f",heading);
ihl396 2:3466e9e16c99 234 strcpy(snd, "conn:send(\"<h1>Magnetic Heading: ");
ihl396 2:3466e9e16c99 235 strcat(snd, str_buf);
ihl396 2:3466e9e16c99 236 strcat(snd, "</h1>\")\r\n");
ihl396 2:3466e9e16c99 237 SendCMD();
ihl396 2:3466e9e16c99 238 wait(1);
ihl396 2:3466e9e16c99 239
ihl396 2:3466e9e16c99 240 strcpy(snd, "conn:send(\"</html>\")\r\n");
ihl396 2:3466e9e16c99 241 SendCMD();
ihl396 2:3466e9e16c99 242 wait(1);
ihl396 2:3466e9e16c99 243
ihl396 2:3466e9e16c99 244 strcpy(snd, "end)\r\n");
ihl396 2:3466e9e16c99 245 SendCMD();
ihl396 2:3466e9e16c99 246 wait(1);
ihl396 2:3466e9e16c99 247
ihl396 2:3466e9e16c99 248 strcpy(snd, "conn:on(\"sent\",function(conn) conn:close() end)\r\n");
ihl396 2:3466e9e16c99 249 SendCMD();
ihl396 2:3466e9e16c99 250 wait(1);
ihl396 2:3466e9e16c99 251 strcpy(snd, "end)\r\n");
ihl396 2:3466e9e16c99 252 SendCMD();
ihl396 2:3466e9e16c99 253 wait(1);
ihl396 2:3466e9e16c99 254 timeout=17;
ihl396 2:3466e9e16c99 255 getreply();
ihl396 2:3466e9e16c99 256 pc.printf(buf);
ihl396 2:3466e9e16c99 257 pc.printf("\r\nDONE");
ihl396 2:3466e9e16c99 258 }
ihl396 2:3466e9e16c99 259 if(sample_time > 100000)
ihl396 2:3466e9e16c99 260 {
ihl396 2:3466e9e16c99 261 sample_time = 1;
ihl396 2:3466e9e16c99 262 }
ihl396 2:3466e9e16c99 263 sample_time++;
electromotivated 0:11bc7a815367 264 }
electromotivated 0:11bc7a815367 265 }
electromotivated 0:11bc7a815367 266 }
electromotivated 0:11bc7a815367 267
ihl396 2:3466e9e16c99 268 void ESPconfig()
ihl396 2:3466e9e16c99 269 {
ihl396 2:3466e9e16c99 270 wait(5);
ihl396 2:3466e9e16c99 271 pc.printf("\f---------- Starting ESP Config ----------\r\n\n");
ihl396 2:3466e9e16c99 272 strcpy(snd,".\r\n.\r\n");
ihl396 2:3466e9e16c99 273 SendCMD();
ihl396 2:3466e9e16c99 274 wait(1);
ihl396 2:3466e9e16c99 275 pc.printf("---------- Reset & get Firmware ----------\r\n");
ihl396 2:3466e9e16c99 276 strcpy(snd,"node.restart()\r\n");
ihl396 2:3466e9e16c99 277 SendCMD();
ihl396 2:3466e9e16c99 278 timeout=5;
ihl396 2:3466e9e16c99 279 getreply();
ihl396 2:3466e9e16c99 280 pc.printf(buf);
ihl396 2:3466e9e16c99 281
ihl396 2:3466e9e16c99 282 wait(2);
ihl396 2:3466e9e16c99 283
ihl396 2:3466e9e16c99 284 pc.printf("\n---------- Get Version ----------\r\n");
ihl396 2:3466e9e16c99 285 strcpy(snd,"print(node.info())\r\n");
ihl396 2:3466e9e16c99 286 SendCMD();
ihl396 2:3466e9e16c99 287 timeout=4;
ihl396 2:3466e9e16c99 288 getreply();
ihl396 2:3466e9e16c99 289 pc.printf(buf);
ihl396 2:3466e9e16c99 290 wait(3);
ihl396 2:3466e9e16c99 291
ihl396 2:3466e9e16c99 292 // set CWMODE to 1=Station,2=AP,3=BOTH, default mode 1 (Station)
ihl396 2:3466e9e16c99 293 pc.printf("\n---------- Setting Mode ----------\r\n");
ihl396 2:3466e9e16c99 294 strcpy(snd, "wifi.setmode(wifi.STATION)\r\n");
ihl396 2:3466e9e16c99 295 SendCMD();
ihl396 2:3466e9e16c99 296 timeout=4;
ihl396 2:3466e9e16c99 297 getreply();
ihl396 2:3466e9e16c99 298 pc.printf(buf);
ihl396 2:3466e9e16c99 299
ihl396 2:3466e9e16c99 300 wait(2);
ihl396 2:3466e9e16c99 301
ihl396 2:3466e9e16c99 302 pc.printf("\n---------- Connecting to AP ----------\r\n");
ihl396 2:3466e9e16c99 303 pc.printf("ssid = %s pwd = %s\r\n",ssid,pwd);
ihl396 2:3466e9e16c99 304 strcpy(snd, "wifi.sta.config(\"");
ihl396 2:3466e9e16c99 305 strcat(snd, ssid);
ihl396 2:3466e9e16c99 306 strcat(snd, "\",\"");
ihl396 2:3466e9e16c99 307 strcat(snd, pwd);
ihl396 2:3466e9e16c99 308 strcat(snd, "\")\r\n");
ihl396 2:3466e9e16c99 309 SendCMD();
ihl396 2:3466e9e16c99 310 timeout=10;
ihl396 2:3466e9e16c99 311 getreply();
ihl396 2:3466e9e16c99 312 pc.printf(buf);
ihl396 2:3466e9e16c99 313
ihl396 2:3466e9e16c99 314 wait(5);
ihl396 2:3466e9e16c99 315
ihl396 2:3466e9e16c99 316 pc.printf("\n---------- Get IP's ----------\r\n");
ihl396 2:3466e9e16c99 317 strcpy(snd, "print(wifi.sta.getip())\r\n");
ihl396 2:3466e9e16c99 318 SendCMD();
ihl396 2:3466e9e16c99 319 timeout=3;
ihl396 2:3466e9e16c99 320 getreply();
ihl396 2:3466e9e16c99 321 pc.printf(buf);
ihl396 2:3466e9e16c99 322
ihl396 2:3466e9e16c99 323 wait(1);
ihl396 2:3466e9e16c99 324
ihl396 2:3466e9e16c99 325 pc.printf("\n---------- Get Connection Status ----------\r\n");
ihl396 2:3466e9e16c99 326 strcpy(snd, "print(wifi.sta.status())\r\n");
ihl396 2:3466e9e16c99 327 SendCMD();
ihl396 2:3466e9e16c99 328 timeout=5;
ihl396 2:3466e9e16c99 329 getreply();
ihl396 2:3466e9e16c99 330 pc.printf(buf);
ihl396 2:3466e9e16c99 331
ihl396 2:3466e9e16c99 332 pc.printf("\n---------- Setting up http server ----------\r\n");
ihl396 2:3466e9e16c99 333 strcpy(snd, "srv=net.createServer(net.TCP)\r\n");
ihl396 2:3466e9e16c99 334 SendCMD();
ihl396 2:3466e9e16c99 335 wait(1);
ihl396 2:3466e9e16c99 336 strcpy(snd, "srv:listen(80,function(conn)\r\n");
ihl396 2:3466e9e16c99 337 SendCMD();
ihl396 2:3466e9e16c99 338 wait(1);
ihl396 2:3466e9e16c99 339 strcpy(snd, "conn:on(\"receive\",function(conn,payload)\r\n");
ihl396 2:3466e9e16c99 340 SendCMD();
ihl396 2:3466e9e16c99 341 wait(1);
ihl396 2:3466e9e16c99 342 strcpy(snd, "print(payload)\r\n");
ihl396 2:3466e9e16c99 343 SendCMD();
ihl396 2:3466e9e16c99 344 wait(1);
ihl396 2:3466e9e16c99 345 strcpy(snd, "conn:send(\"<!DOCTYPE html>\")\r\n");
ihl396 2:3466e9e16c99 346 SendCMD();
ihl396 2:3466e9e16c99 347 wait(1);
electromotivated 0:11bc7a815367 348
ihl396 2:3466e9e16c99 349 strcpy(snd, "conn:send(\"<html>\")\r\n");
ihl396 2:3466e9e16c99 350 SendCMD();
ihl396 2:3466e9e16c99 351 wait(1);
electromotivated 0:11bc7a815367 352
ihl396 2:3466e9e16c99 353 strcpy(snd, "conn:send(\"<h1>WIFI Monitoring Program</h1>\")\r\n");
ihl396 2:3466e9e16c99 354 SendCMD();
ihl396 2:3466e9e16c99 355 wait(1);
ihl396 2:3466e9e16c99 356
ihl396 2:3466e9e16c99 357 strcpy(snd, "conn:send(\"</html>\")\r\n");
ihl396 2:3466e9e16c99 358 SendCMD();
ihl396 2:3466e9e16c99 359 wait(1);
electromotivated 0:11bc7a815367 360
ihl396 2:3466e9e16c99 361 strcpy(snd, "end)\r\n");
ihl396 2:3466e9e16c99 362 SendCMD();
ihl396 2:3466e9e16c99 363 wait(1);
electromotivated 0:11bc7a815367 364
ihl396 2:3466e9e16c99 365 strcpy(snd, "conn:on(\"sent\",function(conn) conn:close() end)\r\n");
ihl396 2:3466e9e16c99 366 SendCMD();
ihl396 2:3466e9e16c99 367 wait(1);
ihl396 2:3466e9e16c99 368 strcpy(snd, "end)\r\n");
ihl396 2:3466e9e16c99 369 SendCMD();
ihl396 2:3466e9e16c99 370 wait(1);
ihl396 2:3466e9e16c99 371 timeout=17;
ihl396 2:3466e9e16c99 372 getreply();
ihl396 2:3466e9e16c99 373 pc.printf(buf);
ihl396 2:3466e9e16c99 374 pc.printf("\r\nDONE");
ihl396 2:3466e9e16c99 375
electromotivated 0:11bc7a815367 376 }
electromotivated 0:11bc7a815367 377
ihl396 2:3466e9e16c99 378 void SendCMD()
ihl396 2:3466e9e16c99 379 {
ihl396 2:3466e9e16c99 380 esp.printf("%s", snd);
ihl396 2:3466e9e16c99 381 }
ihl396 2:3466e9e16c99 382
ihl396 2:3466e9e16c99 383 void getreply()
ihl396 2:3466e9e16c99 384 {
ihl396 2:3466e9e16c99 385 memset(buf, '\0', sizeof(buf));
ihl396 2:3466e9e16c99 386 t.start();
ihl396 2:3466e9e16c99 387 ended=0;
ihl396 2:3466e9e16c99 388 int count=0;
ihl396 2:3466e9e16c99 389 while(!ended) {
ihl396 2:3466e9e16c99 390 if(esp.readable()) {
ihl396 2:3466e9e16c99 391 buf[count] = esp.getc();
ihl396 2:3466e9e16c99 392 count++;
ihl396 2:3466e9e16c99 393 }
ihl396 2:3466e9e16c99 394 if(t.read() > timeout) {
ihl396 2:3466e9e16c99 395 ended = 1;
ihl396 2:3466e9e16c99 396 t.stop();
ihl396 2:3466e9e16c99 397 t.reset();
ihl396 2:3466e9e16c99 398 }
electromotivated 0:11bc7a815367 399 }
ihl396 2:3466e9e16c99 400 }
ihl396 2:3466e9e16c99 401 void calculate_heading(float mx, float my, float mz)
ihl396 2:3466e9e16c99 402 {
ihl396 2:3466e9e16c99 403
ihl396 2:3466e9e16c99 404 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
ihl396 2:3466e9e16c99 405 mx = -mx;
ihl396 2:3466e9e16c99 406 if (my == 0.0)
ihl396 2:3466e9e16c99 407 heading = (mx < 0.0) ? 180.0 : 0.0;
ihl396 2:3466e9e16c99 408 else
ihl396 2:3466e9e16c99 409 heading = atan2(mx, my)*360.0/(2.0*PI);
ihl396 2:3466e9e16c99 410 heading -= DECLINATION; //correct for geo location
ihl396 2:3466e9e16c99 411 if(heading>180.0) heading = heading - 360.0;
ihl396 2:3466e9e16c99 412 else if(heading<-180.0) heading = 360.0 + heading;
ihl396 2:3466e9e16c99 413 else if(heading<0.0) heading = 360.0 + heading;
electromotivated 0:11bc7a815367 414
ihl396 2:3466e9e16c99 415 #ifdef DEBUG
ihl396 2:3466e9e16c99 416 pc.printf("Magnetic Heading: %f degress\n\r",heading);
ihl396 2:3466e9e16c99 417 #endif
electromotivated 0:11bc7a815367 418 }
electromotivated 0:11bc7a815367 419
ihl396 2:3466e9e16c99 420 void pid_callback()
ihl396 2:3466e9e16c99 421 {
ihl396 2:3466e9e16c99 422
ihl396 2:3466e9e16c99 423 // Deal with feedback and update motors
ihl396 2:3466e9e16c99 424 // Motor direction based on working setpoint var
ihl396 2:3466e9e16c99 425
ihl396 2:3466e9e16c99 426 //myled[0] = dirL;
ihl396 2:3466e9e16c99 427 //myled[1] = dirR;
ihl396 2:3466e9e16c99 428 // Setpoint vars used by PID objects are concerned with
ihl396 2:3466e9e16c99 429 // only SPEED not direction.
ihl396 2:3466e9e16c99 430 setpoint = abs(working_setpoint);
ihl396 2:3466e9e16c99 431
ihl396 2:3466e9e16c99 432 float k = Ts_PID_CALLBACK; // Discrete time, (Ts/2 because this callback is called
ihl396 2:3466e9e16c99 433 // at interval of Ts/2... or twice as fast as pid controller)
ihl396 2:3466e9e16c99 434 static int last_count_L = 0;
ihl396 2:3466e9e16c99 435 static int last_count_R = 0;
ihl396 2:3466e9e16c99 436 int countL = encL.read();
ihl396 2:3466e9e16c99 437 int countR = encR.read();
ihl396 2:3466e9e16c99 438 //pc.printf("%d\r\n", countL);
electromotivated 1:d4a95e3a8aeb 439
ihl396 2:3466e9e16c99 440 // Because encoders are not quadrature we must handle the sign ourselves,
ihl396 2:3466e9e16c99 441 // i.e. explicitly make calcs based on the direction we have set the motor
ihl396 2:3466e9e16c99 442 float raw_speed_L = ((countL - last_count_L)*FEEDBACK_SCALE) / k;
ihl396 2:3466e9e16c99 443 float rpm_speed_L = raw_speed_L * 60.0; // Convert speed to RPM
ihl396 2:3466e9e16c99 444
ihl396 2:3466e9e16c99 445 float raw_speed_R = ((countR - last_count_R)*FEEDBACK_SCALE) / k;
ihl396 2:3466e9e16c99 446 float rpm_speed_R = raw_speed_R * 60.0; // Convert speed to RPM
electromotivated 1:d4a95e3a8aeb 447
ihl396 2:3466e9e16c99 448 last_count_L = countL; // Save last count
ihl396 2:3466e9e16c99 449 last_count_R = countR;
ihl396 2:3466e9e16c99 450 feedbackL = rpm_speed_L;
ihl396 2:3466e9e16c99 451 feedbackR = rpm_speed_R;
ihl396 2:3466e9e16c99 452
ihl396 2:3466e9e16c99 453 mtrL.forceSetSpeed(outputL * dirL);
ihl396 2:3466e9e16c99 454 mtrR.forceSetSpeed(outputR * dirR);
electromotivated 0:11bc7a815367 455 }
electromotivated 0:11bc7a815367 456
electromotivated 0:11bc7a815367 457 /*
electromotivated 0:11bc7a815367 458 Clips value to lower/ uppper
electromotivated 0:11bc7a815367 459 @param value The value to clip
electromotivated 0:11bc7a815367 460 @param lower The mininum allowable value
electromotivated 0:11bc7a815367 461 @param upper The maximum allowable value
electromotivated 0:11bc7a815367 462 @return The resulting clipped value
electromotivated 0:11bc7a815367 463 */
electromotivated 0:11bc7a815367 464 float clip(float value, float lower, float upper){
electromotivated 0:11bc7a815367 465 return std::max(lower, std::min(value, upper));
ihl396 2:3466e9e16c99 466 }