USNA WSE ES456 / Mbed 2 deprecated MadPulseCntrl

Template for ES456 MadPulse Control Lab

Dependencies:   BNO055_fusion ServoIn ServoOut mbed

Fork of ES456_Final_Proj_Faculty by USNA WSE ES456

main.cpp@4:ec2978ff7a1e, 2016-11-17 (annotated)

Committer:
piper
Date:
Thu Nov 17 19:41:38 2016 +0000
Revision:
4:ec2978ff7a1e
Parent:
3:ec0efa222dfa
Child:
5:03074e90ef7a
Included integrated gyro heading

Who changed what in which revision?

UserRevisionLine numberNew contents of line
piper 4:ec2978ff7a1e 1 // =========================================================================
piper 4:ec2978ff7a1e 2 // ES456 Autonomous Vehicles
piper 4:ec2978ff7a1e 3 // Template for MadPulse Vehicle Control
piper 4:ec2978ff7a1e 4 // Dawkins, Piper - Nov 2016
piper 4:ec2978ff7a1e 5 // =========================================================================
Sissors 0:bd0546063b0a 6
jdawkins 3:ec0efa222dfa 7 #define LOG_RATE 25.0
jdawkins 3:ec0efa222dfa 8 #define LOOP_RATE 200.0
jdawkins 3:ec0efa222dfa 9 #define CMD_TIMEOUT 1.0
piper 4:ec2978ff7a1e 10 #define WHL_RADIUS 0.7188 // Wheel radius (inches)
jdawkins 3:ec0efa222dfa 11 #define GEAR_RATIO (1/2.75)
jdawkins 3:ec0efa222dfa 12 #define PI 3.14159
Sissors 0:bd0546063b0a 13 #include "mbed.h"
jdawkins 3:ec0efa222dfa 14
jdawkins 3:ec0efa222dfa 15 #include "BNO055.h"
jdawkins 3:ec0efa222dfa 16 #include "ServoIn.h"
jdawkins 3:ec0efa222dfa 17 #include "ServoOut.h"
jdawkins 3:ec0efa222dfa 18
jdawkins 3:ec0efa222dfa 19 //I2C i2c(p9, p10); // SDA, SCL
jdawkins 3:ec0efa222dfa 20 BNO055 imu(p9, p10);
jdawkins 3:ec0efa222dfa 21
piper 4:ec2978ff7a1e 22 void he_callback();
jdawkins 3:ec0efa222dfa 23 float saturateCmd(float cmd);
jdawkins 3:ec0efa222dfa 24 void menuFunction(Serial *port);
jdawkins 3:ec0efa222dfa 25 DigitalOut status_LED(LED1);
jdawkins 3:ec0efa222dfa 26 DigitalOut armed_LED(LED2);
jdawkins 3:ec0efa222dfa 27 DigitalOut auto_LED(LED3);
jdawkins 3:ec0efa222dfa 28 DigitalOut hall_LED(LED4);
jdawkins 3:ec0efa222dfa 29
jdawkins 3:ec0efa222dfa 30 Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc
jdawkins 3:ec0efa222dfa 31 Serial xbee(p28, p27); // tx, rx for Xbee
jdawkins 3:ec0efa222dfa 32 ServoIn CH1(p15);
jdawkins 3:ec0efa222dfa 33 ServoIn CH2(p16);
jdawkins 3:ec0efa222dfa 34
jdawkins 3:ec0efa222dfa 35 InterruptIn he_sensor(p23);
jdawkins 3:ec0efa222dfa 36 ServoOut Steer(p22);
jdawkins 3:ec0efa222dfa 37 ServoOut Throttle(p21);
jdawkins 3:ec0efa222dfa 38 Timer t; // create timer instance
jdawkins 3:ec0efa222dfa 39 Ticker log_tick;
jdawkins 3:ec0efa222dfa 40 Ticker heartbeat;
jdawkins 3:ec0efa222dfa 41 float t_imu,t_cmd,str_cmd,thr_cmd,str,thr,dt;
jdawkins 3:ec0efa222dfa 42 float t_hall, dt_hall,t_run,t_stop,t_log_start,t_log,t_loop;
jdawkins 3:ec0efa222dfa 43 float psi_err,spd_err, psi_err_i,spd_err_i, psi_est, psi_est_dot, psi_comp;
jdawkins 3:ec0efa222dfa 44 float L;
jdawkins 3:ec0efa222dfa 45
jdawkins 3:ec0efa222dfa 46 bool armed, auto_ctrl;
jdawkins 3:ec0efa222dfa 47 float speed;
jdawkins 3:ec0efa222dfa 48 float arm_clock;
jdawkins 3:ec0efa222dfa 49 bool str_cond,thr_cond,run_ctrl,log_data;
jdawkins 3:ec0efa222dfa 50
piper 4:ec2978ff7a1e 51 // User defined variables
piper 4:ec2978ff7a1e 52 float distance;
piper 4:ec2978ff7a1e 53 float steering_offset;
piper 4:ec2978ff7a1e 54 float hdg_comp, hdg_gyro;
piper 4:ec2978ff7a1e 55 float hdg_target;
piper 4:ec2978ff7a1e 56 float K;
jdawkins 3:ec0efa222dfa 57
piper 4:ec2978ff7a1e 58 // ============================================
piper 4:ec2978ff7a1e 59 // Main Program
piper 4:ec2978ff7a1e 60 // ============================================
jdawkins 3:ec0efa222dfa 61 int main()
jdawkins 3:ec0efa222dfa 62 {
jdawkins 3:ec0efa222dfa 63
jdawkins 3:ec0efa222dfa 64 pc.baud(115200);
jdawkins 3:ec0efa222dfa 65 xbee.baud(115200);
Sissors 0:bd0546063b0a 66
piper 4:ec2978ff7a1e 67 he_sensor.rise(&he_callback); // Set hall effect speed sensor interrupt
jdawkins 3:ec0efa222dfa 68
jdawkins 3:ec0efa222dfa 69 str_cmd = 0;
jdawkins 3:ec0efa222dfa 70 str=0;
jdawkins 3:ec0efa222dfa 71 thr=0;
jdawkins 3:ec0efa222dfa 72 L =0;
jdawkins 3:ec0efa222dfa 73 thr_cmd = 0;
jdawkins 3:ec0efa222dfa 74 arm_clock =0;
jdawkins 3:ec0efa222dfa 75 psi_est=0;
jdawkins 3:ec0efa222dfa 76 psi_est_dot=0;
jdawkins 3:ec0efa222dfa 77 str_cond = false;
jdawkins 3:ec0efa222dfa 78 thr_cond = false;
jdawkins 3:ec0efa222dfa 79 armed = false;
jdawkins 3:ec0efa222dfa 80 auto_ctrl = false;
jdawkins 3:ec0efa222dfa 81 run_ctrl = false;
jdawkins 3:ec0efa222dfa 82 log_data = false;
jdawkins 3:ec0efa222dfa 83
jdawkins 3:ec0efa222dfa 84 t.start();
jdawkins 3:ec0efa222dfa 85 t_log = t.read();
jdawkins 3:ec0efa222dfa 86 t_log_start = t.read();
jdawkins 3:ec0efa222dfa 87 t_cmd = 0;
jdawkins 3:ec0efa222dfa 88
jdawkins 3:ec0efa222dfa 89 status_LED = 1;
jdawkins 3:ec0efa222dfa 90
jdawkins 3:ec0efa222dfa 91 if(imu.check()) {
jdawkins 3:ec0efa222dfa 92 pc.printf("BNO055 connected\r\n");
jdawkins 3:ec0efa222dfa 93 xbee.printf("BNO055 connected\r\n");
jdawkins 3:ec0efa222dfa 94 imu.setmode(OPERATION_MODE_CONFIG);
jdawkins 3:ec0efa222dfa 95 imu.SetExternalCrystal(1);
jdawkins 3:ec0efa222dfa 96 imu.setmode(OPERATION_MODE_NDOF); //Uses magnetometer
jdawkins 3:ec0efa222dfa 97 imu.set_angle_units(DEGREES);
jdawkins 3:ec0efa222dfa 98 imu.set_accel_units(MPERSPERS);
jdawkins 3:ec0efa222dfa 99 imu.set_anglerate_units(DEG_PER_SEC);
jdawkins 3:ec0efa222dfa 100 imu.set_mapping(2);
jdawkins 3:ec0efa222dfa 101
jdawkins 3:ec0efa222dfa 102 } else {
jdawkins 3:ec0efa222dfa 103 pc.printf("IMU BNO055 NOT connected\r\n Program Trap.");
jdawkins 3:ec0efa222dfa 104 status_LED = 1;
jdawkins 3:ec0efa222dfa 105 armed_LED = 1;
jdawkins 3:ec0efa222dfa 106 hall_LED = 1;
jdawkins 3:ec0efa222dfa 107 auto_LED = 1;
jdawkins 3:ec0efa222dfa 108 while(1) {
jdawkins 3:ec0efa222dfa 109 status_LED = !status_LED;
jdawkins 3:ec0efa222dfa 110 armed_LED = !armed_LED;
jdawkins 3:ec0efa222dfa 111 hall_LED = !hall_LED;
jdawkins 3:ec0efa222dfa 112 auto_LED = !auto_LED;
jdawkins 3:ec0efa222dfa 113 wait(0.5);
jdawkins 3:ec0efa222dfa 114 }
piper 4:ec2978ff7a1e 115 } // imu.check
jdawkins 3:ec0efa222dfa 116
piper 4:ec2978ff7a1e 117
piper 4:ec2978ff7a1e 118 // Initialize user defined variables
piper 4:ec2978ff7a1e 119 distance = 0; // Initialize distance
piper 4:ec2978ff7a1e 120 hdg_gyro = 0; // Initialize gyro heading
piper 4:ec2978ff7a1e 121 steering_offset = -.12;
piper 4:ec2978ff7a1e 122 K = 0.1; // Heading control gain
Sissors 0:bd0546063b0a 123
piper 4:ec2978ff7a1e 124
piper 4:ec2978ff7a1e 125 // ============================================
piper 4:ec2978ff7a1e 126 // Control Loop
piper 4:ec2978ff7a1e 127 // ============================================
jdawkins 3:ec0efa222dfa 128 while(1) {
jdawkins 3:ec0efa222dfa 129
piper 4:ec2978ff7a1e 130 // *******************************************
piper 4:ec2978ff7a1e 131 // Check failsafe conditions
piper 4:ec2978ff7a1e 132 // *******************************************
piper 4:ec2978ff7a1e 133 // Arm car when RC Reciever is connected
piper 4:ec2978ff7a1e 134 if(CH1.servoPulse == 0 || CH2.servoPulse ==0) {
jdawkins 3:ec0efa222dfa 135 armed = false;
jdawkins 3:ec0efa222dfa 136 } else {
jdawkins 3:ec0efa222dfa 137 armed = true;
jdawkins 3:ec0efa222dfa 138 }
piper 4:ec2978ff7a1e 139 // -------------------------------------------
piper 4:ec2978ff7a1e 140 // Enable auto control when throttle is zero
jdawkins 3:ec0efa222dfa 141 str_cond = (CH1.servoPulse > 1800); // If steering is full right
jdawkins 3:ec0efa222dfa 142 thr_cond = abs(CH2.servoPulse-1500)<50; // If throttle is near zero
jdawkins 3:ec0efa222dfa 143 if(str_cond&thr_cond) { // Both of the above conditions must be met
jdawkins 3:ec0efa222dfa 144 if(t.read()-arm_clock > 5) { // If conditions have been met for 5 seconds
jdawkins 3:ec0efa222dfa 145 Steer.write((int)(1500.0));
jdawkins 3:ec0efa222dfa 146 auto_ctrl = true; //Active auto control loop
jdawkins 3:ec0efa222dfa 147 }
jdawkins 3:ec0efa222dfa 148 } else {
jdawkins 3:ec0efa222dfa 149 arm_clock = t.read();
piper 4:ec2978ff7a1e 150 }
piper 4:ec2978ff7a1e 151 // ------------------------------------------
piper 4:ec2978ff7a1e 152 // Check PC terminal for run command
piper 4:ec2978ff7a1e 153 //menuFunction(&pc);
piper 4:ec2978ff7a1e 154 menuFunction(&xbee);
piper 4:ec2978ff7a1e 155
piper 4:ec2978ff7a1e 156 // *******************************************
piper 4:ec2978ff7a1e 157 // Get Sensor Data
piper 4:ec2978ff7a1e 158 // *******************************************
piper 4:ec2978ff7a1e 159 // Compute speed from hall effect sensor
piper 4:ec2978ff7a1e 160 if(t.read()-t_hall < 0.2) {
piper 4:ec2978ff7a1e 161 //speed = 0.036*GEAR_RATIO*(2*PI)/(dt_hall); // meters/sec ?
piper 4:ec2978ff7a1e 162 speed = WHL_RADIUS*GEAR_RATIO*(2*PI)/(dt_hall); // inches/sec
piper 4:ec2978ff7a1e 163 } else {
piper 4:ec2978ff7a1e 164 speed = 0;
jdawkins 3:ec0efa222dfa 165 }
piper 4:ec2978ff7a1e 166 // ------------------------------------------
piper 4:ec2978ff7a1e 167 // IMU data
piper 4:ec2978ff7a1e 168 //imu.get_angles();
piper 4:ec2978ff7a1e 169 imu.get_accel();
piper 4:ec2978ff7a1e 170 imu.get_gyro();
piper 4:ec2978ff7a1e 171 imu.get_mag();
jdawkins 3:ec0efa222dfa 172
piper 4:ec2978ff7a1e 173
piper 4:ec2978ff7a1e 174 // *******************************************
piper 4:ec2978ff7a1e 175 // Begin Control
piper 4:ec2978ff7a1e 176 // *******************************************
piper 4:ec2978ff7a1e 177
jdawkins 3:ec0efa222dfa 178 if(armed) { // Is System Armed
jdawkins 3:ec0efa222dfa 179 armed_LED = 1;
jdawkins 3:ec0efa222dfa 180
piper 4:ec2978ff7a1e 181 if(auto_ctrl) { // Armed and Auto Control enabled
jdawkins 3:ec0efa222dfa 182 auto_LED = 1; // Turn on LED to indicate Auto Control
jdawkins 3:ec0efa222dfa 183
piper 4:ec2978ff7a1e 184 if(run_ctrl) { // Armed, in Auto Control enabled, and Run command recieved
jdawkins 3:ec0efa222dfa 185
jdawkins 3:ec0efa222dfa 186 float run_time = t.read()-t_run;
piper 4:ec2978ff7a1e 187
piper 4:ec2978ff7a1e 188 //Integrate speed to determine distance (inches)
piper 4:ec2978ff7a1e 189 distance = distance + speed *dt;
piper 4:ec2978ff7a1e 190
piper 4:ec2978ff7a1e 191 // Heading via magnetometer (deg)
piper 4:ec2978ff7a1e 192 hdg_comp = (180/PI)*atan2(-imu.mag.y,imu.mag.x);
piper 4:ec2978ff7a1e 193
piper 4:ec2978ff7a1e 194 // Heading via integrated gyro
piper 4:ec2978ff7a1e 195 hdg_gyro = hdg_gyro + imu.gyro.z * dt;
piper 4:ec2978ff7a1e 196
piper 4:ec2978ff7a1e 197 if(distance <= 60 ) {
piper 4:ec2978ff7a1e 198 thr_cmd = 0.3;
piper 4:ec2978ff7a1e 199 //str_cmd = 0;
piper 4:ec2978ff7a1e 200 hdg_target = 0;
piper 4:ec2978ff7a1e 201 str_cmd = K * (hdg_target - hdg_gyro);
piper 4:ec2978ff7a1e 202 } else if(distance > 60 && distance <= 120) {
piper 4:ec2978ff7a1e 203 thr_cmd = 0.3;
piper 4:ec2978ff7a1e 204 hdg_target = 90;
piper 4:ec2978ff7a1e 205 str_cmd = K * (hdg_target - hdg_gyro);
piper 4:ec2978ff7a1e 206 } else { // Run completed
jdawkins 3:ec0efa222dfa 207 thr_cmd = 0;
jdawkins 3:ec0efa222dfa 208 str_cmd = 0;
piper 4:ec2978ff7a1e 209 distance = 0; // Reset distance for next run
piper 4:ec2978ff7a1e 210 run_ctrl = false; // Turn off run control
jdawkins 3:ec0efa222dfa 211 log_data = false;
jdawkins 3:ec0efa222dfa 212 }
piper 4:ec2978ff7a1e 213
piper 4:ec2978ff7a1e 214 // Compensate for steering offset
piper 4:ec2978ff7a1e 215 str_cmd = str_cmd + steering_offset;
jdawkins 3:ec0efa222dfa 216
jdawkins 3:ec0efa222dfa 217 } // End if run_ctrl
jdawkins 3:ec0efa222dfa 218
piper 4:ec2978ff7a1e 219 // Write steering and throttle commands to vehicle
jdawkins 3:ec0efa222dfa 220 Steer.write((int)((str_cmd*500.0)+1500.0));
jdawkins 3:ec0efa222dfa 221 Throttle.write((int)((thr_cmd*500.0)+1500.0));
jdawkins 3:ec0efa222dfa 222
jdawkins 3:ec0efa222dfa 223 if(!thr_cond) { // If the throttle is moved away from neutral switch to manual control
jdawkins 3:ec0efa222dfa 224 auto_ctrl = false;
jdawkins 3:ec0efa222dfa 225 run_ctrl = false;
jdawkins 3:ec0efa222dfa 226 }
jdawkins 3:ec0efa222dfa 227 } // End if auto_ctrl
Sissors 0:bd0546063b0a 228
jdawkins 3:ec0efa222dfa 229 else { // Armed but in Manual Control
jdawkins 3:ec0efa222dfa 230
jdawkins 3:ec0efa222dfa 231 if(xbee.readable()) {
jdawkins 3:ec0efa222dfa 232 t_run = t.read();
jdawkins 3:ec0efa222dfa 233 log_data = !log_data;
jdawkins 3:ec0efa222dfa 234 }
jdawkins 3:ec0efa222dfa 235 str_cmd = ((CH1.servoPulse-1500.0)/500.0); // Convert Pulse to Normalized Command +/- 1
jdawkins 3:ec0efa222dfa 236 thr_cmd = ((CH2.servoPulse-1500.0)/500.0); // Convert Pulse to Normalized Command +/- 1
jdawkins 3:ec0efa222dfa 237 Steer.write((int)((str_cmd*500.0)+1500.0)); // Write Steering Pulse
jdawkins 3:ec0efa222dfa 238 Throttle.write((int)((thr_cmd*500.0)+1500.0)); //Write Throttle Pulse
jdawkins 3:ec0efa222dfa 239
jdawkins 3:ec0efa222dfa 240 auto_LED = 0;
jdawkins 3:ec0efa222dfa 241 } // end if autocontrol
jdawkins 3:ec0efa222dfa 242
piper 4:ec2978ff7a1e 243 } else { // System is not aArmed
jdawkins 3:ec0efa222dfa 244 armed_LED = 0; //Turn off armed LED indicator
jdawkins 3:ec0efa222dfa 245 str_cmd = 0;
jdawkins 3:ec0efa222dfa 246 thr_cmd = 0;
jdawkins 3:ec0efa222dfa 247 Steer.write(1500); //Set Steer PWM to center 1000-2000 range
jdawkins 3:ec0efa222dfa 248 Throttle.write(1500); //Set Throttle to Low
jdawkins 3:ec0efa222dfa 249 }/// end else armed
jdawkins 3:ec0efa222dfa 250
piper 4:ec2978ff7a1e 251
Sissors 0:bd0546063b0a 252
piper 4:ec2978ff7a1e 253
jdawkins 3:ec0efa222dfa 254 psi_comp = (180/PI)*atan2(-imu.mag.y,imu.mag.x);
jdawkins 3:ec0efa222dfa 255 psi_est_dot = imu.gyro.z + L*(psi_comp - psi_est);
jdawkins 3:ec0efa222dfa 256
jdawkins 3:ec0efa222dfa 257 psi_est = psi_est + psi_est_dot*dt;
jdawkins 3:ec0efa222dfa 258
piper 4:ec2978ff7a1e 259 // ***************************************
piper 4:ec2978ff7a1e 260 // Transmit data to ground station
piper 4:ec2978ff7a1e 261 // ***************************************
piper 4:ec2978ff7a1e 262 if(t.read()-t_log > (1/LOG_RATE)) {
jdawkins 3:ec0efa222dfa 263 if(log_data) {
jdawkins 3:ec0efa222dfa 264 /* Data Output Options
jdawkins 3:ec0efa222dfa 265 imu.accel.x imu.accel.y imu.accel.z
jdawkins 3:ec0efa222dfa 266 imu.gyro.x imu.gyro.y imu.gyro.z
jdawkins 3:ec0efa222dfa 267 wheel_spd thr str*/
piper 4:ec2978ff7a1e 268 xbee.printf("%.3f, %.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n",t.read()-t_run,speed, distance, thr_cmd, str_cmd, imu.gyro.z, hdg_gyro);
piper 4:ec2978ff7a1e 269 // xbee.printf("%.3f, %.3f, %.3f, %.3f, %.3f\r\n",t.read()-t_run,speed, thr_cmd, str_cmd, imu.gyro.z);
jdawkins 3:ec0efa222dfa 270 // xbee.printf("$MAG,%.1f, %.1f, %.1f\r\n",imu.mag.x,imu.mag.y,imu.mag.z);
jdawkins 3:ec0efa222dfa 271 // xbee.printf("$ODO,%.3f, %.3f, %.3f\r\n",wheel_spd,thr,str);
jdawkins 3:ec0efa222dfa 272 }
jdawkins 3:ec0efa222dfa 273 t_log = t.read();
piper 4:ec2978ff7a1e 274 } // End transmit data
piper 4:ec2978ff7a1e 275
piper 4:ec2978ff7a1e 276 dt = t.read()-t_loop; // Sample period (sec)
piper 4:ec2978ff7a1e 277 t_loop = t.read();
piper 4:ec2978ff7a1e 278
jdawkins 3:ec0efa222dfa 279 wait(1/LOOP_RATE);
jdawkins 3:ec0efa222dfa 280 status_LED=!status_LED;
piper 4:ec2978ff7a1e 281
piper 4:ec2978ff7a1e 282 } // End control loop
piper 4:ec2978ff7a1e 283
piper 4:ec2978ff7a1e 284 } // End main
piper 4:ec2978ff7a1e 285
Sissors 0:bd0546063b0a 286
piper 4:ec2978ff7a1e 287 void he_callback()
piper 4:ec2978ff7a1e 288 {
piper 4:ec2978ff7a1e 289 // Hall effect speed sensor callback
piper 4:ec2978ff7a1e 290 hall_LED = !hall_LED;
piper 4:ec2978ff7a1e 291
piper 4:ec2978ff7a1e 292 dt_hall = t.read()-t_hall;
piper 4:ec2978ff7a1e 293 t_hall = t.read();
Sissors 0:bd0546063b0a 294 }
jdawkins 3:ec0efa222dfa 295
jdawkins 3:ec0efa222dfa 296 void menuFunction(Serial *port){
jdawkins 3:ec0efa222dfa 297 if(port->readable()) {
jdawkins 3:ec0efa222dfa 298 char c = port->getc();
jdawkins 3:ec0efa222dfa 299 if(c == 'H' || c == 'h') {
jdawkins 3:ec0efa222dfa 300 port->printf("Command List...\r\n");
jdawkins 3:ec0efa222dfa 301 port->printf("a - set autonomous mode\r\n");
jdawkins 3:ec0efa222dfa 302 port->printf("l - toggle data logging\r\n");
jdawkins 3:ec0efa222dfa 303 port->printf("r - run auto control loop\r\n");
jdawkins 3:ec0efa222dfa 304 port->printf("h - display this prompt and exit\r\n");
jdawkins 3:ec0efa222dfa 305 port->printf("Enter Command\r\n");
jdawkins 3:ec0efa222dfa 306 }
jdawkins 3:ec0efa222dfa 307 if(c == 'A' || c == 'a') {
jdawkins 3:ec0efa222dfa 308 auto_ctrl = true;
jdawkins 3:ec0efa222dfa 309 Steer.write((int)(1500.0));
jdawkins 3:ec0efa222dfa 310 }
jdawkins 3:ec0efa222dfa 311
jdawkins 3:ec0efa222dfa 312 if(c == 'R' || c == 'r') {
jdawkins 3:ec0efa222dfa 313 auto_ctrl = true;
jdawkins 3:ec0efa222dfa 314 Steer.write((int)(1500.0));
jdawkins 3:ec0efa222dfa 315
jdawkins 3:ec0efa222dfa 316 if(!log_data) {
jdawkins 3:ec0efa222dfa 317 t_log_start = t.read();
jdawkins 3:ec0efa222dfa 318 t_run = t.read();
jdawkins 3:ec0efa222dfa 319 xbee.printf("\r\n\r\n");
jdawkins 3:ec0efa222dfa 320 }
jdawkins 3:ec0efa222dfa 321 log_data = !log_data;
jdawkins 3:ec0efa222dfa 322 run_ctrl = true;
jdawkins 3:ec0efa222dfa 323 }
jdawkins 3:ec0efa222dfa 324
jdawkins 3:ec0efa222dfa 325 }
jdawkins 3:ec0efa222dfa 326
jdawkins 3:ec0efa222dfa 327 }
piper 4:ec2978ff7a1e 328
jdawkins 3:ec0efa222dfa 329 float saturateCmd(float cmd)
jdawkins 3:ec0efa222dfa 330 {
jdawkins 3:ec0efa222dfa 331 if(cmd>1.0) {
jdawkins 3:ec0efa222dfa 332 cmd = 1.0;
jdawkins 3:ec0efa222dfa 333 }
jdawkins 3:ec0efa222dfa 334 if(cmd < -1.0) {
jdawkins 3:ec0efa222dfa 335 cmd = -1.0;
jdawkins 3:ec0efa222dfa 336 }
jdawkins 3:ec0efa222dfa 337 return cmd;
jdawkins 3:ec0efa222dfa 338 }
piper 4:ec2978ff7a1e 339
piper 4:ec2978ff7a1e 340
jdawkins 3:ec0efa222dfa 341 float saturateCmd(float cmd, float max,float min)
jdawkins 3:ec0efa222dfa 342 {
jdawkins 3:ec0efa222dfa 343 if(cmd>max) {
jdawkins 3:ec0efa222dfa 344 cmd = max;
jdawkins 3:ec0efa222dfa 345 }
jdawkins 3:ec0efa222dfa 346 if(cmd < min) {
jdawkins 3:ec0efa222dfa 347 cmd = min;
jdawkins 3:ec0efa222dfa 348 }
jdawkins 3:ec0efa222dfa 349 return cmd;
jdawkins 3:ec0efa222dfa 350 }
jdawkins 3:ec0efa222dfa 351
jdawkins 3:ec0efa222dfa 352