USNA WSE ES456 / Mbed 2 deprecated MadPulseCntrl

Template for ES456 MadPulse Control Lab

Dependencies:   BNO055_fusion ServoIn ServoOut mbed

Fork of ES456_Final_Proj_Faculty by USNA WSE ES456

main.cpp@3:ec0efa222dfa, 2016-11-15 (annotated)

Committer:
jdawkins
Date:
Tue Nov 15 14:33:49 2016 +0000
Revision:
3:ec0efa222dfa
Parent:
0:bd0546063b0a
Child:
4:ec2978ff7a1e
Development code for Final Project

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Sissors 0:bd0546063b0a 1 //Uses the measured z-acceleration to drive leds 2 and 3 of the mbed
Sissors 0:bd0546063b0a 2
jdawkins 3:ec0efa222dfa 3 #define LOG_RATE 25.0
jdawkins 3:ec0efa222dfa 4 #define LOOP_RATE 200.0
jdawkins 3:ec0efa222dfa 5 #define CMD_TIMEOUT 1.0
jdawkins 3:ec0efa222dfa 6 #define GEAR_RATIO (1/2.75)
jdawkins 3:ec0efa222dfa 7 #define PI 3.14159
Sissors 0:bd0546063b0a 8 #include "mbed.h"
jdawkins 3:ec0efa222dfa 9
jdawkins 3:ec0efa222dfa 10 #include "BNO055.h"
jdawkins 3:ec0efa222dfa 11 #include "ServoIn.h"
jdawkins 3:ec0efa222dfa 12 #include "ServoOut.h"
jdawkins 3:ec0efa222dfa 13
jdawkins 3:ec0efa222dfa 14
jdawkins 3:ec0efa222dfa 15 //I2C i2c(p9, p10); // SDA, SCL
jdawkins 3:ec0efa222dfa 16 BNO055 imu(p9, p10);
jdawkins 3:ec0efa222dfa 17
jdawkins 3:ec0efa222dfa 18
jdawkins 3:ec0efa222dfa 19 int left;
jdawkins 3:ec0efa222dfa 20 float saturateCmd(float cmd);
jdawkins 3:ec0efa222dfa 21 void menuFunction(Serial *port);
jdawkins 3:ec0efa222dfa 22 DigitalOut status_LED(LED1);
jdawkins 3:ec0efa222dfa 23 DigitalOut armed_LED(LED2);
jdawkins 3:ec0efa222dfa 24 DigitalOut auto_LED(LED3);
jdawkins 3:ec0efa222dfa 25 DigitalOut hall_LED(LED4);
jdawkins 3:ec0efa222dfa 26
jdawkins 3:ec0efa222dfa 27 Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc
jdawkins 3:ec0efa222dfa 28 Serial xbee(p28, p27); // tx, rx for Xbee
jdawkins 3:ec0efa222dfa 29 ServoIn CH1(p15);
jdawkins 3:ec0efa222dfa 30 ServoIn CH2(p16);
jdawkins 3:ec0efa222dfa 31
jdawkins 3:ec0efa222dfa 32 InterruptIn he_sensor(p23);
jdawkins 3:ec0efa222dfa 33 ServoOut Steer(p22);
jdawkins 3:ec0efa222dfa 34 ServoOut Throttle(p21);
jdawkins 3:ec0efa222dfa 35 Timer t; // create timer instance
jdawkins 3:ec0efa222dfa 36 Ticker log_tick;
jdawkins 3:ec0efa222dfa 37 Ticker heartbeat;
jdawkins 3:ec0efa222dfa 38 float t_imu,t_cmd,str_cmd,thr_cmd,str,thr,dt;
jdawkins 3:ec0efa222dfa 39 float t_hall, dt_hall,t_run,t_stop,t_log_start,t_log,t_loop;
jdawkins 3:ec0efa222dfa 40 float psi_err,spd_err, psi_err_i,spd_err_i, psi_est, psi_est_dot, psi_comp;
jdawkins 3:ec0efa222dfa 41 float L;
jdawkins 3:ec0efa222dfa 42
jdawkins 3:ec0efa222dfa 43 bool armed, auto_ctrl;
jdawkins 3:ec0efa222dfa 44 float speed;
jdawkins 3:ec0efa222dfa 45 float arm_clock;
jdawkins 3:ec0efa222dfa 46 bool str_cond,thr_cond,run_ctrl,log_data;
jdawkins 3:ec0efa222dfa 47
jdawkins 3:ec0efa222dfa 48 void he_callback()
jdawkins 3:ec0efa222dfa 49 {
jdawkins 3:ec0efa222dfa 50 hall_LED = !hall_LED;
jdawkins 3:ec0efa222dfa 51
jdawkins 3:ec0efa222dfa 52 dt_hall = t.read()-t_hall;
jdawkins 3:ec0efa222dfa 53 t_hall = t.read();
jdawkins 3:ec0efa222dfa 54 }
jdawkins 3:ec0efa222dfa 55
jdawkins 3:ec0efa222dfa 56 int main()
jdawkins 3:ec0efa222dfa 57 {
jdawkins 3:ec0efa222dfa 58
jdawkins 3:ec0efa222dfa 59 pc.baud(115200);
jdawkins 3:ec0efa222dfa 60 xbee.baud(115200);
Sissors 0:bd0546063b0a 61
jdawkins 3:ec0efa222dfa 62 he_sensor.rise(&he_callback);
jdawkins 3:ec0efa222dfa 63
jdawkins 3:ec0efa222dfa 64 left = 0;
jdawkins 3:ec0efa222dfa 65 str_cmd = 0;
jdawkins 3:ec0efa222dfa 66 str=0;
jdawkins 3:ec0efa222dfa 67 thr=0;
jdawkins 3:ec0efa222dfa 68 L =0;
jdawkins 3:ec0efa222dfa 69 thr_cmd = 0;
jdawkins 3:ec0efa222dfa 70 arm_clock =0;
jdawkins 3:ec0efa222dfa 71 psi_est=0;
jdawkins 3:ec0efa222dfa 72 psi_est_dot=0;
jdawkins 3:ec0efa222dfa 73 str_cond = false;
jdawkins 3:ec0efa222dfa 74 thr_cond = false;
jdawkins 3:ec0efa222dfa 75 armed = false;
jdawkins 3:ec0efa222dfa 76 auto_ctrl = false;
jdawkins 3:ec0efa222dfa 77 run_ctrl = false;
jdawkins 3:ec0efa222dfa 78 log_data = false;
jdawkins 3:ec0efa222dfa 79
jdawkins 3:ec0efa222dfa 80 t.start();
jdawkins 3:ec0efa222dfa 81 t_log = t.read();
jdawkins 3:ec0efa222dfa 82 t_log_start = t.read();
jdawkins 3:ec0efa222dfa 83 t_cmd = 0;
jdawkins 3:ec0efa222dfa 84
jdawkins 3:ec0efa222dfa 85 status_LED = 1;
jdawkins 3:ec0efa222dfa 86
jdawkins 3:ec0efa222dfa 87 if(imu.check()) {
jdawkins 3:ec0efa222dfa 88 pc.printf("BNO055 connected\r\n");
jdawkins 3:ec0efa222dfa 89 xbee.printf("BNO055 connected\r\n");
jdawkins 3:ec0efa222dfa 90 imu.setmode(OPERATION_MODE_CONFIG);
jdawkins 3:ec0efa222dfa 91 imu.SetExternalCrystal(1);
jdawkins 3:ec0efa222dfa 92 imu.setmode(OPERATION_MODE_NDOF); //Uses magnetometer
jdawkins 3:ec0efa222dfa 93 imu.set_angle_units(DEGREES);
jdawkins 3:ec0efa222dfa 94 imu.set_accel_units(MPERSPERS);
jdawkins 3:ec0efa222dfa 95 imu.set_anglerate_units(DEG_PER_SEC);
jdawkins 3:ec0efa222dfa 96 imu.set_mapping(2);
jdawkins 3:ec0efa222dfa 97
jdawkins 3:ec0efa222dfa 98 } else {
jdawkins 3:ec0efa222dfa 99 pc.printf("IMU BNO055 NOT connected\r\n Program Trap.");
jdawkins 3:ec0efa222dfa 100 status_LED = 1;
jdawkins 3:ec0efa222dfa 101 armed_LED = 1;
jdawkins 3:ec0efa222dfa 102 hall_LED = 1;
jdawkins 3:ec0efa222dfa 103 auto_LED = 1;
jdawkins 3:ec0efa222dfa 104 while(1) {
jdawkins 3:ec0efa222dfa 105 status_LED = !status_LED;
jdawkins 3:ec0efa222dfa 106 armed_LED = !armed_LED;
jdawkins 3:ec0efa222dfa 107 hall_LED = !hall_LED;
jdawkins 3:ec0efa222dfa 108 auto_LED = !auto_LED;
jdawkins 3:ec0efa222dfa 109 wait(0.5);
jdawkins 3:ec0efa222dfa 110 }
jdawkins 3:ec0efa222dfa 111 }
jdawkins 3:ec0efa222dfa 112
jdawkins 3:ec0efa222dfa 113 pc.printf("ES456 Vehcile Program\r\n");
Sissors 0:bd0546063b0a 114
jdawkins 3:ec0efa222dfa 115 while(1) {
jdawkins 3:ec0efa222dfa 116
jdawkins 3:ec0efa222dfa 117 //menuFunction(&pc);
jdawkins 3:ec0efa222dfa 118 menuFunction(&xbee);
jdawkins 3:ec0efa222dfa 119
jdawkins 3:ec0efa222dfa 120
jdawkins 3:ec0efa222dfa 121 if(CH1.servoPulse == 0 || CH2.servoPulse ==0) { //RC Reciever must be connected For Car to be armed
jdawkins 3:ec0efa222dfa 122 armed = false;
jdawkins 3:ec0efa222dfa 123 } else {
jdawkins 3:ec0efa222dfa 124 armed = true;
jdawkins 3:ec0efa222dfa 125 }
jdawkins 3:ec0efa222dfa 126
jdawkins 3:ec0efa222dfa 127 str_cond = (CH1.servoPulse > 1800); // If steering is full right
jdawkins 3:ec0efa222dfa 128 thr_cond = abs(CH2.servoPulse-1500)<50; // If throttle is near zero
jdawkins 3:ec0efa222dfa 129 //pc.printf("Cond 1: %d Cond 2: %d\r\n",str_cond,thr_cond);
jdawkins 3:ec0efa222dfa 130
jdawkins 3:ec0efa222dfa 131 // pc.printf("Timeer: %f \r\n",t.read()-arm_clock);
jdawkins 3:ec0efa222dfa 132 if(str_cond&thr_cond) { // Both of the above conditions must be met
jdawkins 3:ec0efa222dfa 133 if(t.read()-arm_clock > 5) { // If conditions have been met for 5 seconds
jdawkins 3:ec0efa222dfa 134 Steer.write((int)(1500.0));
jdawkins 3:ec0efa222dfa 135 auto_ctrl = true; //Active auto control loop
jdawkins 3:ec0efa222dfa 136 }
jdawkins 3:ec0efa222dfa 137 } else {
jdawkins 3:ec0efa222dfa 138 arm_clock = t.read();
jdawkins 3:ec0efa222dfa 139 }
jdawkins 3:ec0efa222dfa 140
jdawkins 3:ec0efa222dfa 141 if(armed) { // Is System Armed
jdawkins 3:ec0efa222dfa 142 armed_LED = 1;
jdawkins 3:ec0efa222dfa 143
jdawkins 3:ec0efa222dfa 144 if(auto_ctrl) { // Armed and in Auto Control
jdawkins 3:ec0efa222dfa 145 auto_LED = 1; // Turn on LED to indicate Auto Control
jdawkins 3:ec0efa222dfa 146
jdawkins 3:ec0efa222dfa 147 if(run_ctrl) {
jdawkins 3:ec0efa222dfa 148
jdawkins 3:ec0efa222dfa 149 float run_time = t.read()-t_run;
jdawkins 3:ec0efa222dfa 150 if(run_time > 0 && run_time <= 1) {
jdawkins 3:ec0efa222dfa 151 thr_cmd = 0.2;
jdawkins 3:ec0efa222dfa 152 str_cmd = 0.0;
jdawkins 3:ec0efa222dfa 153 } else if(run_time >1 && run_time< 3) {
jdawkins 3:ec0efa222dfa 154 thr_cmd = 0.2;
jdawkins 3:ec0efa222dfa 155 str_cmd = -1.0;
jdawkins 3:ec0efa222dfa 156 } else {
jdawkins 3:ec0efa222dfa 157 thr_cmd = 0;
jdawkins 3:ec0efa222dfa 158 str_cmd = 0;
jdawkins 3:ec0efa222dfa 159 log_data = false;
jdawkins 3:ec0efa222dfa 160 }
jdawkins 3:ec0efa222dfa 161
jdawkins 3:ec0efa222dfa 162 } // End if run_ctrl
jdawkins 3:ec0efa222dfa 163
jdawkins 3:ec0efa222dfa 164 Steer.write((int)((str_cmd*500.0)+1500.0));
jdawkins 3:ec0efa222dfa 165 Throttle.write((int)((thr_cmd*500.0)+1500.0));
jdawkins 3:ec0efa222dfa 166
jdawkins 3:ec0efa222dfa 167 if(!thr_cond) { // If the throttle is moved away from neutral switch to manual control
jdawkins 3:ec0efa222dfa 168 auto_ctrl = false;
jdawkins 3:ec0efa222dfa 169 run_ctrl = false;
jdawkins 3:ec0efa222dfa 170 }
jdawkins 3:ec0efa222dfa 171 } // End if auto_ctrl
Sissors 0:bd0546063b0a 172
jdawkins 3:ec0efa222dfa 173 else { // Armed but in Manual Control
jdawkins 3:ec0efa222dfa 174
jdawkins 3:ec0efa222dfa 175 if(xbee.readable()) {
jdawkins 3:ec0efa222dfa 176 t_run = t.read();
jdawkins 3:ec0efa222dfa 177 log_data = !log_data;
jdawkins 3:ec0efa222dfa 178 }
jdawkins 3:ec0efa222dfa 179 str_cmd = ((CH1.servoPulse-1500.0)/500.0); // Convert Pulse to Normalized Command +/- 1
jdawkins 3:ec0efa222dfa 180 thr_cmd = ((CH2.servoPulse-1500.0)/500.0); // Convert Pulse to Normalized Command +/- 1
jdawkins 3:ec0efa222dfa 181 Steer.write((int)((str_cmd*500.0)+1500.0)); // Write Steering Pulse
jdawkins 3:ec0efa222dfa 182 Throttle.write((int)((thr_cmd*500.0)+1500.0)); //Write Throttle Pulse
jdawkins 3:ec0efa222dfa 183
jdawkins 3:ec0efa222dfa 184 auto_LED = 0;
jdawkins 3:ec0efa222dfa 185 } // end if autocontrol
jdawkins 3:ec0efa222dfa 186
jdawkins 3:ec0efa222dfa 187 } else {
jdawkins 3:ec0efa222dfa 188 armed_LED = 0; //Turn off armed LED indicator
jdawkins 3:ec0efa222dfa 189 str_cmd = 0;
jdawkins 3:ec0efa222dfa 190 thr_cmd = 0;
jdawkins 3:ec0efa222dfa 191 Steer.write(1500); //Set Steer PWM to center 1000-2000 range
jdawkins 3:ec0efa222dfa 192 Throttle.write(1500); //Set Throttle to Low
jdawkins 3:ec0efa222dfa 193 }/// end else armed
jdawkins 3:ec0efa222dfa 194
jdawkins 3:ec0efa222dfa 195 if(t.read()-t_hall < 0.2) {
jdawkins 3:ec0efa222dfa 196 speed = 0.036*GEAR_RATIO*(2*PI)/(dt_hall);
jdawkins 3:ec0efa222dfa 197 } else {
jdawkins 3:ec0efa222dfa 198 speed = 0;
jdawkins 3:ec0efa222dfa 199 }
jdawkins 3:ec0efa222dfa 200
jdawkins 3:ec0efa222dfa 201
jdawkins 3:ec0efa222dfa 202 // Read Angles
jdawkins 3:ec0efa222dfa 203 imu.get_angles();
jdawkins 3:ec0efa222dfa 204 imu.get_accel();
jdawkins 3:ec0efa222dfa 205 imu.get_gyro();
jdawkins 3:ec0efa222dfa 206 imu.get_mag();
Sissors 0:bd0546063b0a 207
jdawkins 3:ec0efa222dfa 208 dt = t.read()-t_loop;
jdawkins 3:ec0efa222dfa 209 t_loop = t.read();
jdawkins 3:ec0efa222dfa 210 psi_comp = (180/PI)*atan2(-imu.mag.y,imu.mag.x);
jdawkins 3:ec0efa222dfa 211 psi_est_dot = imu.gyro.z + L*(psi_comp - psi_est);
jdawkins 3:ec0efa222dfa 212
jdawkins 3:ec0efa222dfa 213 psi_est = psi_est + psi_est_dot*dt;
jdawkins 3:ec0efa222dfa 214
jdawkins 3:ec0efa222dfa 215 if(t.read()-t_log > (1/LOG_RATE)) {
jdawkins 3:ec0efa222dfa 216
jdawkins 3:ec0efa222dfa 217 if(log_data) {
jdawkins 3:ec0efa222dfa 218 /* Data Output Options
jdawkins 3:ec0efa222dfa 219 imu.accel.x imu.accel.y imu.accel.z
jdawkins 3:ec0efa222dfa 220 imu.gyro.x imu.gyro.y imu.gyro.z
jdawkins 3:ec0efa222dfa 221 wheel_spd thr str*/
jdawkins 3:ec0efa222dfa 222 xbee.printf("%.3f, %.3f, %.3f, %.3f, %.3f\r\n",t.read()-t_run,speed, thr_cmd, str_cmd, imu.gyro.z);
jdawkins 3:ec0efa222dfa 223 // xbee.printf("$MAG,%.1f, %.1f, %.1f\r\n",imu.mag.x,imu.mag.y,imu.mag.z);
jdawkins 3:ec0efa222dfa 224 // xbee.printf("$ODO,%.3f, %.3f, %.3f\r\n",wheel_spd,thr,str);
jdawkins 3:ec0efa222dfa 225 }
jdawkins 3:ec0efa222dfa 226 t_log = t.read();
jdawkins 3:ec0efa222dfa 227 }
jdawkins 3:ec0efa222dfa 228 wait(1/LOOP_RATE);
jdawkins 3:ec0efa222dfa 229 status_LED=!status_LED;
Sissors 0:bd0546063b0a 230 }
Sissors 0:bd0546063b0a 231
Sissors 0:bd0546063b0a 232 }
jdawkins 3:ec0efa222dfa 233
jdawkins 3:ec0efa222dfa 234 void menuFunction(Serial *port){
jdawkins 3:ec0efa222dfa 235 if(port->readable()) {
jdawkins 3:ec0efa222dfa 236 char c = port->getc();
jdawkins 3:ec0efa222dfa 237 if(c == 'H' || c == 'h') {
jdawkins 3:ec0efa222dfa 238 port->printf("Command List...\r\n");
jdawkins 3:ec0efa222dfa 239 port->printf("a - set autonomous mode\r\n");
jdawkins 3:ec0efa222dfa 240 port->printf("l - toggle data logging\r\n");
jdawkins 3:ec0efa222dfa 241 port->printf("r - run auto control loop\r\n");
jdawkins 3:ec0efa222dfa 242 port->printf("h - display this prompt and exit\r\n");
jdawkins 3:ec0efa222dfa 243 port->printf("Enter Command\r\n");
jdawkins 3:ec0efa222dfa 244 }
jdawkins 3:ec0efa222dfa 245 if(c == 'A' || c == 'a') {
jdawkins 3:ec0efa222dfa 246 auto_ctrl = true;
jdawkins 3:ec0efa222dfa 247 Steer.write((int)(1500.0));
jdawkins 3:ec0efa222dfa 248 }
jdawkins 3:ec0efa222dfa 249
jdawkins 3:ec0efa222dfa 250 if(c == 'R' || c == 'r') {
jdawkins 3:ec0efa222dfa 251 auto_ctrl = true;
jdawkins 3:ec0efa222dfa 252 Steer.write((int)(1500.0));
jdawkins 3:ec0efa222dfa 253
jdawkins 3:ec0efa222dfa 254 if(!log_data) {
jdawkins 3:ec0efa222dfa 255 t_log_start = t.read();
jdawkins 3:ec0efa222dfa 256 t_run = t.read();
jdawkins 3:ec0efa222dfa 257 xbee.printf("\r\n\r\n");
jdawkins 3:ec0efa222dfa 258 }
jdawkins 3:ec0efa222dfa 259 log_data = !log_data;
jdawkins 3:ec0efa222dfa 260 run_ctrl = true;
jdawkins 3:ec0efa222dfa 261 }
jdawkins 3:ec0efa222dfa 262
jdawkins 3:ec0efa222dfa 263 }
jdawkins 3:ec0efa222dfa 264
jdawkins 3:ec0efa222dfa 265 }
jdawkins 3:ec0efa222dfa 266 float saturateCmd(float cmd)
jdawkins 3:ec0efa222dfa 267 {
jdawkins 3:ec0efa222dfa 268 if(cmd>1.0) {
jdawkins 3:ec0efa222dfa 269 cmd = 1.0;
jdawkins 3:ec0efa222dfa 270 }
jdawkins 3:ec0efa222dfa 271 if(cmd < -1.0) {
jdawkins 3:ec0efa222dfa 272 cmd = -1.0;
jdawkins 3:ec0efa222dfa 273 }
jdawkins 3:ec0efa222dfa 274 return cmd;
jdawkins 3:ec0efa222dfa 275 }
jdawkins 3:ec0efa222dfa 276 float saturateCmd(float cmd, float max,float min)
jdawkins 3:ec0efa222dfa 277 {
jdawkins 3:ec0efa222dfa 278 if(cmd>max) {
jdawkins 3:ec0efa222dfa 279 cmd = max;
jdawkins 3:ec0efa222dfa 280 }
jdawkins 3:ec0efa222dfa 281 if(cmd < min) {
jdawkins 3:ec0efa222dfa 282 cmd = min;
jdawkins 3:ec0efa222dfa 283 }
jdawkins 3:ec0efa222dfa 284 return cmd;
jdawkins 3:ec0efa222dfa 285 }
jdawkins 3:ec0efa222dfa 286
jdawkins 3:ec0efa222dfa 287