main.cpp

00001 #include "mbed.h"
00002 #include "PID.h"
00003 #include "HALLFX_ENCODER.h"
00004 #include "MotorDriver.h"
00005 #include "LSM9DS1.h"
00006 #include <algorithm>
00007 #define PI 3.14159
00008 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
00009 #define REFRESH_TIME_MS 1
00010 #define DEBUG
00011 
00012 Serial blue(p28,p27);
00013 BusOut myled(LED1,LED2,LED3,LED4);
00014 Serial pc(USBTX, USBRX);
00015 
00016 // -------- PID Initialization -----------
00017 float kp, ki, kd;                               // Working gain vars
00018 float working_setpoint;                         // Used for web parsing and updating
00019 float setpoint;                                 // This is used by PID objects, because 
00020                                                 // encoders are not quadrature and 
00021                                                 // do not have direction information
00022                                                 // we use this to store the absolute
00023                                                 // value of the working_setpoint.
00024                                                 // Specifically setpoint is used only
00025                                                 // for PID objects. Refer to 
00026                                                 // pid_callback() function
00027 float feedbackL, outputL;                       // Should these be volatile?
00028 float feedbackR, outputR;                       // Should these be volatile? 
00029 const float output_lower_limit = 0.0;          
00030 const float output_upper_limit = 1.0;
00031 const float FEEDBACK_SCALE = 1.0/384.0;         // Scale feedback to 1rev/3000cnts
00032                                                 // this is encoder specific.
00033 const float Ts = 0.04;                         // 25Hz Sample Freq (40ms Sample Time)
00034 const float Ts_PID_CALLBACK = Ts/2.0;          // Update Motors and sensers twice as 
00035                                                // fast as PID sample rate, ensures
00036                                                // PID feedback is upto date every 
00037                                                // time PID calculations run
00038 
00039 const float kp_init = 0.01;        // Good Kp for Speed Control; Start with this
00040 const float ki_init= 0.015;        // Good Ki for Speed Control; Start with this
00041 const float kd_init = 0.0001;      // Good Kd for Speed Control; Start with this
00042 
00043 PID pidL(&setpoint, &feedbackL, &outputL,
00044         output_lower_limit, output_upper_limit,
00045         kp_init, ki_init, kd_init, Ts); 
00046 PID pidR(&setpoint, &feedbackR, &outputR,
00047         output_lower_limit, output_upper_limit, 
00048         kp_init, ki_init, kd_init, Ts);
00049 MotorDriver mtrR(p20, p19, p25, 10000.0, true); // in1, in2, pwm, pwmFreq, isBrakeable
00050 MotorDriver mtrL(p18, p17, p24, 10000.0, true); // in1, in2, pwm, pwmFreq, isBrakeable
00051 HALLFX_ENCODER encR(p21);
00052 HALLFX_ENCODER encL(p22);
00053 void pid_callback();            // Updates encoder feedback and motor output
00054         
00055 Ticker motor;                   // Interrupt for feedback and motor updates
00056 //global variables for main and interrupt routine
00057 
00058 char state_num = 0, bnum =0 ;
00059 int dirL = 0, dirR = 0;
00060 
00061 //Interrupt routine to parse message with one new character per serial RX interrupt
00062 
00063 float clip(float value, float lower, float upper);
00064 
00065 LSM9DS1 imu(p9, p10, 0xD6, 0x3C);
00066 void calculate_heading(float mx, float my, float mz);
00067 float oldX = 0;
00068 float oldY = 0;
00069 float oldZ = 0;
00070 float x= 0;
00071 float y = 0;
00072 float z  = 0;
00073 
00074 float posx = 0;
00075 float velx = 0;
00076 float oldPosx = 0;
00077 float oldVelx = 0;
00078 
00079 float posy = 0;
00080 float vely = 0;
00081 float oldPosy = 0;
00082 float oldVely = 0;
00083 
00084 float posz = 0;
00085 float velz = 0;
00086 float oldPosz = 0;
00087 float oldVelz = 0;
00088 int sample_time = 0;
00089 
00090 float accelx = 0, accely = 0, accelz = 0;
00091 Serial esp(p13, p14);           // tx, rx
00092 DigitalOut reset(p26);
00093 
00094 Timer t;
00095 void send_data(float *accelx, float *accely, float *accelz, float *velx, float *vely, float *velz);
00096 int  count,ended,timeout;
00097 float heading = 0.0;
00098 char buf[1024];
00099 char snd[1024];
00100 char str_buf[80];
00101  
00102 char ssid[32] = "4180_test";     // enter WiFi router ssid inside the quotes
00103 char pwd [32] = "4180isawesome"; // enter WiFi router password inside the quotes
00104  
00105 void SendCMD(),getreply(),ESPconfig(),ESPsetbaudrate();
00106 int main()
00107 {
00108     pc.printf("\f---------- Program Start ----------\r\n\n");
00109     reset=0; //hardware reset for 8266
00110     pc.baud(9600);  // set what you want here depending on your terminal program speed
00111     pc.printf("\f\n\r-------------ESP8266 Hardware Reset-------------\n\r");
00112     wait(0.5);
00113     reset=1;
00114     timeout=2;
00115     getreply();
00116     
00117     ESPconfig();        //******************  include Config to set the ESP8266 configuration  ***********************
00118 
00119     imu.begin();
00120     if (!imu.begin()) {
00121         pc.printf("Failed to communicate with LSM9DS1.\n");
00122     }
00123     imu.calibrate(1);
00124     imu.calibrateMag(0);
00125 
00126     working_setpoint = 0.0;
00127     
00128     encL.reset();
00129     encR.reset();
00130     feedbackL = encL.read();
00131     feedbackR = encR.read();
00132      
00133     // Update sensors and feedback twice as fast as PID sample time
00134     // this makes pid react in real-time avoiding errors due to 
00135     // missing counts etc. 
00136     
00137     motor.attach(&pid_callback, Ts_PID_CALLBACK);
00138 
00139     // Start PID sampling
00140     pidL.start();
00141     pidR.start();
00142     working_setpoint = 0;
00143     dirL = 1;
00144     dirR = 1;
00145     while(1)
00146     {
00147         if(blue.readable())
00148         {
00149             if (blue.getc()=='!')
00150             {
00151                 if (blue.getc()=='B') //button data
00152                 {
00153                     bnum = blue.getc(); //button number
00154                     if(bnum=='1') {
00155                         working_setpoint = 50;
00156                         sample_time = 1;
00157                         pc.printf("setpoint: %2.0f\r\n", working_setpoint);
00158                     }
00159                 }
00160             }
00161         } 
00162         while(!imu.accelAvailable());
00163         imu.readAccel();
00164         //while(!imu.magAvailable());
00165         imu.readMag();
00166         
00167         if( sample_time % 800 == 0)
00168         {
00169             velx = oldVelx + REFRESH_TIME_MS * oldX/1000;     
00170             posx = oldPosx + REFRESH_TIME_MS * oldVelx/1000;
00171             
00172             vely = oldVely + REFRESH_TIME_MS * oldY/1000;     
00173             posy = oldPosy + REFRESH_TIME_MS * oldVely/1000;
00174                         
00175             accelx = imu.calcAccel(imu.ax);
00176             accely = imu.calcAccel(imu.ay);
00177             accelz = imu.calcAccel(imu.az);
00178             calculate_heading(imu.calcMag(imu.mx), imu.calcMag(imu.my), imu.calcMag(imu.mz)); 
00179          
00180             #ifdef DEBUG
00181             pc.printf("        X axis    Y axis    Z axis\n\r");
00182             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));
00183             pc.printf("Veloc: %2.2f %2.2f %2.2f in G*s\r\n", velx, vely, velz);
00184             pc.printf("Magnetic heading: %2.2f", heading);
00185             #endif
00186            
00187             oldVelx = velx;
00188             oldPosx = posx;
00189             oldVely = vely;
00190             oldPosy = posy;
00191             oldX = imu.ax;
00192             oldY = imu.ay;
00193         }
00194         if(working_setpoint > 0)
00195         {
00196             if(sample_time % 1500 == 0)
00197             {   
00198                 #ifdef DEBUG
00199                 pc.printf("Pushing data to server\r\n");
00200                 #endif
00201                 
00202                 myled[0] = !myled[0];
00203                 working_setpoint = 0;
00204     
00205                 strcpy(snd, "srv:listen(80,function(conn)\r\n");
00206                 SendCMD();
00207                 wait(1);
00208                 strcpy(snd, "conn:on(\"receive\",function(conn,payload)\r\n");
00209                 SendCMD();
00210                 wait(1);
00211                 strcpy(snd, "print(payload)\r\n");
00212                 SendCMD();
00213                 wait(1);
00214                 strcpy(snd, "conn:send(\"<!DOCTYPE html>\")\r\n");
00215                 SendCMD();
00216                 wait(1);
00217                 
00218                 strcpy(snd, "conn:send(\"<html>\")\r\n");
00219                 SendCMD();
00220                 wait(1);
00221                 sprintf(str_buf,"%2.2f",velx);
00222                 strcpy(snd, "conn:send(\"<h1>Velocity x: ");
00223                 strcat(snd, str_buf);
00224                 strcat(snd, "</h1>\")\r\n");
00225                 SendCMD();
00226                 wait(1);
00227                 
00228                 sprintf(str_buf,"%2.2f",vely);
00229                 strcpy(snd, "conn:send(\"<h1>Velocity y: ");
00230                 strcat(snd, str_buf);
00231                 strcat(snd, "</h1>\")\r\n");
00232                 SendCMD();
00233                 wait(1);
00234                 
00235                 sprintf(str_buf,"%2.2f",heading);
00236                 strcpy(snd, "conn:send(\"<h1>Magnetic Heading: ");
00237                 strcat(snd, str_buf);
00238                 strcat(snd, "</h1>\")\r\n");
00239                 SendCMD();
00240                 wait(1);
00241                 
00242                 strcpy(snd, "conn:send(\"</html>\")\r\n");
00243                 SendCMD();
00244                 wait(1);
00245                 
00246                 strcpy(snd, "end)\r\n");
00247                 SendCMD();
00248                 wait(1);
00249                 
00250                 strcpy(snd, "conn:on(\"sent\",function(conn) conn:close() end)\r\n");
00251                 SendCMD();
00252                 wait(1);
00253                 strcpy(snd, "end)\r\n");
00254                 SendCMD();
00255                 wait(1);
00256                 timeout=17;
00257                 getreply();
00258                 pc.printf(buf);
00259                 pc.printf("\r\nDONE");
00260             }
00261             if(sample_time > 100000)
00262             {
00263                 sample_time = 1;
00264             } 
00265             sample_time++;
00266         }
00267     }
00268 }
00269 
00270 void ESPconfig()
00271 {
00272     wait(5);
00273     pc.printf("\f---------- Starting ESP Config ----------\r\n\n");
00274     strcpy(snd,".\r\n.\r\n");
00275     SendCMD();
00276     wait(1);
00277     pc.printf("---------- Reset & get Firmware ----------\r\n");
00278     strcpy(snd,"node.restart()\r\n");
00279     SendCMD();
00280     timeout=5;
00281     getreply();
00282     pc.printf(buf);
00283  
00284     wait(2);
00285  
00286     pc.printf("\n---------- Get Version ----------\r\n");
00287     strcpy(snd,"print(node.info())\r\n");
00288     SendCMD();
00289     timeout=4;
00290     getreply();
00291     pc.printf(buf);
00292     wait(3);
00293  
00294     // set CWMODE to 1=Station,2=AP,3=BOTH, default mode 1 (Station)
00295     pc.printf("\n---------- Setting Mode ----------\r\n");
00296     strcpy(snd, "wifi.setmode(wifi.STATION)\r\n");
00297     SendCMD();
00298     timeout=4;
00299     getreply();
00300     pc.printf(buf);
00301  
00302     wait(2);
00303 
00304     pc.printf("\n---------- Connecting to AP ----------\r\n");
00305     pc.printf("ssid = %s   pwd = %s\r\n",ssid,pwd);
00306     strcpy(snd, "wifi.sta.config(\"");
00307     strcat(snd, ssid);
00308     strcat(snd, "\",\"");
00309     strcat(snd, pwd);
00310     strcat(snd, "\")\r\n");
00311     SendCMD();
00312     timeout=10;
00313     getreply();
00314     pc.printf(buf);
00315  
00316     wait(5);
00317  
00318     pc.printf("\n---------- Get IP's ----------\r\n");
00319     strcpy(snd, "print(wifi.sta.getip())\r\n");
00320     SendCMD();
00321     timeout=3;
00322     getreply();
00323     pc.printf(buf);
00324  
00325     wait(1);
00326  
00327     pc.printf("\n---------- Get Connection Status ----------\r\n");
00328     strcpy(snd, "print(wifi.sta.status())\r\n");
00329     SendCMD();
00330     timeout=5;
00331     getreply();
00332     pc.printf(buf);
00333       
00334     pc.printf("\n---------- Setting up http server ----------\r\n");
00335     strcpy(snd, "srv=net.createServer(net.TCP)\r\n");
00336     SendCMD();
00337     wait(1);
00338     strcpy(snd, "srv:listen(80,function(conn)\r\n");
00339     SendCMD();
00340     wait(1);
00341     strcpy(snd, "conn:on(\"receive\",function(conn,payload)\r\n");
00342     SendCMD();
00343     wait(1);
00344     strcpy(snd, "print(payload)\r\n");
00345     SendCMD();
00346     wait(1);
00347     strcpy(snd, "conn:send(\"<!DOCTYPE html>\")\r\n");
00348     SendCMD();
00349     wait(1);
00350     
00351     strcpy(snd, "conn:send(\"<html>\")\r\n");
00352     SendCMD();
00353     wait(1);
00354     
00355     strcpy(snd, "conn:send(\"<h1>WIFI Monitoring Program</h1>\")\r\n");
00356     SendCMD();
00357     wait(1);
00358     
00359     strcpy(snd, "conn:send(\"</html>\")\r\n");
00360     SendCMD();
00361     wait(1);
00362     
00363     strcpy(snd, "end)\r\n");
00364     SendCMD();
00365     wait(1);
00366     
00367     strcpy(snd, "conn:on(\"sent\",function(conn) conn:close() end)\r\n");
00368     SendCMD();
00369     wait(1);
00370     strcpy(snd, "end)\r\n");
00371     SendCMD();
00372     wait(1);
00373     timeout=17;
00374     getreply();
00375     pc.printf(buf);
00376     pc.printf("\r\nDONE");
00377 
00378 }
00379 
00380 void SendCMD()
00381 {
00382     esp.printf("%s", snd);
00383 }
00384  
00385 void getreply()
00386 {
00387     memset(buf, '\0', sizeof(buf));
00388     t.start();
00389     ended=0;
00390     int count=0;
00391     while(!ended) {
00392         if(esp.readable()) {
00393             buf[count] = esp.getc();
00394             count++;
00395         }
00396         if(t.read() > timeout) {
00397             ended = 1;
00398             t.stop();
00399             t.reset();
00400         }
00401     }
00402 }
00403 void calculate_heading(float mx, float my, float mz)
00404 {
00405 
00406 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
00407     mx = -mx;
00408     if (my == 0.0)
00409         heading = (mx < 0.0) ? 180.0 : 0.0;
00410     else
00411         heading = atan2(mx, my)*360.0/(2.0*PI);
00412     heading -= DECLINATION; //correct for geo location
00413     if(heading>180.0) heading = heading - 360.0;
00414     else if(heading<-180.0) heading = 360.0 + heading;
00415     else if(heading<0.0) heading = 360.0  + heading;
00416     
00417     #ifdef DEBUG
00418     pc.printf("Magnetic Heading: %f degress\n\r",heading);
00419     #endif
00420 }
00421 
00422 void pid_callback()
00423 {
00424 
00425     // Deal with feedback and update motors
00426     // Motor direction based on working setpoint var
00427 
00428     //myled[0] = dirL;
00429     //myled[1] = dirR;
00430     // Setpoint vars used by PID objects are concerned with 
00431     // only SPEED not direction. 
00432     setpoint = abs(working_setpoint);
00433 
00434     float k = Ts_PID_CALLBACK;    // Discrete time, (Ts/2 because this callback is called
00435                                   // at interval of Ts/2... or twice as fast as pid controller)
00436     static int last_count_L = 0;
00437     static int last_count_R = 0;
00438     int countL = encL.read();
00439     int countR = encR.read();
00440     //pc.printf("%d\r\n", countL);
00441     
00442     // Because encoders are not quadrature we must handle the sign ourselves, 
00443     // i.e. explicitly make calcs based on the direction we have set the motor
00444     float raw_speed_L = ((countL - last_count_L)*FEEDBACK_SCALE) / k; 
00445     float rpm_speed_L = raw_speed_L * 60.0;     // Convert speed to RPM
00446     
00447     float raw_speed_R = ((countR - last_count_R)*FEEDBACK_SCALE) / k; 
00448     float rpm_speed_R = raw_speed_R * 60.0;     // Convert speed to RPM
00449     
00450     last_count_L = countL;                      // Save last count
00451     last_count_R = countR;
00452     feedbackL = rpm_speed_L;
00453     feedbackR = rpm_speed_R;
00454     
00455     mtrL.forceSetSpeed(outputL * dirL);
00456     mtrR.forceSetSpeed(outputR * dirR);
00457 }
00458 
00459 /*
00460     Clips value to lower/ uppper
00461     @param value    The value to clip
00462     @param lower    The mininum allowable value
00463     @param upper    The maximum allowable value
00464     @return         The resulting clipped value
00465 */
00466 float clip(float value, float lower, float upper){
00467     return std::max(lower, std::min(value, upper));
00468 }