Diff: madpulse_ctrl.cpp

Revision:

0:daea75c21ac1

Child:

1:f4c9926fb4c9

diff -r 000000000000 -r daea75c21ac1 madpulse_ctrl.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/madpulse_ctrl.cpp	Tue Nov 14 13:51:29 2017 +0000
@@ -0,0 +1,245 @@
+#include "madpulse.h"
+
+#define BUFFER_SIZE 1024
+
+#define FIVE_HZ_LOOP 5.0
+#define LOG_LOOP_FREQ 25.0 // Four messages are taking turns logging, it should be about 25Hz overall
+#define CTRL_LOOP_FREQ 100.0
+#define MAIN_LOOP_FREQ 300.0
+#define LOG_IMU 1
+#define LOG_MAG 2
+#define LOG_EUL 3
+#define LOG_ODO 4
+#define VEH_ID 1
+
+#define PI (3.14159)
+
+
+// Instantiate Serial Ports
+Serial pc(USBTX, USBRX, 115200);           // USB UART Terminal
+Serial xbee(p13,p14,115200);
+
+// Instantiate Class Objects
+BNO055 imu(p9,p10);             // BNO055 Class Object I2C(SDL,SCL)
+ServoIn CH1(p15);               // Read RC Servo Channel 1 (steering) Pin 15
+ServoIn CH2(p16);               // Read RC Servo Channel 2 (Throttle) Pin 16
+InterruptIn wheel_irq(p17);     // Attach Interrupt for Wheel Encoder Pin 17
+ServoOut Steer(p21);            // Steer command object  on Pin 21 goes to steering servo
+ServoOut Throttle(p22);         // Throttle command object on Pin 22 goes to esc
+
+Ticker ctrl_ticker;
+
+// Create Instance of Madpulse class which manages low level vehicle functions and exposes a set of methods for
+// controlling the vehicle
+MadPulse mp(VEH_ID,&xbee,&imu,&CH1,&CH2,&wheel_irq,&Steer,&Throttle);
+
+// Control Gains
+
+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;
+float spd,des_spd;
+float dist;
+
+Timer t;    // Create Timer
+int log_it; // Iterator for Spacing logging of different messages
+
+uint8_t buf[BUFFER_SIZE];
+
+float t_start,t_log,t_ctrl,t_5hz,t_run; // Create timer variables
+float dt;
+float str,thr,wheel_spd;
+float wrapTo2pi(float ang);
+float wrapTopi(float ang);
+bool rc_conn,auto_flag;
+
+void initializeControl(){
+    printf("Initialize Control\r\n");
+    t_run = t.read();
+}
+
+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;
+    } else {
+        rc_conn = true;
+    } // end if(Channels connected)
+    
+    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("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 == 'R' || c == 'r') {                
+                auto_flag = true;
+                initializeControl();                
+            }
+            if(c == 'S' || c == 's') {
+                auto_flag = false;
+            }            
+
+            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()
+{
+
+    log_it = 0;
+    auto_mode = 0;
+// Default Steering and Throttle commands to neutral
+    Steer.write(1500);  // Write Steering Pulse
+    Throttle.write(1500);
+
+
+    t.start();
+    t_start = t.read();
+    t_log = t.read();
+    t_ctrl = t.read();
+    t_5hz = t.read();
+
+    while(1) {
+        auto_LED = auto_flag;
+        dwm_LED = rc_conn;
+        readKeyboard(); // Check for Input from Keyboard over xbee
+
+        // 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();
+            
+        }
+        
+        if(t.read()-t_ctrl > (1.0/CTRL_LOOP_FREQ)) {
+            // 100 Hz loop for calling control loop
+            controlLoop();
+            t_ctrl = t.read();
+        }        
+
+
+        
+        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);
+
+
+
+            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);
+    } // End While 1 Loop
+} // End Main Function
+
+float wrapTo2pi(float ang)
+{
+    if(ang > 2*PI) {
+        ang = ang - 2*PI;
+    }
+
+    if(ang < 0) {
+        ang = ang + 2*PI;
+    }
+    return ang;
+}
+
+float wrapTopi(float ang)
+{
+    if(ang > PI) {
+        ang = ang - 2*PI;
+    }
+
+    if(ang < -PI) {
+        ang = ang + 2*PI;
+    }
+    return ang;
+}
+