USNA WSE ES456 / Mbed 2 deprecated Emaxx_Navigation_madpulse

Emaxx Navigation code ported for MBED

Dependencies:   BNO055_fusion Emaxx_Navigation_Dynamic_HIL MODSERIAL ServoIn ServoOut Vehicle_Model mbed

Fork of Emaxx_Navigation_Dynamic_HIL by Emaxx Navigation Group

main.cpp@9:3aa9b689bda5, 2018-04-03 (annotated)

Committer:
jdawkins
Date:
Tue Apr 03 12:52:52 2018 +0000
Revision:
9:3aa9b689bda5
Parent:
8:9817993e5df7 
Publish Code for MIDS

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jdawkins 0:f8fd381a5b8a 1 #include "mbed.h"
jdawkins 7:a8c2e9d049e8 2 #include "utilityFunctions.h"
jdawkins 0:f8fd381a5b8a 3 #include "BNO055.h"
jdawkins 0:f8fd381a5b8a 4 #include "ServoIn.h"
jdawkins 0:f8fd381a5b8a 5 #include "ServoOut.h"
lddevrie 2:0c9c3c1f3b18 6 #include "MODSERIAL.h"
jdawkins 7:a8c2e9d049e8 7 #include "vehicleModel.h"
jdawkins 8:9817993e5df7 8 #include "motor_driver.h"
jdawkins 0:f8fd381a5b8a 9
jdawkins 0:f8fd381a5b8a 10 #define MAX_MESSAGE_SIZE 64
jdawkins 0:f8fd381a5b8a 11 #define IMU_RATE 100.0
jdawkins 8:9817993e5df7 12 #define LOG_RATE 10.0
jdawkins 0:f8fd381a5b8a 13 #define LOOP_RATE 300.0
jdawkins 0:f8fd381a5b8a 14 #define CMD_TIMEOUT 1.0
jdawkins 8:9817993e5df7 15 #define GEAR_RATIO (1/2.75) // gear ratio
jdawkins 8:9817993e5df7 16 #define WHL_RADIUS 0.036 // Wheel radius (m)
jdawkins 8:9817993e5df7 17 #define PI 3.14159
jdawkins 8:9817993e5df7 18
jdawkins 0:f8fd381a5b8a 19 // Reference origin is at entrance to hospital point monument
jdawkins 0:f8fd381a5b8a 20 #define REF_LAT 38.986534
jdawkins 0:f8fd381a5b8a 21 #define REF_LON -76.489914
jdawkins 0:f8fd381a5b8a 22 #define REF_ALT 1.8
jdawkins 0:f8fd381a5b8a 23 #define NUM_LED 28
jdawkins 0:f8fd381a5b8a 24 #define LED_CLUSTERS 4
jdawkins 0:f8fd381a5b8a 25 #define LED_PER_CLUSTER 12
jdawkins 0:f8fd381a5b8a 26
jdawkins 0:f8fd381a5b8a 27 #define DIRECT_MODE 0 //command maps to throttle and steer commands normalized
jdawkins 0:f8fd381a5b8a 28 #define COURSE_MODE 1 //Commands map to heading and speed
jdawkins 7:a8c2e9d049e8 29
jdawkins 8:9817993e5df7 30 #define HARDWARE_MODE 0
jdawkins 7:a8c2e9d049e8 31 #define HIL_MODE 1 // commands map to hardware active simulation
jdawkins 7:a8c2e9d049e8 32 #define SIL_MODE 2 // commands map to software only simulation
jdawkins 0:f8fd381a5b8a 33
jdawkins 0:f8fd381a5b8a 34 //I2C i2c(p9, p10); // SDA, SCL
jdawkins 0:f8fd381a5b8a 35 BNO055 imu(p9, p10);
jdawkins 0:f8fd381a5b8a 36
jdawkins 0:f8fd381a5b8a 37 int left;
jdawkins 0:f8fd381a5b8a 38
jdawkins 0:f8fd381a5b8a 39 // Function Prototypes
jdawkins 8:9817993e5df7 40 void he_callback();
jdawkins 7:a8c2e9d049e8 41 int parseMessage(char * msg);
jdawkins 0:f8fd381a5b8a 42
jdawkins 0:f8fd381a5b8a 43 // LED Definitions
jdawkins 0:f8fd381a5b8a 44 DigitalOut rc_LED(LED1);
jdawkins 0:f8fd381a5b8a 45 DigitalOut armed_LED(LED2);
jdawkins 0:f8fd381a5b8a 46 DigitalOut auto_LED(LED3);
jdawkins 8:9817993e5df7 47 // DigitalOut imu_LED(LED4);
jdawkins 8:9817993e5df7 48 DigitalOut hall_LED(LED4);
jdawkins 0:f8fd381a5b8a 49
jdawkins 0:f8fd381a5b8a 50
lddevrie 2:0c9c3c1f3b18 51 // Comms and control object definitions
jdawkins 8:9817993e5df7 52 //MODSERIAL pc(p28, p27); // tx, rx for serial USB interface to pc
jdawkins 7:a8c2e9d049e8 53 //MODSERIAL xbee(USBTX, USBRX); // tx, rx for Xbee
lddevrie 6:f64b1eba4d5e 54
jdawkins 7:a8c2e9d049e8 55 MODSERIAL pc(USBTX, USBRX); // tx, rx for serial USB interface to pc
jdawkins 7:a8c2e9d049e8 56 MODSERIAL xbee(p28, p27); // tx, rx for Xbee
jdawkins 0:f8fd381a5b8a 57 ServoIn CH1(p15);
jdawkins 0:f8fd381a5b8a 58 ServoIn CH2(p16);
jdawkins 0:f8fd381a5b8a 59
jdawkins 9:3aa9b689bda5 60 InterruptIn he_sensor(p15);
jdawkins 9:3aa9b689bda5 61 ServoOut Steer(p20);
jdawkins 0:f8fd381a5b8a 62 ServoOut Throttle(p21);
jdawkins 8:9817993e5df7 63
jdawkins 8:9817993e5df7 64
jdawkins 0:f8fd381a5b8a 65 Timer t; // create timer instance
jdawkins 0:f8fd381a5b8a 66 Ticker log_tick;
jdawkins 0:f8fd381a5b8a 67 Ticker heartbeat;
jdawkins 7:a8c2e9d049e8 68 float t_imu,t_gps,t_cmd,t_sim,t_ctrl;
jdawkins 7:a8c2e9d049e8 69 float str_cmd,thr_cmd,str,thr,des_psi,des_spd; // control parameters
jdawkins 8:9817993e5df7 70 float psi_err,spd_err, psi_err_i,spd_err_i,speed; // control variables
jdawkins 0:f8fd381a5b8a 71 float t_hall, dt_hall,t_run,t_stop,t_log;
lddevrie 2:0c9c3c1f3b18 72 bool armed, auto_ctrl,auto_ctrl_old,rc_conn; // arming state modes
lddevrie 2:0c9c3c1f3b18 73 float wheel_spd; // wheel speed variable
lddevrie 2:0c9c3c1f3b18 74 float arm_clock,auto_clock; // timing for arming procedures
lddevrie 2:0c9c3c1f3b18 75 bool str_cond,thr_cond,run_ctrl,log_data; // data saving variables?
lddevrie 2:0c9c3c1f3b18 76 bool log_imu,log_bno,log_odo,log_mag = false; // data saving variables?
lddevrie 2:0c9c3c1f3b18 77 int cmd_mode; // integer to set command mode of controller
lddevrie 2:0c9c3c1f3b18 78 float Kp_psi, Kp_spd, Ki_psi, Ki_spd; // controller gains
lddevrie 2:0c9c3c1f3b18 79 int led1,led2,led3,led4; // neo-strip variables?
lddevrie 2:0c9c3c1f3b18 80 volatile bool newpacket = false; // boolean identifier of new odroid packet
lddevrie 4:e27ca0c82814 81 float x0; // initial x-position when in software or hardware in the loop simulation
lddevrie 4:e27ca0c82814 82 float y0; // initial y-position when in software or hardware in the loop simulation
lddevrie 4:e27ca0c82814 83 int sim_mode = 0; // sets simulation mode, zero by default, 1 for HIL, 2 for SIL
lddevrie 4:e27ca0c82814 84 float x = 0.0; // simulation variables
lddevrie 4:e27ca0c82814 85 float y = 0.0; // simulation variables
lddevrie 4:e27ca0c82814 86 float psi = 0.0; // simulation variables
lddevrie 5:d6d8ecd418cf 87 float spd = 0.0; // simulation speed
jdawkins 8:9817993e5df7 88 float tau = 0.25; // throttle time constant
jdawkins 8:9817993e5df7 89 float Kdc = 1; // throttle dc gain
jdawkins 8:9817993e5df7 90 float thr_out,thr_out_old;
jdawkins 8:9817993e5df7 91 float KpWhl = 0.6;
jdawkins 8:9817993e5df7 92 float dz;
jdawkins 8:9817993e5df7 93 int reverse;
lddevrie 2:0c9c3c1f3b18 94
jdawkins 7:a8c2e9d049e8 95 vehicleModel vm; //create vehicle model object
jdawkins 7:a8c2e9d049e8 96
jdawkins 8:9817993e5df7 97
lddevrie 2:0c9c3c1f3b18 98 void rxCallback(MODSERIAL_IRQ_INFO *q)
lddevrie 2:0c9c3c1f3b18 99 {
lddevrie 2:0c9c3c1f3b18 100 MODSERIAL *serial = q->serial;
lddevrie 2:0c9c3c1f3b18 101 if ( serial->rxGetLastChar() == '\n') {
lddevrie 2:0c9c3c1f3b18 102 newpacket = true;
lddevrie 2:0c9c3c1f3b18 103 }
lddevrie 2:0c9c3c1f3b18 104 }
lddevrie 2:0c9c3c1f3b18 105
jdawkins 0:f8fd381a5b8a 106
jdawkins 0:f8fd381a5b8a 107 int main()
jdawkins 0:f8fd381a5b8a 108 {
jdawkins 7:a8c2e9d049e8 109
jdawkins 0:f8fd381a5b8a 110
lddevrie 2:0c9c3c1f3b18 111 pc.baud(115200); // set baud rate of serial comm to pc
jdawkins 8:9817993e5df7 112 xbee.baud(115200); // set baud rate of serial comm of wireless xbee comms
jdawkins 0:f8fd381a5b8a 113 Steer.write(1500); //Set Steer PWM to center 1000-2000 range
jdawkins 0:f8fd381a5b8a 114 Throttle.write(1500); //Set Throttle to Low
lddevrie 2:0c9c3c1f3b18 115
lddevrie 2:0c9c3c1f3b18 116 xbee.attach(&rxCallback, MODSERIAL::RxIrq);
jdawkins 8:9817993e5df7 117
lddevrie 2:0c9c3c1f3b18 118
jdawkins 9:3aa9b689bda5 119 // he_sensor.rise(&he_callback); // Set wheel speed sensor interrupt
lddevrie 2:0c9c3c1f3b18 120
lddevrie 2:0c9c3c1f3b18 121 // initialize necessary float and boolean variables
jdawkins 0:f8fd381a5b8a 122 left = 0;
jdawkins 0:f8fd381a5b8a 123 str_cmd = 0;
jdawkins 0:f8fd381a5b8a 124 str=0;
jdawkins 0:f8fd381a5b8a 125 thr=0;
jdawkins 0:f8fd381a5b8a 126 thr_cmd = 0;
jdawkins 0:f8fd381a5b8a 127 des_psi = 0;
jdawkins 0:f8fd381a5b8a 128 des_spd = 0;
jdawkins 0:f8fd381a5b8a 129 psi_err = 0;
jdawkins 0:f8fd381a5b8a 130 spd_err = 0;
jdawkins 0:f8fd381a5b8a 131 spd_err_i = 0;
jdawkins 0:f8fd381a5b8a 132 arm_clock = 0;
jdawkins 0:f8fd381a5b8a 133 auto_clock = 0;
jdawkins 7:a8c2e9d049e8 134 Kp_psi = 1/1.57;
jdawkins 8:9817993e5df7 135 Kp_spd = 0.4;
jdawkins 8:9817993e5df7 136 Ki_spd = 0.2;
jdawkins 8:9817993e5df7 137 dz = 0.2;
jdawkins 0:f8fd381a5b8a 138 str_cond = false;
jdawkins 0:f8fd381a5b8a 139 thr_cond = false;
jdawkins 0:f8fd381a5b8a 140 armed = false;
jdawkins 0:f8fd381a5b8a 141 rc_conn = false;
jdawkins 0:f8fd381a5b8a 142 auto_ctrl = false;
jdawkins 0:f8fd381a5b8a 143 auto_ctrl_old = false;
jdawkins 0:f8fd381a5b8a 144 run_ctrl = false;
jdawkins 0:f8fd381a5b8a 145 log_data = false;
jdawkins 7:a8c2e9d049e8 146 cmd_mode = 1;
jdawkins 8:9817993e5df7 147 reverse = 0;
jdawkins 8:9817993e5df7 148
jdawkins 0:f8fd381a5b8a 149
lddevrie 2:0c9c3c1f3b18 150 // timer and timing initializations
jdawkins 0:f8fd381a5b8a 151 t.start();
jdawkins 0:f8fd381a5b8a 152 t_imu = t.read();
jdawkins 7:a8c2e9d049e8 153 t_sim = t.read();
jdawkins 7:a8c2e9d049e8 154 t_ctrl = t.read();
jdawkins 7:a8c2e9d049e8 155 t_log = t.read();
jdawkins 0:f8fd381a5b8a 156 t_cmd = 0;
jdawkins 0:f8fd381a5b8a 157
lddevrie 2:0c9c3c1f3b18 158
lddevrie 2:0c9c3c1f3b18 159 rc_LED = 0; // turn off LED 1 to indicate no RC connection
jdawkins 8:9817993e5df7 160 // imu_LED = 0; // turn off IMU indicator (LED 4)
lddevrie 2:0c9c3c1f3b18 161
lddevrie 4:e27ca0c82814 162
lddevrie 2:0c9c3c1f3b18 163 // procedure to ensure IMU is operating correctly
jdawkins 0:f8fd381a5b8a 164 if(imu.check()) {
jdawkins 0:f8fd381a5b8a 165 pc.printf("BNO055 connected\r\n");
jdawkins 0:f8fd381a5b8a 166 imu.setmode(OPERATION_MODE_CONFIG);
jdawkins 0:f8fd381a5b8a 167 imu.SetExternalCrystal(1);
jdawkins 0:f8fd381a5b8a 168 imu.setmode(OPERATION_MODE_NDOF); //Uses magnetometer
jdawkins 0:f8fd381a5b8a 169 imu.set_angle_units(RADIANS);
jdawkins 0:f8fd381a5b8a 170 imu.set_accel_units(MPERSPERS);
jdawkins 0:f8fd381a5b8a 171 imu.setoutputformat(WINDOWS);
jdawkins 0:f8fd381a5b8a 172 imu.set_mapping(2);
jdawkins 0:f8fd381a5b8a 173
lddevrie 2:0c9c3c1f3b18 174 // Blinks light if IMU is not calibrated, stops when calibration is complete
lddevrie 4:e27ca0c82814 175 /*while(int(imu.calib) < 0xCF) {
jdawkins 0:f8fd381a5b8a 176 pc.printf("Calibration = %x.\n\r",imu.calib);
jdawkins 0:f8fd381a5b8a 177 imu.get_calib();
jdawkins 0:f8fd381a5b8a 178 wait(0.5);
jdawkins 0:f8fd381a5b8a 179 imu_LED = !imu_LED;
lddevrie 2:0c9c3c1f3b18 180 } // end while(imu.calib)
lddevrie 4:e27ca0c82814 181 */
jdawkins 8:9817993e5df7 182 //imu_LED = 1; // turns on IMU light when calibration is successful
jdawkins 0:f8fd381a5b8a 183
jdawkins 0:f8fd381a5b8a 184 } else {
lddevrie 4:e27ca0c82814 185 pc.printf("IMU BNO055 NOT connected\r\n Entering Simulation mode..."); // catch statement if IMU is not connected correctly
lddevrie 4:e27ca0c82814 186
jdawkins 8:9817993e5df7 187 //sim_mode = 2; // by default it will go to simulation mode without actuators active (SIL)
lddevrie 4:e27ca0c82814 188
lddevrie 4:e27ca0c82814 189
lddevrie 4:e27ca0c82814 190 /* // turn on all lights is IMU is not connected correctly
jdawkins 0:f8fd381a5b8a 191 rc_LED = 1;
jdawkins 0:f8fd381a5b8a 192 armed_LED = 1;
jdawkins 0:f8fd381a5b8a 193 imu_LED = 1;
jdawkins 0:f8fd381a5b8a 194 auto_LED = 1;
jdawkins 0:f8fd381a5b8a 195 while(1) {
lddevrie 2:0c9c3c1f3b18 196 // blink all lights if IMU is not connected correctly
jdawkins 0:f8fd381a5b8a 197 rc_LED = !rc_LED;
jdawkins 0:f8fd381a5b8a 198 armed_LED = !armed_LED;
jdawkins 0:f8fd381a5b8a 199 imu_LED = !imu_LED;
jdawkins 0:f8fd381a5b8a 200 auto_LED = !auto_LED;
lddevrie 2:0c9c3c1f3b18 201
jdawkins 0:f8fd381a5b8a 202 wait(0.5);
lddevrie 2:0c9c3c1f3b18 203 } // end while(1) {blink if IMU is not connected}
lddevrie 4:e27ca0c82814 204 */
lddevrie 4:e27ca0c82814 205
lddevrie 2:0c9c3c1f3b18 206 } // end if(imu.check)
jdawkins 0:f8fd381a5b8a 207
lddevrie 4:e27ca0c82814 208
lddevrie 2:0c9c3c1f3b18 209 // int colors[4] = {ORANGE,YELLOW,GREEN,CYAN};
lddevrie 2:0c9c3c1f3b18 210
lddevrie 2:0c9c3c1f3b18 211 pc.printf("Emaxx Navigation Program\r\n"); // print indication that calibration is good and nav program is running
lddevrie 2:0c9c3c1f3b18 212
lddevrie 2:0c9c3c1f3b18 213
jdawkins 0:f8fd381a5b8a 214 while(1) {
jdawkins 0:f8fd381a5b8a 215
lddevrie 2:0c9c3c1f3b18 216 // check for servo pulse from either channel of receiver module
jdawkins 0:f8fd381a5b8a 217 if(CH1.servoPulse == 0 || CH2.servoPulse == 0) { //RC Reciever must be connected For Car to be armed
jdawkins 0:f8fd381a5b8a 218 rc_conn = false;
jdawkins 0:f8fd381a5b8a 219 } else {
jdawkins 0:f8fd381a5b8a 220 rc_conn = true;
lddevrie 2:0c9c3c1f3b18 221 } // end if(Channels connected)
lddevrie 2:0c9c3c1f3b18 222
jdawkins 9:3aa9b689bda5 223 // pc.printf("Str %d Thr %d\r\n",CH1.servoPulse,CH2.servoPulse);
lddevrie 2:0c9c3c1f3b18 224 // turn on RC led if transmitter is connected
jdawkins 0:f8fd381a5b8a 225 rc_LED = rc_conn;
jdawkins 0:f8fd381a5b8a 226 auto_LED = auto_ctrl;
jdawkins 0:f8fd381a5b8a 227 armed_LED = armed;
lddevrie 2:0c9c3c1f3b18 228
jdawkins 0:f8fd381a5b8a 229 str_cond = (CH1.servoPulse > 1800); // If steering is full right
jdawkins 0:f8fd381a5b8a 230 thr_cond = abs(CH2.servoPulse-1500)<100; // If throttle is near zero
jdawkins 0:f8fd381a5b8a 231
jdawkins 8:9817993e5df7 232 if(t.read()-auto_clock > 1) { //Auto control timeout if no commands recevied after 1 seconds
lddevrie 2:0c9c3c1f3b18 233 auto_ctrl = false;
lddevrie 2:0c9c3c1f3b18 234 } // end if(t.read()-autoclock>3) timeout procedure
lddevrie 2:0c9c3c1f3b18 235
lddevrie 2:0c9c3c1f3b18 236
lddevrie 2:0c9c3c1f3b18 237
lddevrie 2:0c9c3c1f3b18 238
lddevrie 2:0c9c3c1f3b18 239 if(newpacket) { // if xbee port receives a complete message, parse it
lddevrie 5:d6d8ecd418cf 240 char buf[MAX_MESSAGE_SIZE]; // create buffer for message
lddevrie 4:e27ca0c82814 241
lddevrie 2:0c9c3c1f3b18 242 // reads from modserial port buffer, stores characters into string "buf"
lddevrie 2:0c9c3c1f3b18 243 int i = 0;
lddevrie 2:0c9c3c1f3b18 244 if(xbee.rxBufferGetCount()>0) {
lddevrie 2:0c9c3c1f3b18 245 char c = xbee.getc();
lddevrie 2:0c9c3c1f3b18 246 if(c=='$') {
lddevrie 2:0c9c3c1f3b18 247 buf[i] = c;
lddevrie 2:0c9c3c1f3b18 248 i++;
lddevrie 2:0c9c3c1f3b18 249 while(1) {
lddevrie 2:0c9c3c1f3b18 250 buf[i] = xbee.getc();
lddevrie 2:0c9c3c1f3b18 251 if(buf[i]=='\n') {
lddevrie 2:0c9c3c1f3b18 252 break;
lddevrie 2:0c9c3c1f3b18 253 }
lddevrie 2:0c9c3c1f3b18 254 i++;
lddevrie 2:0c9c3c1f3b18 255 }
lddevrie 2:0c9c3c1f3b18 256 }
lddevrie 5:d6d8ecd418cf 257 } // end if xbee.rxBufferGetCount
lddevrie 2:0c9c3c1f3b18 258 xbee.rxBufferFlush();// empty receive buffer
jdawkins 9:3aa9b689bda5 259 pc.printf("%s",buf); // print message to PC
lddevrie 2:0c9c3c1f3b18 260 parseMessage(buf);
lddevrie 5:d6d8ecd418cf 261 newpacket = false; // reset packet flag
lddevrie 2:0c9c3c1f3b18 262 } // end if(newpacket)
jdawkins 9:3aa9b689bda5 263
jdawkins 8:9817993e5df7 264 float cdt = t.read()-t_ctrl;
jdawkins 8:9817993e5df7 265 t_ctrl = t.read();
jdawkins 0:f8fd381a5b8a 266
jdawkins 0:f8fd381a5b8a 267
lddevrie 5:d6d8ecd418cf 268 if(!rc_conn) { // Is System Armed, system armed if RC not connected
lddevrie 2:0c9c3c1f3b18 269 // printf("auto control: %d, clock %f\r\n",auto_ctrl, t.read()-auto_clock);
jdawkins 8:9817993e5df7 270 if(auto_ctrl) {
jdawkins 7:a8c2e9d049e8 271
jdawkins 8:9817993e5df7 272
lddevrie 2:0c9c3c1f3b18 273 switch (cmd_mode) {
jdawkins 0:f8fd381a5b8a 274 case DIRECT_MODE: {
jdawkins 8:9817993e5df7 275
jdawkins 8:9817993e5df7 276 str = -str_cmd;
jdawkins 8:9817993e5df7 277
jdawkins 8:9817993e5df7 278 des_spd = thr_cmd;
jdawkins 8:9817993e5df7 279 spd_err = des_spd - speed;
jdawkins 8:9817993e5df7 280 spd_err_i += spd_err*cdt;
jdawkins 8:9817993e5df7 281
jdawkins 8:9817993e5df7 282 // saturate integral term
jdawkins 8:9817993e5df7 283 if(spd_err_i> 1) {
jdawkins 8:9817993e5df7 284 spd_err_i = 1;
jdawkins 8:9817993e5df7 285 }
jdawkins 8:9817993e5df7 286 if(spd_err<-1) {
jdawkins 8:9817993e5df7 287 spd_err_i = -1;
jdawkins 8:9817993e5df7 288 }
jdawkins 8:9817993e5df7 289
jdawkins 8:9817993e5df7 290 thr = Kp_spd*(des_spd-speed) + Ki_spd*spd_err_i;
jdawkins 8:9817993e5df7 291
jdawkins 8:9817993e5df7 292 if(des_spd > 0.02){
jdawkins 8:9817993e5df7 293 thr = thr + dz;
jdawkins 8:9817993e5df7 294 }
jdawkins 8:9817993e5df7 295 else if(des_spd < -0.02){
jdawkins 8:9817993e5df7 296 // speed has no sign figure out how to make it track direction using accel perhaps.
jdawkins 8:9817993e5df7 297 thr = -(dz+0.2);
jdawkins 8:9817993e5df7 298 //thr = 0.3*des_spd;
jdawkins 8:9817993e5df7 299 }else {
jdawkins 8:9817993e5df7 300 thr = 0;
jdawkins 8:9817993e5df7 301 spd_err_i = 0;
jdawkins 8:9817993e5df7 302 }
jdawkins 8:9817993e5df7 303 //thr = thr_cmd;
jdawkins 8:9817993e5df7 304 // pc.printf("des_spd %.3f, Spd %.3f Thr_cmd %.3f rev %d\r\n",des_spd,reverse*speed,thr,reverse);
lddevrie 2:0c9c3c1f3b18 305 // xbee.printf("Psi Err: %.3f Spd Err %.3f, Str Cmd %.3f Thr_cmd %.3f\r\n",psi_err,spd_err,str,thr);
jdawkins 0:f8fd381a5b8a 306 break;
lddevrie 2:0c9c3c1f3b18 307 } // end direct mode case
jdawkins 0:f8fd381a5b8a 308 case COURSE_MODE: {
lddevrie 2:0c9c3c1f3b18 309
jdawkins 7:a8c2e9d049e8 310 thr = Kp_spd*spd_err + Ki_spd*spd_err_i*cdt;
lddevrie 2:0c9c3c1f3b18 311
lddevrie 2:0c9c3c1f3b18 312 if (thr >= 1.0) {
jdawkins 0:f8fd381a5b8a 313 thr = 1.0;
jdawkins 7:a8c2e9d049e8 314 spd_err_i = 0.0; // Reset Integral If Saturated
lddevrie 2:0c9c3c1f3b18 315 } // end if thr>=1.0
lddevrie 5:d6d8ecd418cf 316 if (thr < 0.0) {
lddevrie 5:d6d8ecd418cf 317 thr = 0.0;
jdawkins 7:a8c2e9d049e8 318 spd_err_i = 0.0; // Reset Integral If Saturated
lddevrie 2:0c9c3c1f3b18 319 } // end iff thr<0
jdawkins 0:f8fd381a5b8a 320
jdawkins 8:9817993e5df7 321
jdawkins 8:9817993e5df7 322 // pc.printf("Psi Err: %.3f, Str Cmd %.3f, Spd Err %.3f, Kp %.3f, Thr_cmd %.3f rev %d\r\n",psi_err,str,spd_err,Kp_spd,thr,reverse);
jdawkins 8:9817993e5df7 323
jdawkins 0:f8fd381a5b8a 324 break;
jdawkins 0:f8fd381a5b8a 325
lddevrie 2:0c9c3c1f3b18 326 } // end COURSE_MODE case
lddevrie 2:0c9c3c1f3b18 327 default: {
jdawkins 0:f8fd381a5b8a 328 break;
lddevrie 2:0c9c3c1f3b18 329 } // end default status in switch
lddevrie 2:0c9c3c1f3b18 330
lddevrie 2:0c9c3c1f3b18 331 } // end switch(cmd_mode)
jdawkins 7:a8c2e9d049e8 332 str = saturateCmd(str);
jdawkins 8:9817993e5df7 333
lddevrie 4:e27ca0c82814 334 } else { // goes with if auto_ctrl, manual control mode
lddevrie 2:0c9c3c1f3b18 335
jdawkins 0:f8fd381a5b8a 336 Steer.write((int)((str*500.0)+1500.0)); // Write Steering Pulse
jdawkins 0:f8fd381a5b8a 337 Throttle.write(1500); //Write Throttle Pulse
jdawkins 0:f8fd381a5b8a 338
lddevrie 2:0c9c3c1f3b18 339 } // end else
lddevrie 2:0c9c3c1f3b18 340
lddevrie 2:0c9c3c1f3b18 341 } else { // goes with if !rc_conn
lddevrie 5:d6d8ecd418cf 342
lddevrie 5:d6d8ecd418cf 343 // for manual driving
jdawkins 0:f8fd381a5b8a 344 auto_ctrl = false;
jdawkins 0:f8fd381a5b8a 345 armed_LED = 0; //Turn off armed LED indicator
jdawkins 0:f8fd381a5b8a 346 str = ((CH1.servoPulse-1500.0)/500.0); // Convert Pulse to Normalized Command +/- 1
jdawkins 0:f8fd381a5b8a 347 thr = ((CH2.servoPulse-1500.0)/500.0); // Convert Pulse to Normalized Command +/- 1
jdawkins 8:9817993e5df7 348
lddevrie 5:d6d8ecd418cf 349 if(sim_mode<2) { // if hardware is active send command to servo and throttle
jdawkins 8:9817993e5df7 350 //Steer.write((int)((str*500.0)+1500.0)); // Write Steering Pulse
jdawkins 8:9817993e5df7 351 // Throttle.write((int)((thr*500.0)+1500.0)); //Write Throttle Pulse
lddevrie 5:d6d8ecd418cf 352 }
lddevrie 2:0c9c3c1f3b18 353
jdawkins 0:f8fd381a5b8a 354 }/// end else armed
jdawkins 0:f8fd381a5b8a 355
jdawkins 8:9817993e5df7 356 float thr_dot = -(1/tau)*thr_out + (Kdc/tau)*thr; // first order filter for throttle
jdawkins 8:9817993e5df7 357 thr_out = thr_out_old + thr_dot*cdt;
jdawkins 9:3aa9b689bda5 358 thr_out_old = thr_out;//
jdawkins 9:3aa9b689bda5 359
jdawkins 8:9817993e5df7 360 Steer.write((int)((str*500.0)+1500.0)); // Write Steering Pulse
jdawkins 8:9817993e5df7 361 mot_control(thr);
jdawkins 9:3aa9b689bda5 362 pc.printf("crrl cmd: %f %f\r\n",str,thr);
jdawkins 8:9817993e5df7 363
jdawkins 9:3aa9b689bda5 364
jdawkins 9:3aa9b689bda5 365
lddevrie 4:e27ca0c82814 366 float dt = t.read()-t_imu;
lddevrie 4:e27ca0c82814 367 if(dt > (1/IMU_RATE)) {
jdawkins 7:a8c2e9d049e8 368 imu.get_angles();
jdawkins 7:a8c2e9d049e8 369 imu.get_accel();
jdawkins 7:a8c2e9d049e8 370 imu.get_gyro();
jdawkins 7:a8c2e9d049e8 371 imu.get_lia();
jdawkins 8:9817993e5df7 372
lddevrie 4:e27ca0c82814 373 float tic = t.read();
jdawkins 7:a8c2e9d049e8 374 t_imu = t.read();
lddevrie 4:e27ca0c82814 375
jdawkins 8:9817993e5df7 376 // *******************************************
jdawkins 8:9817993e5df7 377 // Get Sensor Data
jdawkins 8:9817993e5df7 378 // *******************************************
jdawkins 8:9817993e5df7 379 // Compute speed from hall effect sensor
jdawkins 8:9817993e5df7 380 if(t.read()-t_hall < 0.2) {
jdawkins 8:9817993e5df7 381 speed = 0.5*WHL_RADIUS*GEAR_RATIO*(2*PI)/(dt_hall); // inches/sec
jdawkins 8:9817993e5df7 382 } else {
jdawkins 8:9817993e5df7 383 speed = 0;
jdawkins 7:a8c2e9d049e8 384 }
jdawkins 8:9817993e5df7 385
jdawkins 8:9817993e5df7 386 if(abs(speed) <0.01){
jdawkins 8:9817993e5df7 387 if(imu.accel.x < -0.5 && thr_out < -0.05){
jdawkins 8:9817993e5df7 388 reverse = -1;
jdawkins 8:9817993e5df7 389 }else{
jdawkins 8:9817993e5df7 390 reverse = 1;
jdawkins 8:9817993e5df7 391 }
jdawkins 8:9817993e5df7 392 }
jdawkins 8:9817993e5df7 393
jdawkins 8:9817993e5df7 394 }
jdawkins 8:9817993e5df7 395
jdawkins 7:a8c2e9d049e8 396 if(t.read()-t_log > (1/LOG_RATE)) {
jdawkins 8:9817993e5df7 397
jdawkins 9:3aa9b689bda5 398
lddevrie 4:e27ca0c82814 399 xbee.printf("$IMU,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",imu.lia.x,imu.lia.y,imu.lia.z,imu.gyro.x,imu.gyro.y,imu.gyro.z,imu.euler.roll,imu.euler.pitch,wrapToPi(imu.euler.yaw));
jdawkins 8:9817993e5df7 400 // xbee.printf("Thr in %.3f Thr_out %.3f\r\n",thr,thr_out);
jdawkins 8:9817993e5df7 401 xbee.printf("$ODO,%.2f,%.2f,%.2f\r\n",speed,thr_out,str);
jdawkins 8:9817993e5df7 402 //pc.printf("$ODO,%.2f,%.2f,%.2f\r\n",speed,thr_out,str);
jdawkins 8:9817993e5df7 403
jdawkins 7:a8c2e9d049e8 404 t_log = t.read();
jdawkins 7:a8c2e9d049e8 405 }
lddevrie 4:e27ca0c82814 406
jdawkins 8:9817993e5df7 407 wait(1/LOOP_RATE);
jdawkins 8:9817993e5df7 408 // status_LED=!status_LED;
jdawkins 8:9817993e5df7 409 auto_ctrl_old = auto_ctrl;
lddevrie 4:e27ca0c82814 410
jdawkins 9:3aa9b689bda5 411 } /*else { // else condition implies a simulation mode is enabled
lddevrie 4:e27ca0c82814 412
jdawkins 8:9817993e5df7 413 float dt = t.read()-t_imu;
jdawkins 8:9817993e5df7 414 if(dt > (1/IMU_RATE)) {
jdawkins 0:f8fd381a5b8a 415
jdawkins 8:9817993e5df7 416 float tic = t.read();
lddevrie 5:d6d8ecd418cf 417
lddevrie 5:d6d8ecd418cf 418
jdawkins 8:9817993e5df7 419 x = x + spd*cos(psi)*dt; // self-propelled particle kinematics
jdawkins 8:9817993e5df7 420 y = y + spd*sin(psi)*dt; // self-propelled particle kinematics
jdawkins 8:9817993e5df7 421 psi = psi + str*dt; // turn rate kinematics
jdawkins 8:9817993e5df7 422 float drag = 0.0;
jdawkins 8:9817993e5df7 423 if(spd>1) {
jdawkins 8:9817993e5df7 424 drag = 0.0059*spd*spd; // based on drag on a cat sized object
jdawkins 8:9817993e5df7 425 } else {
jdawkins 8:9817993e5df7 426 drag = 0.0059*spd;
jdawkins 8:9817993e5df7 427 }
jdawkins 8:9817993e5df7 428 spd = spd + (thr-drag)*dt; // give throttle offset
jdawkins 0:f8fd381a5b8a 429
jdawkins 8:9817993e5df7 430 xbee.printf("$SIM,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",x,y,spd*cos(psi),spd*sin(psi),wrapToPi(psi));
jdawkins 8:9817993e5df7 431 pc.printf("$SIM,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f\r\n",x,y,spd*cos(psi),spd*sin(psi),wrapToPi(psi),thr,des_spd,spd,str);
jdawkins 8:9817993e5df7 432 t_imu = t.read();
jdawkins 8:9817993e5df7 433 } // end if dt
lddevrie 4:e27ca0c82814 434
lddevrie 4:e27ca0c82814 435
jdawkins 8:9817993e5df7 436 } // end if sim_mode */
jdawkins 0:f8fd381a5b8a 437
jdawkins 0:f8fd381a5b8a 438
lddevrie 4:e27ca0c82814 439 } // end while(1)
lddevrie 2:0c9c3c1f3b18 440
lddevrie 4:e27ca0c82814 441 } // end main
jdawkins 0:f8fd381a5b8a 442
jdawkins 8:9817993e5df7 443
jdawkins 8:9817993e5df7 444
jdawkins 8:9817993e5df7 445 void he_callback()
jdawkins 8:9817993e5df7 446 {
jdawkins 8:9817993e5df7 447 // Hall effect speed sensor callback
jdawkins 8:9817993e5df7 448 hall_LED = !hall_LED;
jdawkins 8:9817993e5df7 449
jdawkins 8:9817993e5df7 450 dt_hall = t.read()-t_hall;
jdawkins 8:9817993e5df7 451 t_hall = t.read();
jdawkins 8:9817993e5df7 452 } // end void he_callback
jdawkins 8:9817993e5df7 453
jdawkins 8:9817993e5df7 454
jdawkins 8:9817993e5df7 455
jdawkins 7:a8c2e9d049e8 456 int parseMessage(char * msg)
lddevrie 2:0c9c3c1f3b18 457 {
jdawkins 0:f8fd381a5b8a 458
lddevrie 2:0c9c3c1f3b18 459 if(!strncmp(msg, "$CMD", 4)) {
jdawkins 0:f8fd381a5b8a 460 int arg1;
jdawkins 0:f8fd381a5b8a 461 float arg2,arg3;
jdawkins 0:f8fd381a5b8a 462
jdawkins 0:f8fd381a5b8a 463 sscanf(msg,"$CMD,%d,%f,%f\n",&arg1,&arg2,&arg3);
jdawkins 0:f8fd381a5b8a 464 cmd_mode = arg1;
jdawkins 0:f8fd381a5b8a 465 auto_clock = t.read();
lddevrie 2:0c9c3c1f3b18 466 switch (cmd_mode) {
jdawkins 0:f8fd381a5b8a 467 case DIRECT_MODE: {
jdawkins 0:f8fd381a5b8a 468 auto_ctrl = true;
jdawkins 0:f8fd381a5b8a 469 str_cmd = arg2;
jdawkins 0:f8fd381a5b8a 470 thr_cmd = arg3;
jdawkins 0:f8fd381a5b8a 471 }
jdawkins 0:f8fd381a5b8a 472 case COURSE_MODE: {
jdawkins 0:f8fd381a5b8a 473 auto_ctrl = true;
jdawkins 0:f8fd381a5b8a 474 des_psi = arg2;
jdawkins 0:f8fd381a5b8a 475 des_spd = arg3;
jdawkins 0:f8fd381a5b8a 476 }
lddevrie 2:0c9c3c1f3b18 477 default: {
lddevrie 2:0c9c3c1f3b18 478 }
jdawkins 0:f8fd381a5b8a 479 }
jdawkins 8:9817993e5df7 480 return 1;
jdawkins 0:f8fd381a5b8a 481 } //emd of $CMD
lddevrie 2:0c9c3c1f3b18 482
lddevrie 2:0c9c3c1f3b18 483 if(!strncmp(msg, "$PRM", 4)) {
jdawkins 0:f8fd381a5b8a 484 float arg1,arg2,arg3;
jdawkins 0:f8fd381a5b8a 485 int type;
lddevrie 2:0c9c3c1f3b18 486 // __disable_irq(); // disable interrupts
jdawkins 0:f8fd381a5b8a 487 sscanf(msg,"$PRM,%d,%f,%f,%f",&type,&arg1,&arg2,&arg3);
lddevrie 2:0c9c3c1f3b18 488 // __enable_irq(); // enable interrupts
lddevrie 2:0c9c3c1f3b18 489
lddevrie 2:0c9c3c1f3b18 490 switch(type) {
lddevrie 2:0c9c3c1f3b18 491 case 1: { // sets PID gains on heading and speed controller
lddevrie 2:0c9c3c1f3b18 492 //pc.printf("%s\n",msg);
jdawkins 0:f8fd381a5b8a 493 Kp_psi = arg1;
jdawkins 0:f8fd381a5b8a 494 Kp_spd = arg2;
lddevrie 2:0c9c3c1f3b18 495 Ki_spd = arg3;
lddevrie 4:e27ca0c82814 496
lddevrie 2:0c9c3c1f3b18 497 pc.printf("Params Received: %f %f %f\n",arg1,arg2,arg3);
lddevrie 2:0c9c3c1f3b18 498 //xbee.printf("Params Recieved: %f %f %f\r\n",arg1,arg2,arg3);
jdawkins 0:f8fd381a5b8a 499 break;
jdawkins 0:f8fd381a5b8a 500 }
lddevrie 2:0c9c3c1f3b18 501 case 2: { // sets origin of local reference frame
lddevrie 4:e27ca0c82814 502
lddevrie 2:0c9c3c1f3b18 503 //pc.printf("%s\n",msg);
jdawkins 0:f8fd381a5b8a 504
jdawkins 0:f8fd381a5b8a 505 float ref_lat = arg1;
jdawkins 0:f8fd381a5b8a 506 float ref_lon = arg2;
jdawkins 0:f8fd381a5b8a 507 float ref_alt = arg3;
jdawkins 8:9817993e5df7 508 // gps.setRefPoint(ref_lat,ref_lon,ref_alt);
lddevrie 4:e27ca0c82814 509
lddevrie 2:0c9c3c1f3b18 510 pc.printf("Params Received: %f %f %f\n",arg1,arg2,arg3);
lddevrie 2:0c9c3c1f3b18 511 //xbee.printf("Params Recieved: %f %f %f\r\n",arg1,arg2,arg3);
lddevrie 4:e27ca0c82814 512
jdawkins 0:f8fd381a5b8a 513 break;
lddevrie 2:0c9c3c1f3b18 514 }
lddevrie 2:0c9c3c1f3b18 515 default: {
lddevrie 2:0c9c3c1f3b18 516 }
jdawkins 8:9817993e5df7 517
jdawkins 0:f8fd381a5b8a 518 }
jdawkins 7:a8c2e9d049e8 519 return 1;
lddevrie 2:0c9c3c1f3b18 520 } // end of $PRM
lddevrie 2:0c9c3c1f3b18 521
lddevrie 4:e27ca0c82814 522 if(!strncmp(msg, "$SIM", 4)) {
lddevrie 4:e27ca0c82814 523 int arg1;
lddevrie 4:e27ca0c82814 524 float arg2,arg3,arg4;
lddevrie 4:e27ca0c82814 525 sscanf(msg,"$SIM,%d,%f,%f,%f\n",&arg1,&arg2,&arg3,&arg4);
lddevrie 4:e27ca0c82814 526 sim_mode = arg1; // sets whether in hardware in the loop or software in the loop (actuators active or not during simulation)
jdawkins 7:a8c2e9d049e8 527 switch (sim_mode) {
lddevrie 4:e27ca0c82814 528 case HIL_MODE: {
lddevrie 4:e27ca0c82814 529 auto_ctrl = true;
lddevrie 4:e27ca0c82814 530 x = arg2;
lddevrie 4:e27ca0c82814 531 y = arg3;
lddevrie 4:e27ca0c82814 532 psi = arg4;
jdawkins 7:a8c2e9d049e8 533 vm.initialize(x,y,psi);
lddevrie 4:e27ca0c82814 534 }
lddevrie 4:e27ca0c82814 535 case SIL_MODE: {
lddevrie 4:e27ca0c82814 536 auto_ctrl = true;
lddevrie 4:e27ca0c82814 537 x = arg2;
lddevrie 4:e27ca0c82814 538 y = arg3;
lddevrie 4:e27ca0c82814 539 psi = arg4;
jdawkins 7:a8c2e9d049e8 540 vm.initialize(x,y,psi);
jdawkins 8:9817993e5df7 541
lddevrie 4:e27ca0c82814 542 }
lddevrie 4:e27ca0c82814 543 default: {
lddevrie 4:e27ca0c82814 544 }
lddevrie 4:e27ca0c82814 545 }
jdawkins 7:a8c2e9d049e8 546 return 1;
lddevrie 4:e27ca0c82814 547 } //emd of $SIM
jdawkins 7:a8c2e9d049e8 548 return 0;
jdawkins 0:f8fd381a5b8a 549
jdawkins 0:f8fd381a5b8a 550 }