Template for ES456 MadPulse Control Lab
Dependencies: BNO055_fusion ServoIn ServoOut mbed
Fork of ES456_Final_Proj_Faculty by
Diff: main.cpp
- Revision:
- 3:ec0efa222dfa
- Parent:
- 0:bd0546063b0a
- Child:
- 4:ec2978ff7a1e
--- a/main.cpp Tue May 13 18:15:06 2014 +0000 +++ b/main.cpp Tue Nov 15 14:33:49 2016 +0000 @@ -1,21 +1,287 @@ //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 "MMA7660.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); -MMA7660 MMA(p28, p27); + 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"); -DigitalOut connectionLed(LED1); -PwmOut Zaxis_p(LED2); -PwmOut Zaxis_n(LED3); + 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 -int main() { - if (MMA.testConnection()) - connectionLed = 1; + 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(); - while(1) { - Zaxis_p = MMA.z(); - Zaxis_n = -MMA.z(); + 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; +} + +