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@6:f64b1eba4d5e, 2016-12-14 (annotated)

Committer:
lddevrie
Date:
Wed Dec 14 15:47:49 2016 +0000
Revision:
6:f64b1eba4d5e
Parent:
5:d6d8ecd418cf
Child:
7:a8c2e9d049e8
Updates to SIL kinematics and general commenting of code

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