Template for ES456 MadPulse Control Lab
Dependencies: BNO055_fusion ServoIn ServoOut mbed
Fork of ES456_Final_Proj_Faculty by
main.cpp@6:d133964667f3, 2016-11-28 (annotated)
- Committer:
- piper
- Date:
- Mon Nov 28 13:08:24 2016 +0000
- Revision:
- 6:d133964667f3
- Parent:
- 5:03074e90ef7a
Set hall effect sensor interrupt to pin 11
Who changed what in which revision?
User | Revision | Line number | New 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 | BNO055 imu(p9, p10); |
jdawkins | 3:ec0efa222dfa | 20 | |
piper | 4:ec2978ff7a1e | 21 | void he_callback(); |
jdawkins | 3:ec0efa222dfa | 22 | void menuFunction(Serial *port); |
jdawkins | 3:ec0efa222dfa | 23 | DigitalOut status_LED(LED1); |
jdawkins | 3:ec0efa222dfa | 24 | DigitalOut armed_LED(LED2); |
jdawkins | 3:ec0efa222dfa | 25 | DigitalOut auto_LED(LED3); |
jdawkins | 3:ec0efa222dfa | 26 | DigitalOut hall_LED(LED4); |
jdawkins | 3:ec0efa222dfa | 27 | |
jdawkins | 3:ec0efa222dfa | 28 | Serial pc(USBTX, USBRX); // tx, rx for serial USB interface to pc |
jdawkins | 3:ec0efa222dfa | 29 | Serial xbee(p28, p27); // tx, rx for Xbee |
jdawkins | 3:ec0efa222dfa | 30 | ServoIn CH1(p15); |
jdawkins | 3:ec0efa222dfa | 31 | ServoIn CH2(p16); |
jdawkins | 3:ec0efa222dfa | 32 | |
piper | 6:d133964667f3 | 33 | InterruptIn he_sensor(p11); |
jdawkins | 3:ec0efa222dfa | 34 | ServoOut Steer(p22); |
jdawkins | 3:ec0efa222dfa | 35 | ServoOut Throttle(p21); |
jdawkins | 3:ec0efa222dfa | 36 | Timer t; // create timer instance |
jdawkins | 3:ec0efa222dfa | 37 | Ticker log_tick; |
jdawkins | 3:ec0efa222dfa | 38 | Ticker heartbeat; |
piper | 5:03074e90ef7a | 39 | float t_imu,t_cmd,str_cmd,thr_cmd,dt; |
jdawkins | 3:ec0efa222dfa | 40 | float t_hall, dt_hall,t_run,t_stop,t_log_start,t_log,t_loop; |
jdawkins | 3:ec0efa222dfa | 41 | |
jdawkins | 3:ec0efa222dfa | 42 | bool armed, auto_ctrl; |
jdawkins | 3:ec0efa222dfa | 43 | float speed; |
jdawkins | 3:ec0efa222dfa | 44 | float arm_clock; |
jdawkins | 3:ec0efa222dfa | 45 | bool str_cond,thr_cond,run_ctrl,log_data; |
jdawkins | 3:ec0efa222dfa | 46 | |
piper | 5:03074e90ef7a | 47 | // USER DEFINED VARIABLES |
piper | 4:ec2978ff7a1e | 48 | float distance; |
piper | 4:ec2978ff7a1e | 49 | float steering_offset; |
piper | 6:d133964667f3 | 50 | float hdg_comp, hdg_gyro, hdg_est; |
piper | 5:03074e90ef7a | 51 | |
jdawkins | 3:ec0efa222dfa | 52 | |
piper | 4:ec2978ff7a1e | 53 | // ============================================ |
piper | 4:ec2978ff7a1e | 54 | // Main Program |
piper | 4:ec2978ff7a1e | 55 | // ============================================ |
jdawkins | 3:ec0efa222dfa | 56 | int main() |
jdawkins | 3:ec0efa222dfa | 57 | { |
jdawkins | 3:ec0efa222dfa | 58 | |
jdawkins | 3:ec0efa222dfa | 59 | pc.baud(115200); |
jdawkins | 3:ec0efa222dfa | 60 | xbee.baud(115200); |
Sissors | 0:bd0546063b0a | 61 | |
piper | 4:ec2978ff7a1e | 62 | he_sensor.rise(&he_callback); // Set hall effect speed sensor interrupt |
jdawkins | 3:ec0efa222dfa | 63 | |
jdawkins | 3:ec0efa222dfa | 64 | str_cmd = 0; |
jdawkins | 3:ec0efa222dfa | 65 | thr_cmd = 0; |
piper | 5:03074e90ef7a | 66 | arm_clock =0; |
jdawkins | 3:ec0efa222dfa | 67 | str_cond = false; |
jdawkins | 3:ec0efa222dfa | 68 | thr_cond = false; |
jdawkins | 3:ec0efa222dfa | 69 | armed = false; |
jdawkins | 3:ec0efa222dfa | 70 | auto_ctrl = false; |
jdawkins | 3:ec0efa222dfa | 71 | run_ctrl = false; |
jdawkins | 3:ec0efa222dfa | 72 | log_data = false; |
jdawkins | 3:ec0efa222dfa | 73 | |
jdawkins | 3:ec0efa222dfa | 74 | t.start(); |
jdawkins | 3:ec0efa222dfa | 75 | t_log = t.read(); |
jdawkins | 3:ec0efa222dfa | 76 | t_log_start = t.read(); |
jdawkins | 3:ec0efa222dfa | 77 | t_cmd = 0; |
jdawkins | 3:ec0efa222dfa | 78 | |
jdawkins | 3:ec0efa222dfa | 79 | status_LED = 1; |
jdawkins | 3:ec0efa222dfa | 80 | |
jdawkins | 3:ec0efa222dfa | 81 | if(imu.check()) { |
jdawkins | 3:ec0efa222dfa | 82 | pc.printf("BNO055 connected\r\n"); |
jdawkins | 3:ec0efa222dfa | 83 | xbee.printf("BNO055 connected\r\n"); |
jdawkins | 3:ec0efa222dfa | 84 | imu.setmode(OPERATION_MODE_CONFIG); |
jdawkins | 3:ec0efa222dfa | 85 | imu.SetExternalCrystal(1); |
jdawkins | 3:ec0efa222dfa | 86 | imu.setmode(OPERATION_MODE_NDOF); //Uses magnetometer |
jdawkins | 3:ec0efa222dfa | 87 | imu.set_angle_units(DEGREES); |
jdawkins | 3:ec0efa222dfa | 88 | imu.set_accel_units(MPERSPERS); |
jdawkins | 3:ec0efa222dfa | 89 | imu.set_anglerate_units(DEG_PER_SEC); |
jdawkins | 3:ec0efa222dfa | 90 | imu.set_mapping(2); |
jdawkins | 3:ec0efa222dfa | 91 | |
jdawkins | 3:ec0efa222dfa | 92 | } else { |
jdawkins | 3:ec0efa222dfa | 93 | pc.printf("IMU BNO055 NOT connected\r\n Program Trap."); |
jdawkins | 3:ec0efa222dfa | 94 | status_LED = 1; |
jdawkins | 3:ec0efa222dfa | 95 | armed_LED = 1; |
jdawkins | 3:ec0efa222dfa | 96 | hall_LED = 1; |
jdawkins | 3:ec0efa222dfa | 97 | auto_LED = 1; |
jdawkins | 3:ec0efa222dfa | 98 | while(1) { |
jdawkins | 3:ec0efa222dfa | 99 | status_LED = !status_LED; |
jdawkins | 3:ec0efa222dfa | 100 | armed_LED = !armed_LED; |
jdawkins | 3:ec0efa222dfa | 101 | hall_LED = !hall_LED; |
jdawkins | 3:ec0efa222dfa | 102 | auto_LED = !auto_LED; |
jdawkins | 3:ec0efa222dfa | 103 | wait(0.5); |
jdawkins | 3:ec0efa222dfa | 104 | } |
piper | 4:ec2978ff7a1e | 105 | } // imu.check |
jdawkins | 3:ec0efa222dfa | 106 | |
piper | 4:ec2978ff7a1e | 107 | |
piper | 5:03074e90ef7a | 108 | // INITIALIZE USER DEFINED VARIABLES |
piper | 4:ec2978ff7a1e | 109 | distance = 0; // Initialize distance |
piper | 5:03074e90ef7a | 110 | steering_offset = 0; // Steering command offset |
piper | 5:03074e90ef7a | 111 | hdg_gyro = 0; // Initialize integrated gyro heading |
piper | 5:03074e90ef7a | 112 | hdg_est = 0; // Initialize heading estimate |
piper | 5:03074e90ef7a | 113 | |
piper | 4:ec2978ff7a1e | 114 | |
piper | 4:ec2978ff7a1e | 115 | // ============================================ |
piper | 4:ec2978ff7a1e | 116 | // Control Loop |
piper | 4:ec2978ff7a1e | 117 | // ============================================ |
jdawkins | 3:ec0efa222dfa | 118 | while(1) { |
jdawkins | 3:ec0efa222dfa | 119 | |
piper | 4:ec2978ff7a1e | 120 | // ******************************************* |
piper | 4:ec2978ff7a1e | 121 | // Check failsafe conditions |
piper | 4:ec2978ff7a1e | 122 | // ******************************************* |
piper | 4:ec2978ff7a1e | 123 | // Arm car when RC Reciever is connected |
piper | 4:ec2978ff7a1e | 124 | if(CH1.servoPulse == 0 || CH2.servoPulse ==0) { |
jdawkins | 3:ec0efa222dfa | 125 | armed = false; |
jdawkins | 3:ec0efa222dfa | 126 | } else { |
jdawkins | 3:ec0efa222dfa | 127 | armed = true; |
jdawkins | 3:ec0efa222dfa | 128 | } |
piper | 4:ec2978ff7a1e | 129 | // ------------------------------------------- |
piper | 4:ec2978ff7a1e | 130 | // Enable auto control when throttle is zero |
jdawkins | 3:ec0efa222dfa | 131 | str_cond = (CH1.servoPulse > 1800); // If steering is full right |
jdawkins | 3:ec0efa222dfa | 132 | thr_cond = abs(CH2.servoPulse-1500)<50; // If throttle is near zero |
jdawkins | 3:ec0efa222dfa | 133 | if(str_cond&thr_cond) { // Both of the above conditions must be met |
jdawkins | 3:ec0efa222dfa | 134 | if(t.read()-arm_clock > 5) { // If conditions have been met for 5 seconds |
jdawkins | 3:ec0efa222dfa | 135 | Steer.write((int)(1500.0)); |
jdawkins | 3:ec0efa222dfa | 136 | auto_ctrl = true; //Active auto control loop |
jdawkins | 3:ec0efa222dfa | 137 | } |
jdawkins | 3:ec0efa222dfa | 138 | } else { |
jdawkins | 3:ec0efa222dfa | 139 | arm_clock = t.read(); |
piper | 4:ec2978ff7a1e | 140 | } |
piper | 4:ec2978ff7a1e | 141 | // ------------------------------------------ |
piper | 4:ec2978ff7a1e | 142 | // Check PC terminal for run command |
piper | 4:ec2978ff7a1e | 143 | //menuFunction(&pc); |
piper | 4:ec2978ff7a1e | 144 | menuFunction(&xbee); |
piper | 4:ec2978ff7a1e | 145 | |
piper | 4:ec2978ff7a1e | 146 | // ******************************************* |
piper | 4:ec2978ff7a1e | 147 | // Get Sensor Data |
piper | 4:ec2978ff7a1e | 148 | // ******************************************* |
piper | 4:ec2978ff7a1e | 149 | // Compute speed from hall effect sensor |
piper | 4:ec2978ff7a1e | 150 | if(t.read()-t_hall < 0.2) { |
piper | 6:d133964667f3 | 151 | speed = 2*WHL_RADIUS*GEAR_RATIO*(2*PI)/(dt_hall); // inches/sec |
piper | 4:ec2978ff7a1e | 152 | } else { |
piper | 4:ec2978ff7a1e | 153 | speed = 0; |
jdawkins | 3:ec0efa222dfa | 154 | } |
piper | 4:ec2978ff7a1e | 155 | // ------------------------------------------ |
piper | 4:ec2978ff7a1e | 156 | // IMU data |
piper | 5:03074e90ef7a | 157 | imu.get_accel(); // imu.accel.x, imu.accel.y, imu.accel.z |
piper | 5:03074e90ef7a | 158 | imu.get_gyro(); // imu.gyro.x, imu.gyro.y, imu.gyro.z |
piper | 5:03074e90ef7a | 159 | imu.get_mag(); // imu.mag.x, imu.mag.y, imu.mag.z |
jdawkins | 3:ec0efa222dfa | 160 | |
piper | 4:ec2978ff7a1e | 161 | |
piper | 4:ec2978ff7a1e | 162 | // ******************************************* |
piper | 4:ec2978ff7a1e | 163 | // Begin Control |
piper | 4:ec2978ff7a1e | 164 | // ******************************************* |
piper | 4:ec2978ff7a1e | 165 | |
jdawkins | 3:ec0efa222dfa | 166 | if(armed) { // Is System Armed |
jdawkins | 3:ec0efa222dfa | 167 | armed_LED = 1; |
jdawkins | 3:ec0efa222dfa | 168 | |
piper | 4:ec2978ff7a1e | 169 | if(auto_ctrl) { // Armed and Auto Control enabled |
jdawkins | 3:ec0efa222dfa | 170 | auto_LED = 1; // Turn on LED to indicate Auto Control |
jdawkins | 3:ec0efa222dfa | 171 | |
piper | 4:ec2978ff7a1e | 172 | if(run_ctrl) { // Armed, in Auto Control enabled, and Run command recieved |
jdawkins | 3:ec0efa222dfa | 173 | |
jdawkins | 3:ec0efa222dfa | 174 | float run_time = t.read()-t_run; |
piper | 4:ec2978ff7a1e | 175 | |
piper | 4:ec2978ff7a1e | 176 | //Integrate speed to determine distance (inches) |
piper | 4:ec2978ff7a1e | 177 | distance = distance + speed *dt; |
piper | 4:ec2978ff7a1e | 178 | |
piper | 5:03074e90ef7a | 179 | // ***COMPUTE HEADING HERE*** |
piper | 5:03074e90ef7a | 180 | // Compute Heading via magnetometer (deg) |
piper | 5:03074e90ef7a | 181 | //hdg_comp = ... ; |
piper | 4:ec2978ff7a1e | 182 | |
piper | 4:ec2978ff7a1e | 183 | // Heading via integrated gyro |
piper | 5:03074e90ef7a | 184 | //hdg_gyro = ... ; |
piper | 5:03074e90ef7a | 185 | |
piper | 5:03074e90ef7a | 186 | // Heading estimate |
piper | 5:03074e90ef7a | 187 | //hdg_est = ... ; |
piper | 5:03074e90ef7a | 188 | |
piper | 5:03074e90ef7a | 189 | |
piper | 5:03074e90ef7a | 190 | // ***COMPUTE CONTROL LAW HERE *** |
piper | 5:03074e90ef7a | 191 | // For now it is just a step input steering angle |
piper | 5:03074e90ef7a | 192 | if(distance <= 72 ) { |
piper | 5:03074e90ef7a | 193 | thr_cmd = 0.2; |
piper | 5:03074e90ef7a | 194 | str_cmd = 0.0; |
piper | 5:03074e90ef7a | 195 | } else if(distance > 72 && distance <= 150) { |
piper | 5:03074e90ef7a | 196 | thr_cmd = 0.2; |
piper | 5:03074e90ef7a | 197 | str_cmd = 0.5; |
piper | 4:ec2978ff7a1e | 198 | } else { // Run completed |
jdawkins | 3:ec0efa222dfa | 199 | thr_cmd = 0; |
jdawkins | 3:ec0efa222dfa | 200 | str_cmd = 0; |
piper | 4:ec2978ff7a1e | 201 | distance = 0; // Reset distance for next run |
piper | 4:ec2978ff7a1e | 202 | run_ctrl = false; // Turn off run control |
jdawkins | 3:ec0efa222dfa | 203 | log_data = false; |
jdawkins | 3:ec0efa222dfa | 204 | } |
piper | 5:03074e90ef7a | 205 | |
piper | 4:ec2978ff7a1e | 206 | // Compensate for steering offset |
piper | 4:ec2978ff7a1e | 207 | str_cmd = str_cmd + steering_offset; |
jdawkins | 3:ec0efa222dfa | 208 | |
jdawkins | 3:ec0efa222dfa | 209 | } // End if run_ctrl |
jdawkins | 3:ec0efa222dfa | 210 | |
piper | 4:ec2978ff7a1e | 211 | // Write steering and throttle commands to vehicle |
jdawkins | 3:ec0efa222dfa | 212 | Steer.write((int)((str_cmd*500.0)+1500.0)); |
jdawkins | 3:ec0efa222dfa | 213 | Throttle.write((int)((thr_cmd*500.0)+1500.0)); |
jdawkins | 3:ec0efa222dfa | 214 | |
jdawkins | 3:ec0efa222dfa | 215 | if(!thr_cond) { // If the throttle is moved away from neutral switch to manual control |
jdawkins | 3:ec0efa222dfa | 216 | auto_ctrl = false; |
jdawkins | 3:ec0efa222dfa | 217 | run_ctrl = false; |
jdawkins | 3:ec0efa222dfa | 218 | } |
jdawkins | 3:ec0efa222dfa | 219 | } // End if auto_ctrl |
Sissors | 0:bd0546063b0a | 220 | |
jdawkins | 3:ec0efa222dfa | 221 | else { // Armed but in Manual Control |
jdawkins | 3:ec0efa222dfa | 222 | |
jdawkins | 3:ec0efa222dfa | 223 | if(xbee.readable()) { |
jdawkins | 3:ec0efa222dfa | 224 | t_run = t.read(); |
jdawkins | 3:ec0efa222dfa | 225 | log_data = !log_data; |
jdawkins | 3:ec0efa222dfa | 226 | } |
jdawkins | 3:ec0efa222dfa | 227 | str_cmd = ((CH1.servoPulse-1500.0)/500.0); // Convert Pulse to Normalized Command +/- 1 |
jdawkins | 3:ec0efa222dfa | 228 | thr_cmd = ((CH2.servoPulse-1500.0)/500.0); // Convert Pulse to Normalized Command +/- 1 |
jdawkins | 3:ec0efa222dfa | 229 | Steer.write((int)((str_cmd*500.0)+1500.0)); // Write Steering Pulse |
jdawkins | 3:ec0efa222dfa | 230 | Throttle.write((int)((thr_cmd*500.0)+1500.0)); //Write Throttle Pulse |
jdawkins | 3:ec0efa222dfa | 231 | |
jdawkins | 3:ec0efa222dfa | 232 | auto_LED = 0; |
jdawkins | 3:ec0efa222dfa | 233 | } // end if autocontrol |
jdawkins | 3:ec0efa222dfa | 234 | |
piper | 4:ec2978ff7a1e | 235 | } else { // System is not aArmed |
jdawkins | 3:ec0efa222dfa | 236 | armed_LED = 0; //Turn off armed LED indicator |
jdawkins | 3:ec0efa222dfa | 237 | str_cmd = 0; |
jdawkins | 3:ec0efa222dfa | 238 | thr_cmd = 0; |
jdawkins | 3:ec0efa222dfa | 239 | Steer.write(1500); //Set Steer PWM to center 1000-2000 range |
jdawkins | 3:ec0efa222dfa | 240 | Throttle.write(1500); //Set Throttle to Low |
jdawkins | 3:ec0efa222dfa | 241 | }/// end else armed |
jdawkins | 3:ec0efa222dfa | 242 | |
piper | 4:ec2978ff7a1e | 243 | |
piper | 4:ec2978ff7a1e | 244 | // *************************************** |
piper | 4:ec2978ff7a1e | 245 | // Transmit data to ground station |
piper | 4:ec2978ff7a1e | 246 | // *************************************** |
piper | 4:ec2978ff7a1e | 247 | if(t.read()-t_log > (1/LOG_RATE)) { |
jdawkins | 3:ec0efa222dfa | 248 | if(log_data) { |
piper | 5:03074e90ef7a | 249 | xbee.printf("%.3f, %.3f, %.3f, %.3f, %.3f, %.3f\r\n",t.read()-t_run,speed, distance, thr_cmd, str_cmd, imu.gyro.z); |
jdawkins | 3:ec0efa222dfa | 250 | } |
jdawkins | 3:ec0efa222dfa | 251 | t_log = t.read(); |
piper | 4:ec2978ff7a1e | 252 | } // End transmit data |
piper | 4:ec2978ff7a1e | 253 | |
piper | 4:ec2978ff7a1e | 254 | dt = t.read()-t_loop; // Sample period (sec) |
piper | 4:ec2978ff7a1e | 255 | t_loop = t.read(); |
piper | 4:ec2978ff7a1e | 256 | |
jdawkins | 3:ec0efa222dfa | 257 | wait(1/LOOP_RATE); |
jdawkins | 3:ec0efa222dfa | 258 | status_LED=!status_LED; |
piper | 4:ec2978ff7a1e | 259 | |
piper | 4:ec2978ff7a1e | 260 | } // End control loop |
piper | 4:ec2978ff7a1e | 261 | |
piper | 4:ec2978ff7a1e | 262 | } // End main |
piper | 4:ec2978ff7a1e | 263 | |
Sissors | 0:bd0546063b0a | 264 | |
piper | 4:ec2978ff7a1e | 265 | void he_callback() |
piper | 4:ec2978ff7a1e | 266 | { |
piper | 4:ec2978ff7a1e | 267 | // Hall effect speed sensor callback |
piper | 4:ec2978ff7a1e | 268 | hall_LED = !hall_LED; |
piper | 4:ec2978ff7a1e | 269 | |
piper | 4:ec2978ff7a1e | 270 | dt_hall = t.read()-t_hall; |
piper | 4:ec2978ff7a1e | 271 | t_hall = t.read(); |
Sissors | 0:bd0546063b0a | 272 | } |
jdawkins | 3:ec0efa222dfa | 273 | |
piper | 5:03074e90ef7a | 274 | |
jdawkins | 3:ec0efa222dfa | 275 | void menuFunction(Serial *port){ |
jdawkins | 3:ec0efa222dfa | 276 | if(port->readable()) { |
jdawkins | 3:ec0efa222dfa | 277 | char c = port->getc(); |
jdawkins | 3:ec0efa222dfa | 278 | if(c == 'H' || c == 'h') { |
jdawkins | 3:ec0efa222dfa | 279 | port->printf("Command List...\r\n"); |
jdawkins | 3:ec0efa222dfa | 280 | port->printf("a - set autonomous mode\r\n"); |
jdawkins | 3:ec0efa222dfa | 281 | port->printf("l - toggle data logging\r\n"); |
jdawkins | 3:ec0efa222dfa | 282 | port->printf("r - run auto control loop\r\n"); |
jdawkins | 3:ec0efa222dfa | 283 | port->printf("h - display this prompt and exit\r\n"); |
jdawkins | 3:ec0efa222dfa | 284 | port->printf("Enter Command\r\n"); |
jdawkins | 3:ec0efa222dfa | 285 | } |
jdawkins | 3:ec0efa222dfa | 286 | if(c == 'A' || c == 'a') { |
jdawkins | 3:ec0efa222dfa | 287 | auto_ctrl = true; |
jdawkins | 3:ec0efa222dfa | 288 | Steer.write((int)(1500.0)); |
jdawkins | 3:ec0efa222dfa | 289 | } |
jdawkins | 3:ec0efa222dfa | 290 | |
jdawkins | 3:ec0efa222dfa | 291 | if(c == 'R' || c == 'r') { |
jdawkins | 3:ec0efa222dfa | 292 | auto_ctrl = true; |
jdawkins | 3:ec0efa222dfa | 293 | Steer.write((int)(1500.0)); |
jdawkins | 3:ec0efa222dfa | 294 | |
jdawkins | 3:ec0efa222dfa | 295 | if(!log_data) { |
jdawkins | 3:ec0efa222dfa | 296 | t_log_start = t.read(); |
jdawkins | 3:ec0efa222dfa | 297 | t_run = t.read(); |
jdawkins | 3:ec0efa222dfa | 298 | xbee.printf("\r\n\r\n"); |
jdawkins | 3:ec0efa222dfa | 299 | } |
jdawkins | 3:ec0efa222dfa | 300 | log_data = !log_data; |
jdawkins | 3:ec0efa222dfa | 301 | run_ctrl = true; |
jdawkins | 3:ec0efa222dfa | 302 | } |
jdawkins | 3:ec0efa222dfa | 303 | |
jdawkins | 3:ec0efa222dfa | 304 | } |
jdawkins | 3:ec0efa222dfa | 305 | |
jdawkins | 3:ec0efa222dfa | 306 | } |
piper | 4:ec2978ff7a1e | 307 | |
piper | 4:ec2978ff7a1e | 308 | |
piper | 4:ec2978ff7a1e | 309 | |
jdawkins | 3:ec0efa222dfa | 310 |