Push for Students

Dependencies:   BNO055_fusion Madpulse ServoIn ServoOut mavlink_emaxx mbed

Fork of Madpulse_Speed_Control_temp by USNA WSE ES456

Files at this revision

API Documentation at this revision

Comitter:
jdawkins
Date:
Wed Nov 15 21:51:31 2017 +0000
Parent:
1:f4c9926fb4c9
Commit message:
Initial Template for Project Part 1

Changed in this revision

madpulse_ctrl.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r f4c9926fb4c9 -r 2bb375ab3b2b madpulse_ctrl.cpp
--- a/madpulse_ctrl.cpp	Wed Nov 15 18:36:30 2017 +0000
+++ b/madpulse_ctrl.cpp	Wed Nov 15 21:51:31 2017 +0000
@@ -33,10 +33,14 @@
 // controlling the vehicle
 MadPulse mp(VEH_ID,&xbee,&imu,&CH1,&CH2,&wheel_irq,&Steer,&Throttle);
 
-///////////////////////////////////////////////
-// Define Control Gains and Controller Variable
-//////////////////////////////////////////////
+// Control Gains
 
+float Kp_spd = 0.0;
+float Ki_spd = 0.0;
+
+float Kp_psi = 0.0;
+float Kd_psi = 0.0;
+float L = 0.01;
 
 float psi_est,psi_est_old;
 float spd_err,spd_err_i;
@@ -56,21 +60,32 @@
 bool rc_conn,auto_flag,log_flag;
 
 
+// ============================================
+// Control Loop
+// ============================================
+// Heading & Speed Control Function
+// Sampled at 100 HZ
 void controlLoop(){
-    dt = t.read()-t_ctrl;
+    dt = t.read()-t_ctrl;     // Sample time (sec)
 
-    if(auto_flag){   // If vehicle is in Auto Mode
+    if(auto_flag){
+    // 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)
+        spd = mp.getFreq()*0.0436;  // Read odmeter  (meters/sec)
+        
+        // Steering Command (+/- 1)
+        str = 0.0;  // Set steering trim
+        
+        // Throttle Command (+/- 1)
+        if(t.read()-t_run < 1){      // Set throttle value for 1 secs      
+            thr = 0.05;          
         }else{
             thr = 0.0;
         }
+        // Use manual steering for speed control testing
+        str = ((CH1.servoPulse-1500.0)/600.0);  // Convert Pulse to Normalized Command +/- 1        
     }else{
+    // Maunal RC Mode
         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            
     }
@@ -78,27 +93,26 @@
     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);
+// ============================================
+// Data Log
+// ============================================
+// Print data to Xbee serial port (115200 Baud)
+void dataLog(){
+     xbee.printf("%.3f,%d,%.3f,%.3f,%.3f,%.3f\r\n",t.read()-t_run,mp.getCounts(),dist,spd,thr,str);   
 }
 
 
 void initializeControl(){
     printf("Initialize Control\r\n");
     t_run = t.read();
+    dist = 0.0;
+    spd_err_i = 0;
 }
 
 void stopControl(){
-    printf("Stop Control\r\n");    
+    printf("Stop Control\r\n"); 
+    spd_err_i = 0;   
 }
-
 void readRCSignal(){
     if(CH1.servoPulse == 0 || CH2.servoPulse == 0) { //RC Reciever must be connected For Car to be armed
         rc_conn = false;
@@ -106,26 +120,22 @@
         rc_conn = true;
     } // end if(Channels connected)
     
-    if( (abs(CH1.servoPulse-1500) > 200) || (abs(CH2.servoPulse-1500) > 200)){
+   /* if( (abs(CH1.servoPulse-1500) > 200) || (abs(CH2.servoPulse-1500) > 200)){
         auto_flag = 0;        
-    }
+    }*/
 }
 void readKeyboard(){
     if(xbee.readable()) {
             char c = xbee.getc();
             if(c == 'H' || c == 'h') {
                 xbee.printf("Command List...\r\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("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("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;
@@ -134,9 +144,11 @@
             if(c == 'S' || c == 's') {
                 auto_flag = false;
                 log_flag = false;
-                stopControl();
-            }                                    
+            }            
 
+            if(c == 'L' || c == 'l') {
+                log_flag = !log_flag;
+            }
     }
 }
 
@@ -166,19 +178,20 @@
         // 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
 
-            t_5hz = t.read();           
+           // xbee.printf("Auto Cond: %f , %f \r\n",abs(CH1.servoPulse-1500),abs(CH2.servoPulse-1500));
+            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();
         }        
@@ -187,23 +200,27 @@
         
         if(t.read()-t_log > (1.0/LOG_LOOP_FREQ)) {
                 // Faster loop for logging data for plotting and analysis
-            if(log_flag){
-                logData();
-            }
+
+                    mp.getAccel();
+                    mp.getGyro();
+                    mp.getMag();
+                    mp.getEuler();
+                    mp.getSpeed();
+                    //xbee.printf("%.3f,%d,%.3f,%.3f,%.3f\r\n",t.read()-t_run,mp.getCounts(),spd,thr,str);
+
+                    if(log_flag){
+                    dataLog();
+                    }
             t_log = t.read();
         }     
         
         if(rc_conn) {
-          //  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
             mp.setSteer(str);
             mp.setThrottle(thr);
     
         } else {
         Steer.write(1500);  // Write Steering Pulse
         Throttle.write(1500);        
-          //  mp.setSteer(str);
-          //  mp.setThrottle(thr);
         }           
 
         wait(1.0/MAIN_LOOP_FREQ);
@@ -232,5 +249,4 @@
         ang = ang + 2*PI;
     }
     return ang;
-}
-
+}
\ No newline at end of file