Revision 8:9817993e5df7, committed 2017-04-07

Comitter:

jdawkins

Date:

Fri Apr 07 17:12:29 2017 +0000

Parent:

7:a8c2e9d049e8

Commit message:

Commit for Capstone Students

Changed in this revision

--- a/GPSINT.lib	Fri Dec 16 14:06:42 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://developer.mbed.org/teams/Emaxx-Navigation-Group/code/GPSINT/#19bc24237a6e

--- a/Navigation_EKF.lib	Fri Dec 16 14:06:42 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://developer.mbed.org/teams/Emaxx-Navigation-Group/code/Navigation_EKF/#87fa7be4967a

--- a/NeoStrip.lib	Fri Dec 16 14:06:42 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://developer.mbed.org/teams/Emaxx-Navigation-Group/code/NeoStrip/#b2687bc7cafe

--- a/Vehicle_Model.lib	Fri Dec 16 14:06:42 2016 +0000
+++ b/Vehicle_Model.lib	Fri Apr 07 17:12:29 2017 +0000
@@ -1,1 +1,1 @@
-https://developer.mbed.org/teams/Emaxx-Navigation-Group/code/Vehicle_Model/#18012b4b3f65
+https://developer.mbed.org/teams/Emaxx-Navigation-Group/code/Vehicle_Model/#f669ee7c11e7

--- a/main.cpp	Fri Dec 16 14:06:42 2016 +0000
+++ b/main.cpp	Fri Apr 07 17:12:29 2017 +0000
@@ -1,22 +1,21 @@
 #include "mbed.h"
 #include "utilityFunctions.h"
-#include "GPSINT.h"
 #include "BNO055.h"
-#include "nav_ekf.h"
 #include "ServoIn.h"
 #include "ServoOut.h"
-#include "NeoStrip.h"
 #include "MODSERIAL.h"
 #include "vehicleModel.h"
-
+#include "motor_driver.h"
 
 #define MAX_MESSAGE_SIZE 64
 #define IMU_RATE 100.0
-#define LOG_RATE 20.0
-#define GPS_RATE 1.0
+#define LOG_RATE 10.0
 #define LOOP_RATE 300.0
 #define CMD_TIMEOUT 1.0
-#define GEAR_RATIO (1/2.75)
+#define GEAR_RATIO (1/2.75) // gear ratio
+#define WHL_RADIUS  0.036    // Wheel radius (m)
+#define PI 3.14159
+
 // Reference origin is at entrance to hospital point monument
 #define REF_LAT 38.986534
 #define REF_LON -76.489914
@@ -28,34 +27,29 @@
 #define DIRECT_MODE 0 //command maps to throttle and steer commands normalized
 #define COURSE_MODE 1 //Commands map to heading and speed
 
-#define HARDWARE_MODE 0 
+#define HARDWARE_MODE 0
 #define HIL_MODE 1 // commands map to hardware active simulation
 #define SIL_MODE 2 // commands map to software only simulation
 
 //I2C    i2c(p9, p10); // SDA, SCL
 BNO055 imu(p9, p10);
-GPSINT gps(p13,p14);
 
-vector <int> str_buf;
 int left;
 
 // Function Prototypes
-//float saturateCmd(float cmd);
-//float wrapToPi(float ang);
+void he_callback();
 int parseMessage(char * msg);
-void setLED(int *colors, float brightness);
-
 
 // LED Definitions
 DigitalOut rc_LED(LED1);
 DigitalOut armed_LED(LED2);
 DigitalOut auto_LED(LED3);
-DigitalOut imu_LED(LED4);
+// DigitalOut imu_LED(LED4);
+DigitalOut hall_LED(LED4);
 
-NeoStrip leds(p6,LED_CLUSTERS*LED_PER_CLUSTER);
 
 // Comms and control object definitions
-//MODSERIAL pc(p13, p14); // tx, rx for serial USB interface to pc
+//MODSERIAL pc(p28, p27); // tx, rx for serial USB interface to pc
 //MODSERIAL xbee(USBTX, USBRX); // tx, rx for Xbee
 
 MODSERIAL pc(USBTX, USBRX); // tx, rx for serial USB interface to pc
@@ -63,16 +57,20 @@
 ServoIn CH1(p15);
 ServoIn CH2(p16);
 
-//InterruptIn he_sensor(p11);
-
+InterruptIn he_sensor(p11);
 ServoOut Steer(p22);
 ServoOut Throttle(p21);
+
+
+
+
+
 Timer t; // create timer instance
 Ticker log_tick;
 Ticker heartbeat;
 float t_imu,t_gps,t_cmd,t_sim,t_ctrl;
 float str_cmd,thr_cmd,str,thr,des_psi,des_spd; // control parameters
-float psi_err,spd_err, psi_err_i,spd_err_i; // control variables
+float psi_err,spd_err, psi_err_i,spd_err_i,speed; // control variables
 float t_hall, dt_hall,t_run,t_stop,t_log;
 bool armed, auto_ctrl,auto_ctrl_old,rc_conn; // arming state modes
 float wheel_spd; // wheel speed variable
@@ -90,10 +88,16 @@
 float y = 0.0; // simulation variables
 float psi = 0.0; // simulation variables
 float spd = 0.0; // simulation speed
+float tau = 0.25; // throttle time constant
+float Kdc = 1; // throttle dc gain
+float thr_out,thr_out_old;
+float KpWhl = 0.6;
+float dz;
+int reverse;
 
-nav_EKF ekf; // initialize ekf states
 vehicleModel vm; //create vehicle model object
 
+
 void rxCallback(MODSERIAL_IRQ_INFO *q)
 {
     MODSERIAL *serial = q->serial;
@@ -108,14 +112,14 @@
 
 
     pc.baud(115200); // set baud rate of serial comm to pc
-    xbee.baud(57600); // set baud rate of serial comm of wireless xbee comms
+    xbee.baud(115200); // set baud rate of serial comm of wireless xbee comms
     Steer.write(1500);      //Set Steer PWM to center 1000-2000 range
     Throttle.write(1500);   //Set Throttle to Low
 
     xbee.attach(&rxCallback, MODSERIAL::RxIrq);
-
+   
 
-    led1=led2=led3=led4=WHITE; // set color of neo strip lights?
+    he_sensor.rise(&he_callback); // Set wheel speed sensor interrupt
 
     // initialize necessary float and boolean variables
     left = 0;
@@ -131,8 +135,9 @@
     arm_clock = 0;
     auto_clock = 0;
     Kp_psi = 1/1.57;
-    Kp_spd = 0.3;
-    Ki_spd = 0.05;
+    Kp_spd = 0.4;
+    Ki_spd = 0.2;
+    dz = 0.2;
     str_cond = false;
     thr_cond = false;
     armed = false;
@@ -142,21 +147,20 @@
     run_ctrl = false;
     log_data = false;
     cmd_mode = 1;
+    reverse = 0;
+    
 
     // timer and timing initializations
     t.start();
     t_imu = t.read();
-    t_gps = t.read();
     t_sim = t.read();
     t_ctrl = t.read();
     t_log = t.read();
     t_cmd = 0;
 
-    leds.setBrightness(0.5); // set brightness of leds
 
     rc_LED = 0; // turn off LED 1 to indicate no RC connection
-    imu_LED = 0; // turn off IMU indicator (LED 4)
-    gps.setRefPoint(REF_LAT,REF_LON,REF_ALT); // set local origin of reference frame for GPS conversion
+//    imu_LED = 0; // turn off IMU indicator (LED 4)
 
 
     // procedure to ensure IMU is operating correctly
@@ -178,12 +182,12 @@
             imu_LED = !imu_LED;
         } // end while(imu.calib)
         */
-        imu_LED = 1; // turns on IMU light when calibration is successful
+        //imu_LED = 1; // turns on IMU light when calibration is successful
 
     } else {
         pc.printf("IMU BNO055 NOT connected\r\n Entering Simulation mode..."); // catch statement if IMU is not connected correctly
 
-        sim_mode = 2; // by default it will go to simulation mode without actuators active (SIL)
+        //sim_mode = 2; // by default it will go to simulation mode without actuators active (SIL)
 
 
         /* // turn on all lights is IMU is not connected correctly
@@ -227,7 +231,7 @@
         str_cond = (CH1.servoPulse > 1800); // If steering is full right
         thr_cond = abs(CH2.servoPulse-1500)<100; // If throttle is near zero
 
-        if(t.read()-auto_clock > 3) { //Auto control timeout if no commands recevied after 3 seconds
+        if(t.read()-auto_clock > 1) { //Auto control timeout if no commands recevied after 1 seconds
             auto_ctrl = false;
         } // end if(t.read()-autoclock>3) timeout procedure
 
@@ -259,46 +263,77 @@
             parseMessage(buf);
             newpacket = false; // reset packet flag
         } // end if(newpacket)
+        float cdt = t.read()-t_ctrl;
+        t_ctrl = t.read();
 
 
         if(!rc_conn) { // Is System Armed, system armed if RC not connected
             // printf("auto control: %d, clock %f\r\n",auto_ctrl, t.read()-auto_clock);
+            if(auto_ctrl) {
 
-            if(auto_ctrl) {
-                float cdt = t.read()-t_ctrl;
-                t_ctrl = t.read();
+
                 switch (cmd_mode) {
                     case DIRECT_MODE: {
-                        str = str_cmd;
-                        thr = thr_cmd;
-                        //  pc.printf("Psi Err: %.3f Spd Err %.3f, Str Cmd %.3f Thr_cmd %.3f\r\n",psi_err,spd_err,str,thr);
+
+                        str = -str_cmd;
+
+
+                        des_spd = thr_cmd;
+                        spd_err = des_spd - speed;
+                        spd_err_i += spd_err*cdt;
+
+                        // saturate integral term
+                        if(spd_err_i> 1) {
+                            spd_err_i = 1;
+                        }
+                        if(spd_err<-1) {
+                            spd_err_i = -1;
+                        }
+                        
+                        thr = Kp_spd*(des_spd-speed) + Ki_spd*spd_err_i;
+                        
+                        if(des_spd > 0.02){
+                        thr = thr + dz;    
+                        }   
+                        else if(des_spd < -0.02){
+                         // speed has no sign figure out how to make it track direction using accel perhaps.
+                         thr = -(dz+0.2); 
+                         //thr = 0.3*des_spd;   
+                        }else {
+                         thr = 0; 
+                         spd_err_i = 0;   
+                        }
+                        //thr = thr_cmd;
+                        //  pc.printf("des_spd %.3f, Spd %.3f Thr_cmd %.3f rev %d\r\n",des_spd,reverse*speed,thr,reverse);
                         //  xbee.printf("Psi Err: %.3f Spd Err %.3f, Str Cmd %.3f Thr_cmd %.3f\r\n",psi_err,spd_err,str,thr);
                         break;
                     } // end direct mode case
                     case COURSE_MODE: {
 
                         if(sim_mode==0) { // if hardware is enabled use gyro and ekf
-                            psi_err = wrapToPi(des_psi-imu.euler.yaw); 
-                            
-                            spd_err = des_spd - ekf.getSpd();
-                            spd_err_i += spd_err;
-                            str = -Kp_psi*psi_err/ekf.getSpd();
+                            //  psi_err = wrapToPi(des_psi-imu.euler.yaw);
+
+                            //  spd_err = des_spd - ekf.getSpd();
+                            //  spd_err_i += spd_err;
+                            //  str = -Kp_psi*psi_err/ekf.getSpd();
                         } else { // otherwise design control using simulated variables, bypass ekf states
- 
-                            psi_err = wrapToPi(des_psi-vm.getYaw());
-                            str = Kp_psi*psi_err;         
-                                                    
-                            spd_err = des_spd - vm.getSpd();
-                            spd_err_i += spd_err;
-                              
-                             
-                           /* if(spd>0.05) {
-                                str = Kp_psi*psi_err/vm.getSpd();
-                            } else {
-                                str = Kp_psi*psi_err/0.05;
-                            }*/
+
+                            //  psi_err = wrapToPi(des_psi-vm.getYaw());
+                            // str = Kp_psi*psi_err;
+
+                            // spd_err = des_spd - vm.getSpd();
+                            // spd_err_i += spd_err;
+
+
+                            /* if(spd>0.05) {
+                                 str = Kp_psi*psi_err/vm.getSpd();
+                             } else {
+                                 str = Kp_psi*psi_err/0.05;
+                             }*/
                         } // end if sim_mode
 
+
+
                         thr =  Kp_spd*spd_err + Ki_spd*spd_err_i*cdt;
 
                         if (thr >= 1.0) {
@@ -310,9 +345,9 @@
                             spd_err_i = 0.0;  // Reset Integral If Saturated
                         } // end iff thr<0
 
-                        
-                        pc.printf("Psi Err: %.3f,  Str Cmd %.3f, Spd Err %.3f, Kp %.3f, Thr_cmd %.3f\r\n",psi_err,str,spd_err,Kp_spd,thr);
-                     
+
+                       // pc.printf("Psi Err: %.3f,  Str Cmd %.3f, Spd Err %.3f, Kp %.3f, Thr_cmd %.3f rev %d\r\n",psi_err,str,spd_err,Kp_spd,thr,reverse);
+
                         break;
 
                     } // end COURSE_MODE case
@@ -324,13 +359,13 @@
                 str = saturateCmd(str);
                 //xbee.printf("Psi: %.3f Psi Err: %.3f Str Cmd %.3f\r\n",vm.getYaw(),psi_err,str);
                 if(sim_mode<2) { // only actuates if in experiment or HIL modes
-                    Steer.write((int)((str*500.0)+1500.0));  // Write Steering Pulse
-                    Throttle.write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse
+                    //    Steer.write((int)((str*500.0)+1500.0));  // Write Steering Pulse
+                    //   Throttle.write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse
                 } else {
                     // won't send command to motor and servo if in SIL mode
                 }
 
-                
+
             } else { // goes with if auto_ctrl, manual control mode
 
                 Steer.write((int)((str*500.0)+1500.0));  // Write Steering Pulse
@@ -345,14 +380,24 @@
             armed_LED = 0;          //Turn off armed LED indicator
             str = ((CH1.servoPulse-1500.0)/500.0);  // Convert Pulse to Normalized Command +/- 1
             thr = ((CH2.servoPulse-1500.0)/500.0);  // Convert Pulse to Normalized Command +/- 1
-            
+
             if(sim_mode<2) { // if hardware is active send command to servo and throttle
-                Steer.write((int)((str*500.0)+1500.0));  // Write Steering Pulse
-                Throttle.write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse
+                //Steer.write((int)((str*500.0)+1500.0));  // Write Steering Pulse
+                // Throttle.write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse
             }
 
         }/// end  else armed
 
+        float thr_dot = -(1/tau)*thr_out + (Kdc/tau)*thr; // first order filter for throttle
+        thr_out = thr_out_old + thr_dot*cdt;
+        thr_out_old = thr_out;
+        if(sim_mode<2) { // if hardware is active send command to servo and throttle
+            Steer.write((int)((str*500.0)+1500.0));  // Write Steering Pulse
+            //Throttle.write((int)((thr_out*500.0)+1500.0)); //Write Throttle Pulse
+            mot_control(thr);
+          //  pc.printf("Motcontrol: %f %f\r\n",thr,thr_out);
+        }
+
         if(sim_mode==0) { // reads from IMU if actual hardware is present and sim mode is off
             float dt = t.read()-t_imu;
             if(dt > (1/IMU_RATE)) {
@@ -360,79 +405,114 @@
                 imu.get_accel();
                 imu.get_gyro();
                 imu.get_lia();
-                
+
                 float tic = t.read();
-                ekf.setRot(wrapToPi(imu.euler.yaw),imu.euler.pitch,imu.euler.roll);
-                ekf.timeUpdate(imu.lia.x,imu.lia.y,imu.lia.z,dt);
                 t_imu = t.read();
 
+            // *******************************************
+            // Get Sensor Data
+            // *******************************************
+            // Compute speed from hall effect sensor
+            if(t.read()-t_hall < 0.2) {
+                speed = 0.5*WHL_RADIUS*GEAR_RATIO*(2*PI)/(dt_hall);   // inches/sec
+            } else {
+                speed = 0;
             }
-        }else{ // Else we are running a simulation
+
+            if(abs(speed) <0.01){
+                 if(imu.accel.x < -0.5 && thr_out < -0.05){
+                 reverse = -1;    
+                }else{
+                 reverse = 1;    
+                }
+            }
+
+
+            } // end if simmode ==0
+
+
+
+
+
+        } else { // Else we are running a simulation
             float T = t.read()-t_sim;
-            vm.stepModel(str,thr,T);            
+            vm.stepModel(str,thr,T);
+            speed = vm.getSpd();
             t_sim = t.read();
-        }      
-            
+        }
+
         if(t.read()-t_log > (1/LOG_RATE)) {
-                             
+
             if(sim_mode > 0) {
-               xbee.printf("$EKF,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",vm.getPosNorth(),vm.getPosEast(),vm.getVelNorth(),vm.getVelEast(),wrapToPi(vm.getYaw())); 
-               xbee.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",vm.getAx(),vm.getAy(),0,0,0,vm.getYawRate(),0,0,wrapToPi(vm.getYaw()));
-               
-              // pc.printf("$EKF,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",vm.getPosNorth(),vm.getPosEast(),vm.getVelNorth(),vm.getVelEast(),wrapToPi(vm.getYaw()));                
-                              
-              //  pc.printf("$SIM,%.2f,%.2f,%.2f,%.2f\r\n",vm.getPosNorth(),vm.getPosEast(),vm.getVelNorth(),vm.getVelEast(),wrapToPi(vm.getYaw()));
-              //  pc.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",vm.getAx(),vm.getAy(),0,0,0,vm.getYawRate(),0,0,wrapToPi(vm.getYaw()));
+                xbee.printf("$EKF,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",vm.getPosNorth(),vm.getPosEast(),vm.getVelNorth(),vm.getVelEast(),wrapToPi(vm.getYaw()));
+                xbee.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",vm.getAx(),vm.getAy(),0,0,0,vm.getYawRate(),0,0,wrapToPi(vm.getYaw()));
+                printf("$EKF,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",vm.getPosNorth(),vm.getPosEast(),vm.getVelNorth(),vm.getVelEast(),wrapToPi(vm.getYaw()));
+                printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",vm.getAx(),vm.getAy(),0,0,0,vm.getYawRate(),0,0,wrapToPi(vm.getYaw()));
+                //  pc.printf("$SIM,%.2f,%.2f,%.2f,%.2f\r\n",vm.getPosNorth(),vm.getPosEast(),vm.getVelNorth(),vm.getVelEast(),wrapToPi(vm.getYaw()));
+                //  pc.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",vm.getAx(),vm.getAy(),0,0,0,vm.getYawRate(),0,0,wrapToPi(vm.getYaw()));
             } else {
-                xbee.printf("$EKF,%.2f,%.2f,%.2f,%.2f\r\n",ekf.getPosNorth(),ekf.getPosEast(),ekf.getVelNorth(),ekf.getVelEast());
+                //xbee.printf("$EKF,%.2f,%.2f,%.2f,%.2f\r\n",ekf.getPosNorth(),ekf.getPosEast(),ekf.getVelNorth(),ekf.getVelEast());
+                pc.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",imu.lia.x,imu.lia.y,imu.lia.z,imu.gyro.x,imu.gyro.y,imu.gyro.z,imu.euler.roll,imu.euler.pitch,wrapToPi(imu.euler.yaw));
                 xbee.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",imu.lia.x,imu.lia.y,imu.lia.z,imu.gyro.x,imu.gyro.y,imu.gyro.z,imu.euler.roll,imu.euler.pitch,wrapToPi(imu.euler.yaw));
-              //  pc.printf("$EKF,%.2f,%.2f,%.2f,%.2f\r\n",ekf.getPosNorth(),ekf.getPosEast(),ekf.getVelNorth(),ekf.getVelEast());
-              //  pc.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",imu.lia.x,imu.lia.y,imu.lia.z,imu.gyro.x,imu.gyro.y,imu.gyro.z,imu.euler.roll,imu.euler.pitch,wrapToPi(imu.euler.yaw));
+                // xbee.printf("Thr in %.3f Thr_out %.3f\r\n",thr,thr_out);
+                xbee.printf("$ODO,%.2f,%.2f,%.2f\r\n",speed,thr_out,str);
+                //pc.printf("$ODO,%.2f,%.2f,%.2f\r\n",speed,thr_out,str);
+
+                //  pc.printf("$EKF,%.2f,%.2f,%.2f,%.2f\r\n",ekf.getPosNorth(),ekf.getPosEast(),ekf.getVelNorth(),ekf.getVelEast());
+                //pc.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",imu.lia.x,imu.lia.y,imu.lia.z,imu.gyro.x,imu.gyro.y,imu.gyro.z,imu.euler.roll,imu.euler.pitch,wrapToPi(imu.euler.yaw));
             }
             t_log = t.read();
         }
 
-            if(t.read()-t_gps >(1/GPS_RATE)) {
-                float tic2 = t.read();
-                ekf.measUpdate(gps.pos_north,gps.pos_east,gps.vel_north,gps.vel_east);
-                t_gps = t.read();
-            }
-            wait(1/LOOP_RATE);
-            // status_LED=!status_LED;
-            auto_ctrl_old = auto_ctrl;
+        wait(1/LOOP_RATE);
+        // status_LED=!status_LED;
+        auto_ctrl_old = auto_ctrl;
 
-       /* } else { // else condition implies a simulation mode is enabled
+        /* } else { // else condition implies a simulation mode is enabled
 
-            float dt = t.read()-t_imu;
-            if(dt > (1/IMU_RATE)) {
+             float dt = t.read()-t_imu;
+             if(dt > (1/IMU_RATE)) {
 
-                float tic = t.read();
+                 float tic = t.read();
 
 
-                x = x + spd*cos(psi)*dt; // self-propelled particle kinematics
-                y = y + spd*sin(psi)*dt; // self-propelled particle kinematics
-                psi = psi + str*dt; // turn rate kinematics
-                float drag = 0.0;
-                if(spd>1) {
-                    drag = 0.0059*spd*spd; // based on drag on a cat sized object
-                } else {
-                    drag = 0.0059*spd;
-                }
-                spd = spd + (thr-drag)*dt; // give throttle offset
+                 x = x + spd*cos(psi)*dt; // self-propelled particle kinematics
+                 y = y + spd*sin(psi)*dt; // self-propelled particle kinematics
+                 psi = psi + str*dt; // turn rate kinematics
+                 float drag = 0.0;
+                 if(spd>1) {
+                     drag = 0.0059*spd*spd; // based on drag on a cat sized object
+                 } else {
+                     drag = 0.0059*spd;
+                 }
+                 spd = spd + (thr-drag)*dt; // give throttle offset
 
-                xbee.printf("$SIM,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",x,y,spd*cos(psi),spd*sin(psi),wrapToPi(psi));
-                pc.printf("$SIM,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",x,y,spd*cos(psi),spd*sin(psi),wrapToPi(psi),thr,des_spd,spd,str);
-                t_imu = t.read();
-            } // end if dt
+                 xbee.printf("$SIM,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",x,y,spd*cos(psi),spd*sin(psi),wrapToPi(psi));
+                 pc.printf("$SIM,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",x,y,spd*cos(psi),spd*sin(psi),wrapToPi(psi),thr,des_spd,spd,str);
+                 t_imu = t.read();
+             } // end if dt
 
 
-        } // end if sim_mode */
+         } // end if sim_mode */
 
 
     } // end while(1)
 
 } // end main
 
+
+
+void he_callback()
+{
+    // Hall effect speed sensor callback
+    hall_LED = !hall_LED;
+
+    dt_hall = t.read()-t_hall;
+    t_hall = t.read();
+} // end void he_callback
+
+
+
 int parseMessage(char * msg)
 {
 
@@ -457,7 +537,7 @@
             default: {
             }
         }
-     return 1;
+        return 1;
     }   //emd of $CMD
 
     if(!strncmp(msg, "$PRM", 4)) {
@@ -485,7 +565,7 @@
                 float ref_lat = arg1;
                 float ref_lon = arg2;
                 float ref_alt = arg3;
-                gps.setRefPoint(ref_lat,ref_lon,ref_alt);
+                // gps.setRefPoint(ref_lat,ref_lon,ref_alt);
 
                 pc.printf("Params Received: %f %f %f\n",arg1,arg2,arg3);
                 //xbee.printf("Params Recieved: %f %f %f\r\n",arg1,arg2,arg3);
@@ -494,23 +574,11 @@
             }
             default: {
             }
-            
+
         }
         return 1;
     } // end of $PRM
 
-    if(!strncmp(msg, "$LED", 4)) {
-        int arg1;
-        float arg2;
-        sscanf(msg,"$LED,%x,%f",&arg1,&arg2);
-        //pc.printf("%s\n",msg);
-        int colors[4]= {arg1,arg1,arg1,arg1};
-        float brightness = arg2;
-        setLED(colors,brightness);
-        return 1;
-    } // end of $LED
-
-
     if(!strncmp(msg, "$SIM", 4)) {
         int arg1;
         float arg2,arg3,arg4;
@@ -530,7 +598,7 @@
                 y = arg3;
                 psi = arg4;
                 vm.initialize(x,y,psi);
-                
+
             }
             default: {
             }
@@ -540,22 +608,3 @@
     return 0;
 
 }
-void setLED(int  *colors,float brightness)
-{
-
-    leds.setBrightness(brightness);
-
-    int cidx = 0;
-    int ctr = 0;
-    for (int i=0; i<LED_PER_CLUSTER*LED_CLUSTERS; i++) {
-
-        if(ctr >11) {
-            ctr = 0;
-            cidx++;
-        }
-        leds.setPixel(i,colors[cidx]);
-        ctr++;
-    }
-    leds.write();
-}
-

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motor_driver.h	Fri Apr 07 17:12:29 2017 +0000
@@ -0,0 +1,29 @@
+
+DigitalOut mot_ph1(p29, 0);       
+DigitalOut mot_ph2(p30, 0);
+PwmOut mot_en(p26);
+
+void mot_control(float dc){        
+    if(dc>1.0)
+        dc=1.0;
+        
+    if(dc<-1.0)
+        dc=-1.0;
+    
+        if(dc > 0.0){
+            mot_ph2 = 0;
+            mot_ph1 = 1;
+            mot_en = dc;
+        }
+        else if(dc < -0.0){
+            mot_ph1 = 0;
+            mot_ph2 = 1;
+            mot_en = abs(dc);
+        }
+        else{
+            mot_ph1 = 0;
+            mot_ph2 = 0;
+            mot_en = 0.0;
+        }
+                       
+}
\ No newline at end of file