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@5:03074e90ef7a, 2016-11-20 (annotated)

Committer:
piper
Date:
Sun Nov 20 19:29:37 2016 +0000
Revision:
5:03074e90ef7a
Parent:
4:ec2978ff7a1e
Child:
6:d133964667f3
Template for ES456 MadPulse Control Lab

Who changed what in which revision?

UserRevisionLine numberNew contents of line
piper 4:ec2978ff7a1e 1 // =========================================================================
piper 4:ec2978ff7a1e 2 // ES456 Autonomous Vehicles
piper 4:ec2978ff7a1e 3 // Template for MadPulse Vehicle Control
piper 4:ec2978ff7a1e 4 // Dawkins, Piper - Nov 2016
piper 4:ec2978ff7a1e 5 // =========================================================================
Sissors 0:bd0546063b0a 6
jdawkins 3:ec0efa222dfa 7 #define LOG_RATE 25.0
jdawkins 3:ec0efa222dfa 8 #define LOOP_RATE 200.0
jdawkins 3:ec0efa222dfa 9 #define CMD_TIMEOUT 1.0
piper 4:ec2978ff7a1e 10 #define WHL_RADIUS 0.7188 // Wheel radius (inches)
jdawkins 3:ec0efa222dfa 11 #define GEAR_RATIO (1/2.75)
jdawkins 3:ec0efa222dfa 12 #define PI 3.14159
Sissors 0:bd0546063b0a 13 #include "mbed.h"
jdawkins 3:ec0efa222dfa 14
jdawkins 3:ec0efa222dfa 15 #include "BNO055.h"
jdawkins 3:ec0efa222dfa 16 #include "ServoIn.h"
jdawkins 3:ec0efa222dfa 17 #include "ServoOut.h"
jdawkins 3:ec0efa222dfa 18
jdawkins 3:ec0efa222dfa 19 BNO055 imu(p9, p10);
jdawkins 3:ec0efa222dfa 20
piper 4:ec2978ff7a1e 21 void he_callback();
jdawkins 3:ec0efa222dfa 22 void menuFunction(Serial *port);
jdawkins 3:ec0efa222dfa 23 DigitalOut status_LED(LED1);
jdawkins 3:ec0efa222dfa 24 DigitalOut armed_LED(LED2);
jdawkins 3:ec0efa222dfa 25 DigitalOut auto_LED(LED3);
jdawkins 3:ec0efa222dfa 26 DigitalOut hall_LED(LED4);
jdawkins 3:ec0efa222dfa 27
jdawkins 3:ec0efa222dfa 28 Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc
jdawkins 3:ec0efa222dfa 29 Serial xbee(p28, p27); // tx, rx for Xbee
jdawkins 3:ec0efa222dfa 30 ServoIn CH1(p15);
jdawkins 3:ec0efa222dfa 31 ServoIn CH2(p16);
jdawkins 3:ec0efa222dfa 32
jdawkins 3:ec0efa222dfa 33 InterruptIn he_sensor(p23);
jdawkins 3:ec0efa222dfa 34 ServoOut Steer(p22);
jdawkins 3:ec0efa222dfa 35 ServoOut Throttle(p21);
jdawkins 3:ec0efa222dfa 36 Timer t; // create timer instance
jdawkins 3:ec0efa222dfa 37 Ticker log_tick;
jdawkins 3:ec0efa222dfa 38 Ticker heartbeat;
piper 5:03074e90ef7a 39 float t_imu,t_cmd,str_cmd,thr_cmd,dt;
jdawkins 3:ec0efa222dfa 40 float t_hall, dt_hall,t_run,t_stop,t_log_start,t_log,t_loop;
jdawkins 3:ec0efa222dfa 41
jdawkins 3:ec0efa222dfa 42 bool armed, auto_ctrl;
jdawkins 3:ec0efa222dfa 43 float speed;
jdawkins 3:ec0efa222dfa 44 float arm_clock;
jdawkins 3:ec0efa222dfa 45 bool str_cond,thr_cond,run_ctrl,log_data;
jdawkins 3:ec0efa222dfa 46
piper 5:03074e90ef7a 47 // USER DEFINED VARIABLES
piper 4:ec2978ff7a1e 48 float distance;
piper 4:ec2978ff7a1e 49 float steering_offset;
piper 4:ec2978ff7a1e 50 float hdg_comp, hdg_gyro;
piper 5:03074e90ef7a 51
jdawkins 3:ec0efa222dfa 52
piper 4:ec2978ff7a1e 53 // ============================================
piper 4:ec2978ff7a1e 54 // Main Program
piper 4:ec2978ff7a1e 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
piper 4:ec2978ff7a1e 62 he_sensor.rise(&he_callback); // Set hall effect speed sensor interrupt
jdawkins 3:ec0efa222dfa 63
jdawkins 3:ec0efa222dfa 64 str_cmd = 0;
jdawkins 3:ec0efa222dfa 65 thr_cmd = 0;
piper 5:03074e90ef7a 66 arm_clock =0;
jdawkins 3:ec0efa222dfa 67 str_cond = false;
jdawkins 3:ec0efa222dfa 68 thr_cond = false;
jdawkins 3:ec0efa222dfa 69 armed = false;
jdawkins 3:ec0efa222dfa 70 auto_ctrl = false;
jdawkins 3:ec0efa222dfa 71 run_ctrl = false;
jdawkins 3:ec0efa222dfa 72 log_data = false;
jdawkins 3:ec0efa222dfa 73
jdawkins 3:ec0efa222dfa 74 t.start();
jdawkins 3:ec0efa222dfa 75 t_log = t.read();
jdawkins 3:ec0efa222dfa 76 t_log_start = t.read();
jdawkins 3:ec0efa222dfa 77 t_cmd = 0;
jdawkins 3:ec0efa222dfa 78
jdawkins 3:ec0efa222dfa 79 status_LED = 1;
jdawkins 3:ec0efa222dfa 80
jdawkins 3:ec0efa222dfa 81 if(imu.check()) {
jdawkins 3:ec0efa222dfa 82 pc.printf("BNO055 connected\r\n");
jdawkins 3:ec0efa222dfa 83 xbee.printf("BNO055 connected\r\n");
jdawkins 3:ec0efa222dfa 84 imu.setmode(OPERATION_MODE_CONFIG);
jdawkins 3:ec0efa222dfa 85 imu.SetExternalCrystal(1);
jdawkins 3:ec0efa222dfa 86 imu.setmode(OPERATION_MODE_NDOF); //Uses magnetometer
jdawkins 3:ec0efa222dfa 87 imu.set_angle_units(DEGREES);
jdawkins 3:ec0efa222dfa 88 imu.set_accel_units(MPERSPERS);
jdawkins 3:ec0efa222dfa 89 imu.set_anglerate_units(DEG_PER_SEC);
jdawkins 3:ec0efa222dfa 90 imu.set_mapping(2);
jdawkins 3:ec0efa222dfa 91
jdawkins 3:ec0efa222dfa 92 } else {
jdawkins 3:ec0efa222dfa 93 pc.printf("IMU BNO055 NOT connected\r\n Program Trap.");
jdawkins 3:ec0efa222dfa 94 status_LED = 1;
jdawkins 3:ec0efa222dfa 95 armed_LED = 1;
jdawkins 3:ec0efa222dfa 96 hall_LED = 1;
jdawkins 3:ec0efa222dfa 97 auto_LED = 1;
jdawkins 3:ec0efa222dfa 98 while(1) {
jdawkins 3:ec0efa222dfa 99 status_LED = !status_LED;
jdawkins 3:ec0efa222dfa 100 armed_LED = !armed_LED;
jdawkins 3:ec0efa222dfa 101 hall_LED = !hall_LED;
jdawkins 3:ec0efa222dfa 102 auto_LED = !auto_LED;
jdawkins 3:ec0efa222dfa 103 wait(0.5);
jdawkins 3:ec0efa222dfa 104 }
piper 4:ec2978ff7a1e 105 } // imu.check
jdawkins 3:ec0efa222dfa 106
piper 4:ec2978ff7a1e 107
piper 5:03074e90ef7a 108 // INITIALIZE USER DEFINED VARIABLES
piper 4:ec2978ff7a1e 109 distance = 0; // Initialize distance
piper 5:03074e90ef7a 110 steering_offset = 0; // Steering command offset
piper 5:03074e90ef7a 111 hdg_gyro = 0; // Initialize integrated gyro heading
piper 5:03074e90ef7a 112 hdg_est = 0; // Initialize heading estimate
piper 5:03074e90ef7a 113
piper 4:ec2978ff7a1e 114
piper 4:ec2978ff7a1e 115 // ============================================
piper 4:ec2978ff7a1e 116 // Control Loop
piper 4:ec2978ff7a1e 117 // ============================================
jdawkins 3:ec0efa222dfa 118 while(1) {
jdawkins 3:ec0efa222dfa 119
piper 4:ec2978ff7a1e 120 // *******************************************
piper 4:ec2978ff7a1e 121 // Check failsafe conditions
piper 4:ec2978ff7a1e 122 // *******************************************
piper 4:ec2978ff7a1e 123 // Arm car when RC Reciever is connected
piper 4:ec2978ff7a1e 124 if(CH1.servoPulse == 0 || CH2.servoPulse ==0) {
jdawkins 3:ec0efa222dfa 125 armed = false;
jdawkins 3:ec0efa222dfa 126 } else {
jdawkins 3:ec0efa222dfa 127 armed = true;
jdawkins 3:ec0efa222dfa 128 }
piper 4:ec2978ff7a1e 129 // -------------------------------------------
piper 4:ec2978ff7a1e 130 // Enable auto control when throttle is zero
jdawkins 3:ec0efa222dfa 131 str_cond = (CH1.servoPulse > 1800); // If steering is full right
jdawkins 3:ec0efa222dfa 132 thr_cond = abs(CH2.servoPulse-1500)<50; // If throttle is near zero
jdawkins 3:ec0efa222dfa 133 if(str_cond&thr_cond) { // Both of the above conditions must be met
jdawkins 3:ec0efa222dfa 134 if(t.read()-arm_clock > 5) { // If conditions have been met for 5 seconds
jdawkins 3:ec0efa222dfa 135 Steer.write((int)(1500.0));
jdawkins 3:ec0efa222dfa 136 auto_ctrl = true; //Active auto control loop
jdawkins 3:ec0efa222dfa 137 }
jdawkins 3:ec0efa222dfa 138 } else {
jdawkins 3:ec0efa222dfa 139 arm_clock = t.read();
piper 4:ec2978ff7a1e 140 }
piper 4:ec2978ff7a1e 141 // ------------------------------------------
piper 4:ec2978ff7a1e 142 // Check PC terminal for run command
piper 4:ec2978ff7a1e 143 //menuFunction(&pc);
piper 4:ec2978ff7a1e 144 menuFunction(&xbee);
piper 4:ec2978ff7a1e 145
piper 4:ec2978ff7a1e 146 // *******************************************
piper 4:ec2978ff7a1e 147 // Get Sensor Data
piper 4:ec2978ff7a1e 148 // *******************************************
piper 4:ec2978ff7a1e 149 // Compute speed from hall effect sensor
piper 4:ec2978ff7a1e 150 if(t.read()-t_hall < 0.2) {
piper 4:ec2978ff7a1e 151 speed = WHL_RADIUS*GEAR_RATIO*(2*PI)/(dt_hall); // inches/sec
piper 4:ec2978ff7a1e 152 } else {
piper 4:ec2978ff7a1e 153 speed = 0;
jdawkins 3:ec0efa222dfa 154 }
piper 4:ec2978ff7a1e 155 // ------------------------------------------
piper 4:ec2978ff7a1e 156 // IMU data
piper 5:03074e90ef7a 157 imu.get_accel(); // imu.accel.x, imu.accel.y, imu.accel.z
piper 5:03074e90ef7a 158 imu.get_gyro(); // imu.gyro.x, imu.gyro.y, imu.gyro.z
piper 5:03074e90ef7a 159 imu.get_mag(); // imu.mag.x, imu.mag.y, imu.mag.z
jdawkins 3:ec0efa222dfa 160
piper 4:ec2978ff7a1e 161
piper 4:ec2978ff7a1e 162 // *******************************************
piper 4:ec2978ff7a1e 163 // Begin Control
piper 4:ec2978ff7a1e 164 // *******************************************
piper 4:ec2978ff7a1e 165
jdawkins 3:ec0efa222dfa 166 if(armed) { // Is System Armed
jdawkins 3:ec0efa222dfa 167 armed_LED = 1;
jdawkins 3:ec0efa222dfa 168
piper 4:ec2978ff7a1e 169 if(auto_ctrl) { // Armed and Auto Control enabled
jdawkins 3:ec0efa222dfa 170 auto_LED = 1; // Turn on LED to indicate Auto Control
jdawkins 3:ec0efa222dfa 171
piper 4:ec2978ff7a1e 172 if(run_ctrl) { // Armed, in Auto Control enabled, and Run command recieved
jdawkins 3:ec0efa222dfa 173
jdawkins 3:ec0efa222dfa 174 float run_time = t.read()-t_run;
piper 4:ec2978ff7a1e 175
piper 4:ec2978ff7a1e 176 //Integrate speed to determine distance (inches)
piper 4:ec2978ff7a1e 177 distance = distance + speed *dt;
piper 4:ec2978ff7a1e 178
piper 5:03074e90ef7a 179 // ***COMPUTE HEADING HERE***
piper 5:03074e90ef7a 180 // Compute Heading via magnetometer (deg)
piper 5:03074e90ef7a 181 //hdg_comp = ... ;
piper 4:ec2978ff7a1e 182
piper 4:ec2978ff7a1e 183 // Heading via integrated gyro
piper 5:03074e90ef7a 184 //hdg_gyro = ... ;
piper 5:03074e90ef7a 185
piper 5:03074e90ef7a 186 // Heading estimate
piper 5:03074e90ef7a 187 //hdg_est = ... ;
piper 5:03074e90ef7a 188
piper 5:03074e90ef7a 189
piper 5:03074e90ef7a 190 // ***COMPUTE CONTROL LAW HERE ***
piper 5:03074e90ef7a 191 // For now it is just a step input steering angle
piper 5:03074e90ef7a 192 if(distance <= 72 ) {
piper 5:03074e90ef7a 193 thr_cmd = 0.2;
piper 5:03074e90ef7a 194 str_cmd = 0.0;
piper 5:03074e90ef7a 195 } else if(distance > 72 && distance <= 150) {
piper 5:03074e90ef7a 196 thr_cmd = 0.2;
piper 5:03074e90ef7a 197 str_cmd = 0.5;
piper 4:ec2978ff7a1e 198 } else { // Run completed
jdawkins 3:ec0efa222dfa 199 thr_cmd = 0;
jdawkins 3:ec0efa222dfa 200 str_cmd = 0;
piper 4:ec2978ff7a1e 201 distance = 0; // Reset distance for next run
piper 4:ec2978ff7a1e 202 run_ctrl = false; // Turn off run control
jdawkins 3:ec0efa222dfa 203 log_data = false;
jdawkins 3:ec0efa222dfa 204 }
piper 5:03074e90ef7a 205
piper 4:ec2978ff7a1e 206 // Compensate for steering offset
piper 4:ec2978ff7a1e 207 str_cmd = str_cmd + steering_offset;
jdawkins 3:ec0efa222dfa 208
jdawkins 3:ec0efa222dfa 209 } // End if run_ctrl
jdawkins 3:ec0efa222dfa 210
piper 4:ec2978ff7a1e 211 // Write steering and throttle commands to vehicle
jdawkins 3:ec0efa222dfa 212 Steer.write((int)((str_cmd*500.0)+1500.0));
jdawkins 3:ec0efa222dfa 213 Throttle.write((int)((thr_cmd*500.0)+1500.0));
jdawkins 3:ec0efa222dfa 214
jdawkins 3:ec0efa222dfa 215 if(!thr_cond) { // If the throttle is moved away from neutral switch to manual control
jdawkins 3:ec0efa222dfa 216 auto_ctrl = false;
jdawkins 3:ec0efa222dfa 217 run_ctrl = false;
jdawkins 3:ec0efa222dfa 218 }
jdawkins 3:ec0efa222dfa 219 } // End if auto_ctrl
Sissors 0:bd0546063b0a 220
jdawkins 3:ec0efa222dfa 221 else { // Armed but in Manual Control
jdawkins 3:ec0efa222dfa 222
jdawkins 3:ec0efa222dfa 223 if(xbee.readable()) {
jdawkins 3:ec0efa222dfa 224 t_run = t.read();
jdawkins 3:ec0efa222dfa 225 log_data = !log_data;
jdawkins 3:ec0efa222dfa 226 }
jdawkins 3:ec0efa222dfa 227 str_cmd = ((CH1.servoPulse-1500.0)/500.0); // Convert Pulse to Normalized Command +/- 1
jdawkins 3:ec0efa222dfa 228 thr_cmd = ((CH2.servoPulse-1500.0)/500.0); // Convert Pulse to Normalized Command +/- 1
jdawkins 3:ec0efa222dfa 229 Steer.write((int)((str_cmd*500.0)+1500.0)); // Write Steering Pulse
jdawkins 3:ec0efa222dfa 230 Throttle.write((int)((thr_cmd*500.0)+1500.0)); //Write Throttle Pulse
jdawkins 3:ec0efa222dfa 231
jdawkins 3:ec0efa222dfa 232 auto_LED = 0;
jdawkins 3:ec0efa222dfa 233 } // end if autocontrol
jdawkins 3:ec0efa222dfa 234
piper 4:ec2978ff7a1e 235 } else { // System is not aArmed
jdawkins 3:ec0efa222dfa 236 armed_LED = 0; //Turn off armed LED indicator
jdawkins 3:ec0efa222dfa 237 str_cmd = 0;
jdawkins 3:ec0efa222dfa 238 thr_cmd = 0;
jdawkins 3:ec0efa222dfa 239 Steer.write(1500); //Set Steer PWM to center 1000-2000 range
jdawkins 3:ec0efa222dfa 240 Throttle.write(1500); //Set Throttle to Low
jdawkins 3:ec0efa222dfa 241 }/// end else armed
jdawkins 3:ec0efa222dfa 242
piper 4:ec2978ff7a1e 243
piper 4:ec2978ff7a1e 244 // ***************************************
piper 4:ec2978ff7a1e 245 // Transmit data to ground station
piper 4:ec2978ff7a1e 246 // ***************************************
piper 4:ec2978ff7a1e 247 if(t.read()-t_log > (1/LOG_RATE)) {
jdawkins 3:ec0efa222dfa 248 if(log_data) {
piper 5:03074e90ef7a 249 xbee.printf("%.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n",t.read()-t_run,speed, distance, thr_cmd, str_cmd, imu.gyro.z);
jdawkins 3:ec0efa222dfa 250 }
jdawkins 3:ec0efa222dfa 251 t_log = t.read();
piper 4:ec2978ff7a1e 252 } // End transmit data
piper 4:ec2978ff7a1e 253
piper 4:ec2978ff7a1e 254 dt = t.read()-t_loop; // Sample period (sec)
piper 4:ec2978ff7a1e 255 t_loop = t.read();
piper 4:ec2978ff7a1e 256
jdawkins 3:ec0efa222dfa 257 wait(1/LOOP_RATE);
jdawkins 3:ec0efa222dfa 258 status_LED=!status_LED;
piper 4:ec2978ff7a1e 259
piper 4:ec2978ff7a1e 260 } // End control loop
piper 4:ec2978ff7a1e 261
piper 4:ec2978ff7a1e 262 } // End main
piper 4:ec2978ff7a1e 263
Sissors 0:bd0546063b0a 264
piper 4:ec2978ff7a1e 265 void he_callback()
piper 4:ec2978ff7a1e 266 {
piper 4:ec2978ff7a1e 267 // Hall effect speed sensor callback
piper 4:ec2978ff7a1e 268 hall_LED = !hall_LED;
piper 4:ec2978ff7a1e 269
piper 4:ec2978ff7a1e 270 dt_hall = t.read()-t_hall;
piper 4:ec2978ff7a1e 271 t_hall = t.read();
Sissors 0:bd0546063b0a 272 }
jdawkins 3:ec0efa222dfa 273
piper 5:03074e90ef7a 274
jdawkins 3:ec0efa222dfa 275 void menuFunction(Serial *port){
jdawkins 3:ec0efa222dfa 276 if(port->readable()) {
jdawkins 3:ec0efa222dfa 277 char c = port->getc();
jdawkins 3:ec0efa222dfa 278 if(c == 'H' || c == 'h') {
jdawkins 3:ec0efa222dfa 279 port->printf("Command List...\r\n");
jdawkins 3:ec0efa222dfa 280 port->printf("a - set autonomous mode\r\n");
jdawkins 3:ec0efa222dfa 281 port->printf("l - toggle data logging\r\n");
jdawkins 3:ec0efa222dfa 282 port->printf("r - run auto control loop\r\n");
jdawkins 3:ec0efa222dfa 283 port->printf("h - display this prompt and exit\r\n");
jdawkins 3:ec0efa222dfa 284 port->printf("Enter Command\r\n");
jdawkins 3:ec0efa222dfa 285 }
jdawkins 3:ec0efa222dfa 286 if(c == 'A' || c == 'a') {
jdawkins 3:ec0efa222dfa 287 auto_ctrl = true;
jdawkins 3:ec0efa222dfa 288 Steer.write((int)(1500.0));
jdawkins 3:ec0efa222dfa 289 }
jdawkins 3:ec0efa222dfa 290
jdawkins 3:ec0efa222dfa 291 if(c == 'R' || c == 'r') {
jdawkins 3:ec0efa222dfa 292 auto_ctrl = true;
jdawkins 3:ec0efa222dfa 293 Steer.write((int)(1500.0));
jdawkins 3:ec0efa222dfa 294
jdawkins 3:ec0efa222dfa 295 if(!log_data) {
jdawkins 3:ec0efa222dfa 296 t_log_start = t.read();
jdawkins 3:ec0efa222dfa 297 t_run = t.read();
jdawkins 3:ec0efa222dfa 298 xbee.printf("\r\n\r\n");
jdawkins 3:ec0efa222dfa 299 }
jdawkins 3:ec0efa222dfa 300 log_data = !log_data;
jdawkins 3:ec0efa222dfa 301 run_ctrl = true;
jdawkins 3:ec0efa222dfa 302 }
jdawkins 3:ec0efa222dfa 303
jdawkins 3:ec0efa222dfa 304 }
jdawkins 3:ec0efa222dfa 305
jdawkins 3:ec0efa222dfa 306 }
piper 4:ec2978ff7a1e 307
piper 4:ec2978ff7a1e 308
piper 4:ec2978ff7a1e 309
jdawkins 3:ec0efa222dfa 310