Revision 9:3aa9b689bda5, committed 2018-04-03

Comitter:

jdawkins

Date:

Tue Apr 03 12:52:52 2018 +0000

Parent:

8:9817993e5df7

Commit message:

Publish Code for MIDS

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Emaxx_Navigation_madpulse.lib	Tue Apr 03 12:52:52 2018 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/teams/Emaxx-Navigation-Group/code/Emaxx_Navigation_Dynamic_HIL/#9817993e5df7

--- a/main.cpp	Fri Apr 07 17:12:29 2017 +0000
+++ b/main.cpp	Tue Apr 03 12:52:52 2018 +0000
@@ -57,14 +57,11 @@
 ServoIn CH1(p15);
 ServoIn CH2(p16);
 
-InterruptIn he_sensor(p11);
-ServoOut Steer(p22);
+InterruptIn he_sensor(p15);
+ServoOut Steer(p20);
 ServoOut Throttle(p21);
 
 
-
-
-
 Timer t; // create timer instance
 Ticker log_tick;
 Ticker heartbeat;
@@ -119,7 +116,7 @@
     xbee.attach(&rxCallback, MODSERIAL::RxIrq);
    
 
-    he_sensor.rise(&he_callback); // Set wheel speed sensor interrupt
+   // he_sensor.rise(&he_callback); // Set wheel speed sensor interrupt
 
     // initialize necessary float and boolean variables
     left = 0;
@@ -223,6 +220,7 @@
             rc_conn = true;
         } // end if(Channels connected)
 
+       // pc.printf("Str %d Thr %d\r\n",CH1.servoPulse,CH2.servoPulse);
         // turn on RC led if transmitter is connected
         rc_LED = rc_conn;
         auto_LED = auto_ctrl;
@@ -245,7 +243,6 @@
             int i = 0;
             if(xbee.rxBufferGetCount()>0) {
                 char c = xbee.getc();
-                //pc.printf("%s",c);
                 if(c=='$') {
                     buf[i] = c;
                     i++;
@@ -259,10 +256,11 @@
                 }
             } // end if xbee.rxBufferGetCount
             xbee.rxBufferFlush();// empty receive buffer
-            //pc.printf("%s",buf); // print message to PC
+            pc.printf("%s",buf); // print message to PC
             parseMessage(buf);
             newpacket = false; // reset packet flag
         } // end if(newpacket)
+        
         float cdt = t.read()-t_ctrl;
         t_ctrl = t.read();
 
@@ -277,7 +275,6 @@
 
                         str = -str_cmd;
 
-
                         des_spd = thr_cmd;
                         spd_err = des_spd - speed;
                         spd_err_i += spd_err*cdt;
@@ -310,30 +307,6 @@
                     } // 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();
-                        } 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;
-                             }*/
-                        } // end if sim_mode
-
-
-
                         thr =  Kp_spd*spd_err + Ki_spd*spd_err_i*cdt;
 
                         if (thr >= 1.0) {
@@ -357,14 +330,6 @@
 
                 } // end switch(cmd_mode)
                 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
-                } else {
-                    // won't send command to motor and servo if in SIL mode
-                }
-
 
             } else { // goes with if auto_ctrl, manual control mode
 
@@ -390,15 +355,14 @@
 
         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
+        thr_out_old = thr_out;//
+
             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);
-        }
+            pc.printf("crrl cmd: %f %f\r\n",str,thr);
 
-        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)) {
                 imu.get_angles();
@@ -427,40 +391,16 @@
                 }
             }
 
-
-            } // end if simmode ==0
-
-
-
-
-
-        } else { // Else we are running a simulation
-            float T = t.read()-t_sim;
-            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()));
-                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());
-                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));
                 // 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();
         }
 
@@ -468,7 +408,7 @@
         // 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)) {

--- a/motor_driver.h	Fri Apr 07 17:12:29 2017 +0000
+++ b/motor_driver.h	Tue Apr 03 12:52:52 2018 +0000
@@ -1,7 +1,7 @@
 
-DigitalOut mot_ph1(p29, 0);       
-DigitalOut mot_ph2(p30, 0);
-PwmOut mot_en(p26);
+DigitalOut mot_ph1(p23, 0);       
+DigitalOut mot_ph2(p24, 0);
+PwmOut mot_en(p25);
 
 void mot_control(float dc){        
     if(dc>1.0)