Push for Students
Dependencies: BNO055_fusion Madpulse ServoIn ServoOut mavlink_emaxx mbed
Fork of Madpulse_Control_Fall2017 by
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; +} +