Template for ES456 MadPulse Control Lab
Dependencies: BNO055_fusion ServoIn ServoOut mbed
Fork of ES456_Final_Proj_Faculty by
main.cpp
- Committer:
- jdawkins
- Date:
- 2016-11-15
- Revision:
- 3:ec0efa222dfa
- Parent:
- 0:bd0546063b0a
- Child:
- 4:ec2978ff7a1e
File content as of revision 3:ec0efa222dfa:
//Uses the measured z-acceleration to drive leds 2 and 3 of the mbed #define LOG_RATE 25.0 #define LOOP_RATE 200.0 #define CMD_TIMEOUT 1.0 #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); int left; 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; void he_callback() { hall_LED = !hall_LED; dt_hall = t.read()-t_hall; t_hall = t.read(); } int main() { pc.baud(115200); xbee.baud(115200); he_sensor.rise(&he_callback); left = 0; 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); } } pc.printf("ES456 Vehcile Program\r\n"); while(1) { //menuFunction(&pc); menuFunction(&xbee); if(CH1.servoPulse == 0 || CH2.servoPulse ==0) { //RC Reciever must be connected For Car to be armed armed = false; } else { armed = true; } str_cond = (CH1.servoPulse > 1800); // If steering is full right thr_cond = abs(CH2.servoPulse-1500)<50; // If throttle is near zero //pc.printf("Cond 1: %d Cond 2: %d\r\n",str_cond,thr_cond); // pc.printf("Timeer: %f \r\n",t.read()-arm_clock); 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(); } if(armed) { // Is System Armed armed_LED = 1; if(auto_ctrl) { // Armed and in Auto Control auto_LED = 1; // Turn on LED to indicate Auto Control if(run_ctrl) { float run_time = t.read()-t_run; if(run_time > 0 && run_time <= 1) { thr_cmd = 0.2; str_cmd = 0.0; } else if(run_time >1 && run_time< 3) { thr_cmd = 0.2; str_cmd = -1.0; } else { thr_cmd = 0; str_cmd = 0; log_data = false; } } // End if run_ctrl 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 { 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 if(t.read()-t_hall < 0.2) { speed = 0.036*GEAR_RATIO*(2*PI)/(dt_hall); } else { speed = 0; } // Read Angles imu.get_angles(); imu.get_accel(); imu.get_gyro(); imu.get_mag(); dt = t.read()-t_loop; t_loop = t.read(); 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; 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\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(); } wait(1/LOOP_RATE); status_LED=!status_LED; } } 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; }