![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Template for ES456 MadPulse Control Lab
Dependencies: BNO055_fusion ServoIn ServoOut mbed
Fork of ES456_Final_Proj_Faculty by
main.cpp
- Committer:
- piper
- Date:
- 2016-11-17
- Revision:
- 4:ec2978ff7a1e
- Parent:
- 3:ec0efa222dfa
- Child:
- 5:03074e90ef7a
File content as of revision 4:ec2978ff7a1e:
// ========================================================================= // ES456 Autonomous Vehicles // Template for MadPulse Vehicle Control // Dawkins, Piper - Nov 2016 // ========================================================================= #define LOG_RATE 25.0 #define LOOP_RATE 200.0 #define CMD_TIMEOUT 1.0 #define WHL_RADIUS 0.7188 // Wheel radius (inches) #define GEAR_RATIO (1/2.75) #define PI 3.14159 #include "mbed.h" #include "BNO055.h" #include "ServoIn.h" #include "ServoOut.h" //I2C i2c(p9, p10); // SDA, SCL BNO055 imu(p9, p10); void he_callback(); float saturateCmd(float cmd); void menuFunction(Serial *port); DigitalOut status_LED(LED1); DigitalOut armed_LED(LED2); DigitalOut auto_LED(LED3); DigitalOut hall_LED(LED4); Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc Serial xbee(p28, p27); // tx, rx for Xbee ServoIn CH1(p15); ServoIn CH2(p16); InterruptIn he_sensor(p23); ServoOut Steer(p22); ServoOut Throttle(p21); Timer t; // create timer instance Ticker log_tick; Ticker heartbeat; float t_imu,t_cmd,str_cmd,thr_cmd,str,thr,dt; float t_hall, dt_hall,t_run,t_stop,t_log_start,t_log,t_loop; float psi_err,spd_err, psi_err_i,spd_err_i, psi_est, psi_est_dot, psi_comp; float L; bool armed, auto_ctrl; float speed; float arm_clock; bool str_cond,thr_cond,run_ctrl,log_data; // User defined variables float distance; float steering_offset; float hdg_comp, hdg_gyro; float hdg_target; float K; // ============================================ // Main Program // ============================================ int main() { pc.baud(115200); xbee.baud(115200); he_sensor.rise(&he_callback); // Set hall effect speed sensor interrupt str_cmd = 0; str=0; thr=0; L =0; thr_cmd = 0; arm_clock =0; psi_est=0; psi_est_dot=0; str_cond = false; thr_cond = false; armed = false; auto_ctrl = false; run_ctrl = false; log_data = false; t.start(); t_log = t.read(); t_log_start = t.read(); t_cmd = 0; status_LED = 1; if(imu.check()) { pc.printf("BNO055 connected\r\n"); xbee.printf("BNO055 connected\r\n"); imu.setmode(OPERATION_MODE_CONFIG); imu.SetExternalCrystal(1); imu.setmode(OPERATION_MODE_NDOF); //Uses magnetometer imu.set_angle_units(DEGREES); imu.set_accel_units(MPERSPERS); imu.set_anglerate_units(DEG_PER_SEC); imu.set_mapping(2); } else { pc.printf("IMU BNO055 NOT connected\r\n Program Trap."); status_LED = 1; armed_LED = 1; hall_LED = 1; auto_LED = 1; while(1) { status_LED = !status_LED; armed_LED = !armed_LED; hall_LED = !hall_LED; auto_LED = !auto_LED; wait(0.5); } } // imu.check // Initialize user defined variables distance = 0; // Initialize distance hdg_gyro = 0; // Initialize gyro heading steering_offset = -.12; K = 0.1; // Heading control gain // ============================================ // Control Loop // ============================================ while(1) { // ******************************************* // Check failsafe conditions // ******************************************* // Arm car when RC Reciever is connected if(CH1.servoPulse == 0 || CH2.servoPulse ==0) { armed = false; } else { armed = true; } // ------------------------------------------- // Enable auto control when throttle is zero str_cond = (CH1.servoPulse > 1800); // If steering is full right thr_cond = abs(CH2.servoPulse-1500)<50; // If throttle is near zero if(str_cond&thr_cond) { // Both of the above conditions must be met if(t.read()-arm_clock > 5) { // If conditions have been met for 5 seconds Steer.write((int)(1500.0)); auto_ctrl = true; //Active auto control loop } } else { arm_clock = t.read(); } // ------------------------------------------ // Check PC terminal for run command //menuFunction(&pc); menuFunction(&xbee); // ******************************************* // Get Sensor Data // ******************************************* // Compute speed from hall effect sensor if(t.read()-t_hall < 0.2) { //speed = 0.036*GEAR_RATIO*(2*PI)/(dt_hall); // meters/sec ? speed = WHL_RADIUS*GEAR_RATIO*(2*PI)/(dt_hall); // inches/sec } else { speed = 0; } // ------------------------------------------ // IMU data //imu.get_angles(); imu.get_accel(); imu.get_gyro(); imu.get_mag(); // ******************************************* // Begin Control // ******************************************* if(armed) { // Is System Armed armed_LED = 1; if(auto_ctrl) { // Armed and Auto Control enabled auto_LED = 1; // Turn on LED to indicate Auto Control if(run_ctrl) { // Armed, in Auto Control enabled, and Run command recieved float run_time = t.read()-t_run; //Integrate speed to determine distance (inches) distance = distance + speed *dt; // Heading via magnetometer (deg) hdg_comp = (180/PI)*atan2(-imu.mag.y,imu.mag.x); // Heading via integrated gyro hdg_gyro = hdg_gyro + imu.gyro.z * dt; if(distance <= 60 ) { thr_cmd = 0.3; //str_cmd = 0; hdg_target = 0; str_cmd = K * (hdg_target - hdg_gyro); } else if(distance > 60 && distance <= 120) { thr_cmd = 0.3; hdg_target = 90; str_cmd = K * (hdg_target - hdg_gyro); } else { // Run completed thr_cmd = 0; str_cmd = 0; distance = 0; // Reset distance for next run run_ctrl = false; // Turn off run control log_data = false; } // Compensate for steering offset str_cmd = str_cmd + steering_offset; } // End if run_ctrl // Write steering and throttle commands to vehicle Steer.write((int)((str_cmd*500.0)+1500.0)); Throttle.write((int)((thr_cmd*500.0)+1500.0)); if(!thr_cond) { // If the throttle is moved away from neutral switch to manual control auto_ctrl = false; run_ctrl = false; } } // End if auto_ctrl else { // Armed but in Manual Control if(xbee.readable()) { t_run = t.read(); log_data = !log_data; } str_cmd = ((CH1.servoPulse-1500.0)/500.0); // Convert Pulse to Normalized Command +/- 1 thr_cmd = ((CH2.servoPulse-1500.0)/500.0); // Convert Pulse to Normalized Command +/- 1 Steer.write((int)((str_cmd*500.0)+1500.0)); // Write Steering Pulse Throttle.write((int)((thr_cmd*500.0)+1500.0)); //Write Throttle Pulse auto_LED = 0; } // end if autocontrol } else { // System is not aArmed armed_LED = 0; //Turn off armed LED indicator str_cmd = 0; thr_cmd = 0; Steer.write(1500); //Set Steer PWM to center 1000-2000 range Throttle.write(1500); //Set Throttle to Low }/// end else armed psi_comp = (180/PI)*atan2(-imu.mag.y,imu.mag.x); psi_est_dot = imu.gyro.z + L*(psi_comp - psi_est); psi_est = psi_est + psi_est_dot*dt; // *************************************** // Transmit data to ground station // *************************************** if(t.read()-t_log > (1/LOG_RATE)) { if(log_data) { /* Data Output Options imu.accel.x imu.accel.y imu.accel.z imu.gyro.x imu.gyro.y imu.gyro.z wheel_spd thr str*/ xbee.printf("%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n",t.read()-t_run,speed, distance, thr_cmd, str_cmd, imu.gyro.z, hdg_gyro); // xbee.printf("%.3f, %.3f, %.3f, %.3f, %.3f\r\n",t.read()-t_run,speed, thr_cmd, str_cmd, imu.gyro.z); // xbee.printf("$MAG,%.1f, %.1f, %.1f\r\n",imu.mag.x,imu.mag.y,imu.mag.z); // xbee.printf("$ODO,%.3f, %.3f, %.3f\r\n",wheel_spd,thr,str); } t_log = t.read(); } // End transmit data dt = t.read()-t_loop; // Sample period (sec) t_loop = t.read(); wait(1/LOOP_RATE); status_LED=!status_LED; } // End control loop } // End main void he_callback() { // Hall effect speed sensor callback hall_LED = !hall_LED; dt_hall = t.read()-t_hall; t_hall = t.read(); } void menuFunction(Serial *port){ if(port->readable()) { char c = port->getc(); if(c == 'H' || c == 'h') { port->printf("Command List...\r\n"); port->printf("a - set autonomous mode\r\n"); port->printf("l - toggle data logging\r\n"); port->printf("r - run auto control loop\r\n"); port->printf("h - display this prompt and exit\r\n"); port->printf("Enter Command\r\n"); } if(c == 'A' || c == 'a') { auto_ctrl = true; Steer.write((int)(1500.0)); } if(c == 'R' || c == 'r') { auto_ctrl = true; Steer.write((int)(1500.0)); if(!log_data) { t_log_start = t.read(); t_run = t.read(); xbee.printf("\r\n\r\n"); } log_data = !log_data; run_ctrl = true; } } } float saturateCmd(float cmd) { if(cmd>1.0) { cmd = 1.0; } if(cmd < -1.0) { cmd = -1.0; } return cmd; } float saturateCmd(float cmd, float max,float min) { if(cmd>max) { cmd = max; } if(cmd < min) { cmd = min; } return cmd; }