Diff: madpulse_ctrl.cpp

Revision:

1:f4c9926fb4c9

Parent:

0:daea75c21ac1

Child:

2:2bb375ab3b2b

--- a/madpulse_ctrl.cpp	Tue Nov 14 13:51:29 2017 +0000
+++ b/madpulse_ctrl.cpp	Wed Nov 15 18:36:30 2017 +0000
@@ -33,14 +33,10 @@
 // controlling the vehicle
 MadPulse mp(VEH_ID,&xbee,&imu,&CH1,&CH2,&wheel_irq,&Steer,&Throttle);
 
-// Control Gains
+///////////////////////////////////////////////
+// Define Control Gains and Controller Variable
+//////////////////////////////////////////////
 
-float Kp_spd = 0.06;
-float Ki_spd = 0.05;
-
-float Kp_psi = 0.65;
-float Kd_psi = 0.1;
-float L = 0.01;
 
 float psi_est,psi_est_old;
 float spd_err,spd_err_i;
@@ -57,7 +53,42 @@
 float str,thr,wheel_spd;
 float wrapTo2pi(float ang);
 float wrapTopi(float ang);
-bool rc_conn,auto_flag;
+bool rc_conn,auto_flag,log_flag;
+
+
+void controlLoop(){
+    dt = t.read()-t_ctrl;
+
+    if(auto_flag){   // If vehicle is in Auto Mode
+    
+        spd = mp.getFreq()/27.8119; // Convert from counts per second to meters/second
+
+        str = 0.001;
+        if(t.read()-t_run < 1){            
+
+            thr = 0.1; // Set throttle, range (-1 to 1)
+        }else{
+            thr = 0.0;
+        }
+    }else{
+        str = ((CH1.servoPulse-1500.0)/600.0);  // Convert Pulse to Normalized Command +/- 1
+        thr = ((CH2.servoPulse-1500.0)/600.0);  // Convert Pulse to Normalized Command +/- 1            
+    }
+
+    t_ctrl = t.read();
+}
+
+void logData(){
+// Accel Data: mp.accel.x,mp.accel.y,mp.accel.z
+// Gyro Data: mp.gyro.x,mp.gyro.y,mp.gyro.z
+// Mag Data: mp.mag.x,mp.mag.y,mp.mag.z
+// Euler Data: mp.euler.roll,mp.euler.pitch, mp.euler.yaw
+// Encoder Data: mp.getCounts(), mp.getFreq()
+
+
+    xbee.printf("%.3f,%d,%.3f,%.3f,%.3f\r\n",t.read()-t_run,mp.getCounts(),spd,thr,str);
+}
+
 
 void initializeControl(){
     printf("Initialize Control\r\n");
@@ -67,6 +98,7 @@
 void stopControl(){
     printf("Stop Control\r\n");    
 }
+
 void readRCSignal(){
     if(CH1.servoPulse == 0 || CH2.servoPulse == 0) { //RC Reciever must be connected For Car to be armed
         rc_conn = false;
@@ -83,55 +115,31 @@
             char c = xbee.getc();
             if(c == 'H' || c == 'h') {
                 xbee.printf("Command List...\r\n");
-                xbee.printf("d - set open loop duty cycle\r\n");
-                xbee.printf("r - run open loop control with current duty cycle\r\n");
-                xbee.printf("h - display this prompt and exit\r\n");
-                xbee.printf("t - set duration of test\r\n\n\n");
+                xbee.printf("h - Display this prompt and return\r\n");
+                xbee.printf("l - Toggle Logging\r\n");
+                xbee.printf("r - Run, switch vehicle to auto mode\r\n");
+                xbee.printf("s - Stop, stop vehicle autor control\r\n");
                 xbee.printf("Enter Command\r\n");
             }
 
+            if(c == 'L' || c == 'l') {                
+                log_flag = !log_flag; //toggle logging flag
+              
+            }
             if(c == 'R' || c == 'r') {                
                 auto_flag = true;
+                log_flag = true;
                 initializeControl();                
             }
             if(c == 'S' || c == 's') {
                 auto_flag = false;
-            }            
+                log_flag = false;
+                stopControl();
+            }                                    
 
-            if(c == 'T' || c == 't') {
-
-            }
     }
 }
-void controlLoop(){
-    dt = t.read()-t_ctrl;
 
-    if(auto_flag){   // If vehicle is in Auto Mode
-    
-        float des_psi = 0;
-        spd = mp.getFreq()/27.8119;
-        //str = -Kp_psi*wrapTopi(des_psi-wrapTopi(mp.euler.yaw));
-        str = 0.001;
-        if(t.read()-t_run < 10){            
-            des_spd = 1.5;
-            spd_err = des_spd-spd;
-            spd_err_i += spd_err*dt;
-           // spd_err_d = (spd - spd_old)/dt;
-            
-            
-           thr = Kp_spd*spd_err + Ki_spd*spd_err_i + 0.04;
-            
-           // thr = 0.04;
-        }else{
-            thr = 0.0;
-        }
-    }else{
-        str = ((CH1.servoPulse-1500.0)/600.0);  // Convert Pulse to Normalized Command +/- 1
-        thr = ((CH2.servoPulse-1500.0)/600.0);  // Convert Pulse to Normalized Command +/- 1            
-    }
-
-    t_ctrl = t.read();
-}
 
 
 int main()
@@ -158,20 +166,19 @@
         // check for servo pulse from either channel of receiver module
          readRCSignal();       
 
-
-
-
-
         if(t.read()-t_5hz > (1.0/FIVE_HZ_LOOP)) {
             // 5 Hz loop intended for debugging print statements
 
-           // xbee.printf("Auto Cond: %f , %f \r\n",abs(CH1.servoPulse-1500),abs(CH2.servoPulse-1500));
-            t_5hz = t.read();
-            
+            t_5hz = t.read();           
         }
         
         if(t.read()-t_ctrl > (1.0/CTRL_LOOP_FREQ)) {
             // 100 Hz loop for calling control loop
+            mp.getAccel();
+            mp.getGyro();
+            mp.getMag();  
+            mp.getEuler(); 
+            mp.getSpeed();                                 
             controlLoop();
             t_ctrl = t.read();
         }        
@@ -180,25 +187,9 @@
         
         if(t.read()-t_log > (1.0/LOG_LOOP_FREQ)) {
                 // Faster loop for logging data for plotting and analysis
-
-                    mp.getAccel();
-                    mp.getGyro();
-                   // xbee.printf("$IMU,%.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n", mp.accel.x,mp.accel.y,mp.accel.z,mp.gyro.x,mp.gyro.y,mp.gyro.z);
-
-
-                    mp.getMag();
-                    //xbee.printf("$MAG,%.1f, %.1f, %.1f\r\n",mp.mag.x,mp.mag.y,mp.mag.z);
-
-
-                    mp.getEuler();
-                   // xbee.printf("$RPY,%.3f, %.3f, %.3f\r\n", mp.euler.roll,mp.euler.pitch,wrapTo2pi(mp.euler.yaw));
-
-
-                    mp.getSpeed();
-                    xbee.printf("%.3f,%d,%.3f,%.3f,%.3f\r\n",t.read()-t_run,mp.getCounts(),spd,thr,str);
-
-
-
+            if(log_flag){
+                logData();
+            }
             t_log = t.read();
         }