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@7:a8c2e9d049e8, 2016-12-16 (annotated)

Committer:
jdawkins
Date:
Fri Dec 16 14:06:42 2016 +0000
Revision:
7:a8c2e9d049e8
Parent:
6:f64b1eba4d5e
Child:
8:9817993e5df7
Abstracted Vehicle Model into Class, Parse Message function returns int so it won't process all of the statements. Separated EKF time update from logging, so logging can happen at a slower rate than EKF. Changed baudrate to 57600 on xbee port.

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