Push for Students
Dependencies: BNO055_fusion Madpulse ServoIn ServoOut mavlink_emaxx mbed
Fork of Madpulse_Control_Fall2017 by
Revision 1:f4c9926fb4c9, committed 2017-11-15
- Comitter:
- jdawkins
- Date:
- Wed Nov 15 18:36:30 2017 +0000
- Parent:
- 0:daea75c21ac1
- Commit message:
- Committ for class
Changed in this revision
diff -r daea75c21ac1 -r f4c9926fb4c9 Madpulse.lib --- a/Madpulse.lib Tue Nov 14 13:51:29 2017 +0000 +++ b/Madpulse.lib Wed Nov 15 18:36:30 2017 +0000 @@ -1,1 +1,1 @@ -https://os.mbed.com/teams/USNA-WSE-ES456/code/Madpulse/#5e55308aa7a6 +https://os.mbed.com/teams/USNA-WSE-ES456/code/Madpulse/#6bba42f337e6
diff -r daea75c21ac1 -r f4c9926fb4c9 NeoStrip.lib --- a/NeoStrip.lib Tue Nov 14 13:51:29 2017 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,1 +0,0 @@ -https://developer.mbed.org/teams/Emaxx-Navigation-Group/code/NeoStrip/#b2687bc7cafe
diff -r daea75c21ac1 -r f4c9926fb4c9 madpulse_ctrl.cpp --- 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(); }