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@3:bf6e71964ceb, 2016-03-16 (annotated)

Committer:
ihl396
Date:
Wed Mar 16 16:21:56 2016 +0000
Revision:
3:bf6e71964ceb
Parent:
2:3466e9e16c99 
commit

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