Diff: main.cpp

Revision:

5:d6d8ecd418cf

Parent:

4:e27ca0c82814

Child:

6:f64b1eba4d5e

--- a/main.cpp	Thu Dec 08 20:38:24 2016 +0000
+++ b/main.cpp	Sat Dec 10 01:44:58 2016 +0000
@@ -85,7 +85,7 @@
 float x = 0.0; // simulation variables
 float y = 0.0; // simulation variables
 float psi = 0.0; // simulation variables
-float spd = 0.005; // simulation speed
+float spd = 0.0; // simulation speed
 
 void rxCallback(MODSERIAL_IRQ_INFO *q)
 {
@@ -225,7 +225,7 @@
 
 
         if(newpacket) { // if xbee port receives a complete message, parse it
-            char buf[MAX_MESSAGE_SIZE];
+            char buf[MAX_MESSAGE_SIZE]; // create buffer for message
 
             // reads from modserial port buffer, stores characters into string "buf"
             int i = 0;
@@ -243,15 +243,15 @@
                         i++;
                     }
                 }
-            }
+            } // end if xbee.rxBufferGetCount
             xbee.rxBufferFlush();// empty receive buffer
-            pc.printf("%s",buf);
+            pc.printf("%s",buf); // print message to PC
             parseMessage(buf);
-            newpacket = false;
+            newpacket = false; // reset packet flag
         } // end if(newpacket)
 
 
-        if(!rc_conn) { // Is System Armed
+        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) {
 
@@ -265,10 +265,21 @@
                     } // end direct mode case
                     case COURSE_MODE: {
 
-                        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();
+                        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-psi);
+                            spd_err = des_spd - spd;
+                            spd_err_i += spd_err;
+                            if(spd>0.05) {
+                                str = Kp_psi*psi_err/spd;
+                            } else {
+                                str = Kp_psi*psi_err/0.05;
+                            }
+                        } // end if sim_mode
 
                         thr =  Kp_spd*spd_err + Ki_spd*spd_err_i/LOOP_RATE;
 
@@ -276,8 +287,8 @@
                             thr = 1.0;
                             spd_err_i = 0; // Reset Integral If Saturated
                         } // end if thr>=1.0
-                        if (thr < 0) {
-                            thr = 0;
+                        if (thr < 0.0) {
+                            thr = 0.0;
                             spd_err_i = 0;  // Reset Integral If Saturated
                         } // end iff thr<0
 
@@ -308,12 +319,16 @@
             } // end else
 
         } else { // goes with if !rc_conn
+
+            // for manual driving
             auto_ctrl = false;
             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
-            Steer.write((int)((str*500.0)+1500.0));  // Write Steering Pulse
-            Throttle.write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse
+            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
+            }
 
         }/// end  else armed
 
@@ -359,20 +374,21 @@
             if(dt > (1/IMU_RATE)) {
 
                 float tic = t.read();
-                
-                psi_err = wrapToPi(des_psi-psi);
-                spd_err = des_spd - spd;
-                spd_err_i += spd_err;
-                str = Kp_psi*psi_err/(spd+0.05);
-                thr =  Kp_spd*spd_err + Ki_spd*spd_err_i/LOOP_RATE;
-                
+
+
                 x = x + spd*cos(psi)*dt;
                 y = y + spd*sin(psi)*dt;
                 psi = psi + str*dt;
-                spd = spd + thr*dt;
+                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\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();
             }