Template for ES456 MadPulse Control Lab
Dependencies: BNO055_fusion ServoIn ServoOut mbed
Fork of ES456_Final_Proj_Faculty by
Diff: main.cpp
- Revision:
- 5:03074e90ef7a
- Parent:
- 4:ec2978ff7a1e
- Child:
- 6:d133964667f3
diff -r ec2978ff7a1e -r 03074e90ef7a main.cpp --- a/main.cpp Thu Nov 17 19:41:38 2016 +0000 +++ b/main.cpp Sun Nov 20 19:29:37 2016 +0000 @@ -16,11 +16,9 @@ #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); @@ -38,22 +36,19 @@ Timer t; // create timer instance Ticker log_tick; Ticker heartbeat; -float t_imu,t_cmd,str_cmd,thr_cmd,str,thr,dt; +float t_imu,t_cmd,str_cmd,thr_cmd,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 +// USER DEFINED VARIABLES float distance; float steering_offset; float hdg_comp, hdg_gyro; -float hdg_target; -float K; + // ============================================ // Main Program @@ -67,13 +62,8 @@ 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; + arm_clock =0; str_cond = false; thr_cond = false; armed = false; @@ -115,12 +105,12 @@ } // imu.check - // Initialize user defined variables + // INITIALIZE USER DEFINED VARIABLES distance = 0; // Initialize distance - hdg_gyro = 0; // Initialize gyro heading - steering_offset = -.12; - K = 0.1; // Heading control gain - + steering_offset = 0; // Steering command offset + hdg_gyro = 0; // Initialize integrated gyro heading + hdg_est = 0; // Initialize heading estimate + // ============================================ // Control Loop @@ -158,17 +148,15 @@ // ******************************************* // 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(); + imu.get_accel(); // imu.accel.x, imu.accel.y, imu.accel.z + imu.get_gyro(); // imu.gyro.x, imu.gyro.y, imu.gyro.z + imu.get_mag(); // imu.mag.x, imu.mag.y, imu.mag.z // ******************************************* @@ -188,21 +176,25 @@ //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); + // ***COMPUTE HEADING HERE*** + // Compute Heading via magnetometer (deg) + //hdg_comp = ... ; // 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); + //hdg_gyro = ... ; + + // Heading estimate + //hdg_est = ... ; + + + // ***COMPUTE CONTROL LAW HERE *** + // For now it is just a step input steering angle + if(distance <= 72 ) { + thr_cmd = 0.2; + str_cmd = 0.0; + } else if(distance > 72 && distance <= 150) { + thr_cmd = 0.2; + str_cmd = 0.5; } else { // Run completed thr_cmd = 0; str_cmd = 0; @@ -210,7 +202,7 @@ run_ctrl = false; // Turn off run control log_data = false; } - + // Compensate for steering offset str_cmd = str_cmd + steering_offset; @@ -249,26 +241,12 @@ }/// 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); + xbee.printf("%.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n",t.read()-t_run,speed, distance, thr_cmd, str_cmd, imu.gyro.z); } t_log = t.read(); } // End transmit data @@ -293,6 +271,7 @@ t_hall = t.read(); } + void menuFunction(Serial *port){ if(port->readable()) { char c = port->getc(); @@ -326,27 +305,6 @@ } -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; -} -