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@8:9817993e5df7, 2017-04-07 (annotated)

Committer:
jdawkins
Date:
Fri Apr 07 17:12:29 2017 +0000
Revision:
8:9817993e5df7
Parent:
7:a8c2e9d049e8
Child:
9:3aa9b689bda5
Commit for Capstone Students

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