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@5:d6d8ecd418cf, 2016-12-10 (annotated)

Committer:
lddevrie
Date:
Sat Dec 10 01:44:58 2016 +0000
Revision:
5:d6d8ecd418cf
Parent:
4:e27ca0c82814
Child:
6:f64b1eba4d5e
Update Euler integration with drag included. Drag proportional to v^2 for speeds greater than 1 m/s and v for speeds less than 1 m/s

Who changed what in which revision?

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