![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
UCR Robosub manual control / PID tuning interface
main.cpp@0:048a74dd7c3a, 2017-07-22 (annotated)
- Committer:
- roger_wee
- Date:
- Sat Jul 22 05:58:03 2017 +0000
- Revision:
- 0:048a74dd7c3a
- Child:
- 1:3f291f2f80d3
UCR Robosub Manual Control / PID tuning interface
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
roger_wee | 0:048a74dd7c3a | 1 | //Robosub Control Interface |
roger_wee | 0:048a74dd7c3a | 2 | |
roger_wee | 0:048a74dd7c3a | 3 | #include "mbed.h" |
roger_wee | 0:048a74dd7c3a | 4 | #include "IMU.h" |
roger_wee | 0:048a74dd7c3a | 5 | #include "PID.h" |
roger_wee | 0:048a74dd7c3a | 6 | #include "HMC5883L.h" |
roger_wee | 0:048a74dd7c3a | 7 | |
roger_wee | 0:048a74dd7c3a | 8 | //------------Declare Objects------------------// |
roger_wee | 0:048a74dd7c3a | 9 | PwmOut m1(D3); |
roger_wee | 0:048a74dd7c3a | 10 | PwmOut m2(D4); |
roger_wee | 0:048a74dd7c3a | 11 | PwmOut m3(D5); |
roger_wee | 0:048a74dd7c3a | 12 | PwmOut m4(D6); |
roger_wee | 0:048a74dd7c3a | 13 | |
roger_wee | 0:048a74dd7c3a | 14 | // Declare mpu6050 and compass object |
roger_wee | 0:048a74dd7c3a | 15 | MPU6050 mpu1; |
roger_wee | 0:048a74dd7c3a | 16 | HMC5883L compass; |
roger_wee | 0:048a74dd7c3a | 17 | |
roger_wee | 0:048a74dd7c3a | 18 | // Serial communication between Arduino NANO |
roger_wee | 0:048a74dd7c3a | 19 | RawSerial device(PA_11, PA_12); //TX RX |
roger_wee | 0:048a74dd7c3a | 20 | |
roger_wee | 0:048a74dd7c3a | 21 | //-----------Global Variables------------------// |
roger_wee | 0:048a74dd7c3a | 22 | char command = 0; |
roger_wee | 0:048a74dd7c3a | 23 | Timer calibrate; |
roger_wee | 0:048a74dd7c3a | 24 | |
roger_wee | 0:048a74dd7c3a | 25 | int pwmMax = 1100; // Reversed due to motor orientation |
roger_wee | 0:048a74dd7c3a | 26 | int pwmMin = 1500; |
roger_wee | 0:048a74dd7c3a | 27 | |
roger_wee | 0:048a74dd7c3a | 28 | // PWM output variable for each motor |
roger_wee | 0:048a74dd7c3a | 29 | int m1pwm = pwmMin; |
roger_wee | 0:048a74dd7c3a | 30 | int m2pwm = pwmMin; |
roger_wee | 0:048a74dd7c3a | 31 | int m3pwm = pwmMin; |
roger_wee | 0:048a74dd7c3a | 32 | int m4pwm = pwmMin; |
roger_wee | 0:048a74dd7c3a | 33 | |
roger_wee | 0:048a74dd7c3a | 34 | // Individual pid parameters |
roger_wee | 0:048a74dd7c3a | 35 | double myYaw, yawPoint, yawOut; |
roger_wee | 0:048a74dd7c3a | 36 | double myPitch, pitchPoint, pitchOut; |
roger_wee | 0:048a74dd7c3a | 37 | double myRoll, rollPoint, rollOut; |
roger_wee | 0:048a74dd7c3a | 38 | double myDepth, depthPoint, depthOut; |
roger_wee | 0:048a74dd7c3a | 39 | |
roger_wee | 0:048a74dd7c3a | 40 | int depth = 1; |
roger_wee | 0:048a74dd7c3a | 41 | |
roger_wee | 0:048a74dd7c3a | 42 | double kpVal = 0; |
roger_wee | 0:048a74dd7c3a | 43 | |
roger_wee | 0:048a74dd7c3a | 44 | //-----------End Global Variables--------------// |
roger_wee | 0:048a74dd7c3a | 45 | |
roger_wee | 0:048a74dd7c3a | 46 | |
roger_wee | 0:048a74dd7c3a | 47 | //----PID objects------// |
roger_wee | 0:048a74dd7c3a | 48 | |
roger_wee | 0:048a74dd7c3a | 49 | // Input, Output, SetPoint, kp, ki, kd, Controller Direction |
roger_wee | 0:048a74dd7c3a | 50 | PID pidy(&myYaw, &yawOut, &yawPoint, 1, 1, 1, DIRECT); |
roger_wee | 0:048a74dd7c3a | 51 | PID pidp(&myPitch, &pitchOut, &pitchPoint, 1, 1, 1, DIRECT); |
roger_wee | 0:048a74dd7c3a | 52 | PID pidr(&myRoll, &rollOut, &rollPoint, 1, 1, 1, DIRECT); |
roger_wee | 0:048a74dd7c3a | 53 | PID pidd(&myDepth, &depthOut, &depthPoint, 1, 1, 1, DIRECT); |
roger_wee | 0:048a74dd7c3a | 54 | |
roger_wee | 0:048a74dd7c3a | 55 | //----End PID objects--// |
roger_wee | 0:048a74dd7c3a | 56 | |
roger_wee | 0:048a74dd7c3a | 57 | |
roger_wee | 0:048a74dd7c3a | 58 | //-----------Helper functions------------------// |
roger_wee | 0:048a74dd7c3a | 59 | void calibrateFilter() |
roger_wee | 0:048a74dd7c3a | 60 | { |
roger_wee | 0:048a74dd7c3a | 61 | calibrate.start(); |
roger_wee | 0:048a74dd7c3a | 62 | while(calibrate.read() < 10) |
roger_wee | 0:048a74dd7c3a | 63 | { |
roger_wee | 0:048a74dd7c3a | 64 | IMUPrintData(mpu1, compass); |
roger_wee | 0:048a74dd7c3a | 65 | |
roger_wee | 0:048a74dd7c3a | 66 | char text[90]; |
roger_wee | 0:048a74dd7c3a | 67 | sprintf(text, "%f,%f,%f \n", yaw, pitch, roll); |
roger_wee | 0:048a74dd7c3a | 68 | pc.printf("%s", text); |
roger_wee | 0:048a74dd7c3a | 69 | } |
roger_wee | 0:048a74dd7c3a | 70 | calibrate.stop(); |
roger_wee | 0:048a74dd7c3a | 71 | } |
roger_wee | 0:048a74dd7c3a | 72 | |
roger_wee | 0:048a74dd7c3a | 73 | void updateMotors() |
roger_wee | 0:048a74dd7c3a | 74 | { |
roger_wee | 0:048a74dd7c3a | 75 | m1.pulsewidth_us(m1pwm); |
roger_wee | 0:048a74dd7c3a | 76 | m2.pulsewidth_us(m2pwm); |
roger_wee | 0:048a74dd7c3a | 77 | m3.pulsewidth_us(m3pwm); |
roger_wee | 0:048a74dd7c3a | 78 | m4.pulsewidth_us(m4pwm); |
roger_wee | 0:048a74dd7c3a | 79 | } |
roger_wee | 0:048a74dd7c3a | 80 | |
roger_wee | 0:048a74dd7c3a | 81 | void neutralizeMotors() |
roger_wee | 0:048a74dd7c3a | 82 | { |
roger_wee | 0:048a74dd7c3a | 83 | m1pwm = pwmMin; |
roger_wee | 0:048a74dd7c3a | 84 | m2pwm = pwmMin; |
roger_wee | 0:048a74dd7c3a | 85 | m3pwm = pwmMin; |
roger_wee | 0:048a74dd7c3a | 86 | m4pwm = pwmMin; |
roger_wee | 0:048a74dd7c3a | 87 | updateMotors(); |
roger_wee | 0:048a74dd7c3a | 88 | } |
roger_wee | 0:048a74dd7c3a | 89 | |
roger_wee | 0:048a74dd7c3a | 90 | void readUserInput() |
roger_wee | 0:048a74dd7c3a | 91 | { |
roger_wee | 0:048a74dd7c3a | 92 | if(pc.readable()) |
roger_wee | 0:048a74dd7c3a | 93 | { |
roger_wee | 0:048a74dd7c3a | 94 | command = pc.getc(); |
roger_wee | 0:048a74dd7c3a | 95 | } |
roger_wee | 0:048a74dd7c3a | 96 | } |
roger_wee | 0:048a74dd7c3a | 97 | |
roger_wee | 0:048a74dd7c3a | 98 | void initializePIDs() |
roger_wee | 0:048a74dd7c3a | 99 | { |
roger_wee | 0:048a74dd7c3a | 100 | pidy.SetMode(AUTOMATIC); // Yaw PID |
roger_wee | 0:048a74dd7c3a | 101 | pidy.SetOutputLimits(pwmMin, pwmMax); |
roger_wee | 0:048a74dd7c3a | 102 | |
roger_wee | 0:048a74dd7c3a | 103 | pidp.SetMode(AUTOMATIC); // Pitch PID |
roger_wee | 0:048a74dd7c3a | 104 | pidp.SetOutputLimits(pwmMin, pwmMax); |
roger_wee | 0:048a74dd7c3a | 105 | |
roger_wee | 0:048a74dd7c3a | 106 | pidr.SetMode(AUTOMATIC); // Roll PID |
roger_wee | 0:048a74dd7c3a | 107 | pidr.SetOutputLimits(pwmMin, pwmMax); |
roger_wee | 0:048a74dd7c3a | 108 | |
roger_wee | 0:048a74dd7c3a | 109 | pidd.SetMode(AUTOMATIC); // Depth PID |
roger_wee | 0:048a74dd7c3a | 110 | pidd.SetOutputLimits(pwmMin, pwmMax); |
roger_wee | 0:048a74dd7c3a | 111 | |
roger_wee | 0:048a74dd7c3a | 112 | depthPoint = 0; |
roger_wee | 0:048a74dd7c3a | 113 | } |
roger_wee | 0:048a74dd7c3a | 114 | |
roger_wee | 0:048a74dd7c3a | 115 | void readDepth() |
roger_wee | 0:048a74dd7c3a | 116 | { |
roger_wee | 0:048a74dd7c3a | 117 | // Read pressure sensor data if available |
roger_wee | 0:048a74dd7c3a | 118 | if (device.readable()) |
roger_wee | 0:048a74dd7c3a | 119 | { |
roger_wee | 0:048a74dd7c3a | 120 | // Receive depth in inches as an integer |
roger_wee | 0:048a74dd7c3a | 121 | depth = device.getc(); |
roger_wee | 0:048a74dd7c3a | 122 | |
roger_wee | 0:048a74dd7c3a | 123 | // Convert to feet |
roger_wee | 0:048a74dd7c3a | 124 | } |
roger_wee | 0:048a74dd7c3a | 125 | } |
roger_wee | 0:048a74dd7c3a | 126 | |
roger_wee | 0:048a74dd7c3a | 127 | void displayTelemetry() |
roger_wee | 0:048a74dd7c3a | 128 | { |
roger_wee | 0:048a74dd7c3a | 129 | char text[90]; |
roger_wee | 0:048a74dd7c3a | 130 | sprintf(text, "%f,%f,%f,%d \n", yaw, pitch, roll, depth); |
roger_wee | 0:048a74dd7c3a | 131 | pc.printf("%s", text); |
roger_wee | 0:048a74dd7c3a | 132 | } |
roger_wee | 0:048a74dd7c3a | 133 | |
roger_wee | 0:048a74dd7c3a | 134 | //-----------End Helper functions--------------// |
roger_wee | 0:048a74dd7c3a | 135 | |
roger_wee | 0:048a74dd7c3a | 136 | |
roger_wee | 0:048a74dd7c3a | 137 | |
roger_wee | 0:048a74dd7c3a | 138 | //-----------Interface States------------------// |
roger_wee | 0:048a74dd7c3a | 139 | enum controlStates { init, idle, manual, preparePid, beginTune } controlState; |
roger_wee | 0:048a74dd7c3a | 140 | |
roger_wee | 0:048a74dd7c3a | 141 | void controlInterface(){ |
roger_wee | 0:048a74dd7c3a | 142 | |
roger_wee | 0:048a74dd7c3a | 143 | switch(controlState) //Actions |
roger_wee | 0:048a74dd7c3a | 144 | { |
roger_wee | 0:048a74dd7c3a | 145 | case init: |
roger_wee | 0:048a74dd7c3a | 146 | pc.printf("Initialize sensors \n"); |
roger_wee | 0:048a74dd7c3a | 147 | IMUinit(mpu1); |
roger_wee | 0:048a74dd7c3a | 148 | compass.init(); |
roger_wee | 0:048a74dd7c3a | 149 | |
roger_wee | 0:048a74dd7c3a | 150 | pc.printf("Initialize motors \n"); |
roger_wee | 0:048a74dd7c3a | 151 | neutralizeMotors(); |
roger_wee | 0:048a74dd7c3a | 152 | |
roger_wee | 0:048a74dd7c3a | 153 | pc.printf("Initialize PID objects \n"); |
roger_wee | 0:048a74dd7c3a | 154 | initializePIDs(); |
roger_wee | 0:048a74dd7c3a | 155 | |
roger_wee | 0:048a74dd7c3a | 156 | pc.printf("Calibrate MARG Filter \n"); |
roger_wee | 0:048a74dd7c3a | 157 | calibrateFilter(); |
roger_wee | 0:048a74dd7c3a | 158 | break; |
roger_wee | 0:048a74dd7c3a | 159 | |
roger_wee | 0:048a74dd7c3a | 160 | case idle: |
roger_wee | 0:048a74dd7c3a | 161 | IMUPrintData(mpu1, compass); |
roger_wee | 0:048a74dd7c3a | 162 | readDepth(); |
roger_wee | 0:048a74dd7c3a | 163 | displayTelemetry(); |
roger_wee | 0:048a74dd7c3a | 164 | break; |
roger_wee | 0:048a74dd7c3a | 165 | |
roger_wee | 0:048a74dd7c3a | 166 | case manual: |
roger_wee | 0:048a74dd7c3a | 167 | //pc.printf("Manual Control \n"); |
roger_wee | 0:048a74dd7c3a | 168 | switch(command) |
roger_wee | 0:048a74dd7c3a | 169 | { |
roger_wee | 0:048a74dd7c3a | 170 | case 'N': |
roger_wee | 0:048a74dd7c3a | 171 | //pc.printf("Neutralize motors\n"); |
roger_wee | 0:048a74dd7c3a | 172 | neutralizeMotors(); |
roger_wee | 0:048a74dd7c3a | 173 | break; |
roger_wee | 0:048a74dd7c3a | 174 | |
roger_wee | 0:048a74dd7c3a | 175 | case 'R': |
roger_wee | 0:048a74dd7c3a | 176 | //pc.printf("Reduce Thrust\n"); |
roger_wee | 0:048a74dd7c3a | 177 | if (m1pwm < pwmMin) |
roger_wee | 0:048a74dd7c3a | 178 | { |
roger_wee | 0:048a74dd7c3a | 179 | m1pwm++; |
roger_wee | 0:048a74dd7c3a | 180 | m2pwm++; |
roger_wee | 0:048a74dd7c3a | 181 | m3pwm++; |
roger_wee | 0:048a74dd7c3a | 182 | m4pwm++; |
roger_wee | 0:048a74dd7c3a | 183 | } |
roger_wee | 0:048a74dd7c3a | 184 | break; |
roger_wee | 0:048a74dd7c3a | 185 | |
roger_wee | 0:048a74dd7c3a | 186 | case 'I': |
roger_wee | 0:048a74dd7c3a | 187 | //pc.printf("Increase Thrust\n"); |
roger_wee | 0:048a74dd7c3a | 188 | if (m1pwm > pwmMax) |
roger_wee | 0:048a74dd7c3a | 189 | { |
roger_wee | 0:048a74dd7c3a | 190 | m1pwm--; |
roger_wee | 0:048a74dd7c3a | 191 | m2pwm--; |
roger_wee | 0:048a74dd7c3a | 192 | m3pwm--; |
roger_wee | 0:048a74dd7c3a | 193 | m4pwm--; |
roger_wee | 0:048a74dd7c3a | 194 | } |
roger_wee | 0:048a74dd7c3a | 195 | break; |
roger_wee | 0:048a74dd7c3a | 196 | |
roger_wee | 0:048a74dd7c3a | 197 | default: |
roger_wee | 0:048a74dd7c3a | 198 | break; |
roger_wee | 0:048a74dd7c3a | 199 | } |
roger_wee | 0:048a74dd7c3a | 200 | updateMotors(); |
roger_wee | 0:048a74dd7c3a | 201 | break; |
roger_wee | 0:048a74dd7c3a | 202 | |
roger_wee | 0:048a74dd7c3a | 203 | case preparePid: |
roger_wee | 0:048a74dd7c3a | 204 | // pc.printf("Prepare PID \n"); |
roger_wee | 0:048a74dd7c3a | 205 | switch(command) |
roger_wee | 0:048a74dd7c3a | 206 | { |
roger_wee | 0:048a74dd7c3a | 207 | case 'I': // Increase Kp gain |
roger_wee | 0:048a74dd7c3a | 208 | kpVal += .1; |
roger_wee | 0:048a74dd7c3a | 209 | break; |
roger_wee | 0:048a74dd7c3a | 210 | |
roger_wee | 0:048a74dd7c3a | 211 | case 'R': |
roger_wee | 0:048a74dd7c3a | 212 | if (kpVal > 0 ) // Decreae Kp gain |
roger_wee | 0:048a74dd7c3a | 213 | { |
roger_wee | 0:048a74dd7c3a | 214 | kpVal -= .1; |
roger_wee | 0:048a74dd7c3a | 215 | } |
roger_wee | 0:048a74dd7c3a | 216 | break; |
roger_wee | 0:048a74dd7c3a | 217 | |
roger_wee | 0:048a74dd7c3a | 218 | case 'S': // Increase depth |
roger_wee | 0:048a74dd7c3a | 219 | depthPoint++; |
roger_wee | 0:048a74dd7c3a | 220 | |
roger_wee | 0:048a74dd7c3a | 221 | case 's': // Decrease setpoint |
roger_wee | 0:048a74dd7c3a | 222 | if(depthPoint > 0) |
roger_wee | 0:048a74dd7c3a | 223 | { |
roger_wee | 0:048a74dd7c3a | 224 | depthPoint--; |
roger_wee | 0:048a74dd7c3a | 225 | } |
roger_wee | 0:048a74dd7c3a | 226 | break; |
roger_wee | 0:048a74dd7c3a | 227 | |
roger_wee | 0:048a74dd7c3a | 228 | default: |
roger_wee | 0:048a74dd7c3a | 229 | break; |
roger_wee | 0:048a74dd7c3a | 230 | } |
roger_wee | 0:048a74dd7c3a | 231 | |
roger_wee | 0:048a74dd7c3a | 232 | // Set tunings |
roger_wee | 0:048a74dd7c3a | 233 | pidd.SetTunings(kpVal,0,0); //Set Ki Kd = 0 |
roger_wee | 0:048a74dd7c3a | 234 | |
roger_wee | 0:048a74dd7c3a | 235 | // Print Set parameters |
roger_wee | 0:048a74dd7c3a | 236 | pc.printf("DepthPoint: %d \n", depthPoint); |
roger_wee | 0:048a74dd7c3a | 237 | pc.printf("Kp gain: %d \n", pidd.GetKp()); |
roger_wee | 0:048a74dd7c3a | 238 | |
roger_wee | 0:048a74dd7c3a | 239 | break; |
roger_wee | 0:048a74dd7c3a | 240 | |
roger_wee | 0:048a74dd7c3a | 241 | case beginTune: |
roger_wee | 0:048a74dd7c3a | 242 | //pc.printf("Tuning process begins.. \n"); |
roger_wee | 0:048a74dd7c3a | 243 | |
roger_wee | 0:048a74dd7c3a | 244 | // Sense inertial / depth data |
roger_wee | 0:048a74dd7c3a | 245 | IMUPrintData(mpu1, compass); |
roger_wee | 0:048a74dd7c3a | 246 | readDepth(); |
roger_wee | 0:048a74dd7c3a | 247 | |
roger_wee | 0:048a74dd7c3a | 248 | // Assign feedback variables |
roger_wee | 0:048a74dd7c3a | 249 | myDepth = depth; |
roger_wee | 0:048a74dd7c3a | 250 | |
roger_wee | 0:048a74dd7c3a | 251 | // Compute PID |
roger_wee | 0:048a74dd7c3a | 252 | pidd.Compute(); |
roger_wee | 0:048a74dd7c3a | 253 | |
roger_wee | 0:048a74dd7c3a | 254 | // Output pwm to motors // FIXME : account for pitch, roll and maybe yaw depending on motor configuration |
roger_wee | 0:048a74dd7c3a | 255 | m1pwm = m2pwm = m3pwm = m4pwm = depthOut; |
roger_wee | 0:048a74dd7c3a | 256 | updateMotors(); |
roger_wee | 0:048a74dd7c3a | 257 | |
roger_wee | 0:048a74dd7c3a | 258 | // Display telemetry |
roger_wee | 0:048a74dd7c3a | 259 | displayTelemetry(); |
roger_wee | 0:048a74dd7c3a | 260 | |
roger_wee | 0:048a74dd7c3a | 261 | break; |
roger_wee | 0:048a74dd7c3a | 262 | |
roger_wee | 0:048a74dd7c3a | 263 | default: |
roger_wee | 0:048a74dd7c3a | 264 | pc.printf("Error! \n"); |
roger_wee | 0:048a74dd7c3a | 265 | break; |
roger_wee | 0:048a74dd7c3a | 266 | } |
roger_wee | 0:048a74dd7c3a | 267 | |
roger_wee | 0:048a74dd7c3a | 268 | switch(controlState) //Transitions |
roger_wee | 0:048a74dd7c3a | 269 | { |
roger_wee | 0:048a74dd7c3a | 270 | case init: |
roger_wee | 0:048a74dd7c3a | 271 | controlState = idle; |
roger_wee | 0:048a74dd7c3a | 272 | break; |
roger_wee | 0:048a74dd7c3a | 273 | |
roger_wee | 0:048a74dd7c3a | 274 | case idle: |
roger_wee | 0:048a74dd7c3a | 275 | controlState = (command == 'm') ? manual : ((command == 'p') ? preparePid : idle); |
roger_wee | 0:048a74dd7c3a | 276 | break; |
roger_wee | 0:048a74dd7c3a | 277 | |
roger_wee | 0:048a74dd7c3a | 278 | case manual: |
roger_wee | 0:048a74dd7c3a | 279 | controlState = (command == 'p') ? preparePid : manual; |
roger_wee | 0:048a74dd7c3a | 280 | if (controlState == preparePid) |
roger_wee | 0:048a74dd7c3a | 281 | { |
roger_wee | 0:048a74dd7c3a | 282 | //pc.printf("Neutralize Motors \n"); |
roger_wee | 0:048a74dd7c3a | 283 | neutralizeMotors(); |
roger_wee | 0:048a74dd7c3a | 284 | } |
roger_wee | 0:048a74dd7c3a | 285 | break; |
roger_wee | 0:048a74dd7c3a | 286 | |
roger_wee | 0:048a74dd7c3a | 287 | case preparePid: |
roger_wee | 0:048a74dd7c3a | 288 | controlState = (command == 'g') ? beginTune : preparePid; |
roger_wee | 0:048a74dd7c3a | 289 | break; |
roger_wee | 0:048a74dd7c3a | 290 | |
roger_wee | 0:048a74dd7c3a | 291 | case beginTune: |
roger_wee | 0:048a74dd7c3a | 292 | controlState = (command == 't') ? preparePid : beginTune; |
roger_wee | 0:048a74dd7c3a | 293 | if (controlState == preparePid) |
roger_wee | 0:048a74dd7c3a | 294 | { |
roger_wee | 0:048a74dd7c3a | 295 | //pc.printf("Neutralize Motors \n"); |
roger_wee | 0:048a74dd7c3a | 296 | neutralizeMotors(); |
roger_wee | 0:048a74dd7c3a | 297 | } |
roger_wee | 0:048a74dd7c3a | 298 | break; |
roger_wee | 0:048a74dd7c3a | 299 | |
roger_wee | 0:048a74dd7c3a | 300 | default: |
roger_wee | 0:048a74dd7c3a | 301 | break; |
roger_wee | 0:048a74dd7c3a | 302 | } |
roger_wee | 0:048a74dd7c3a | 303 | } |
roger_wee | 0:048a74dd7c3a | 304 | |
roger_wee | 0:048a74dd7c3a | 305 | //-----------End Interface---------------------// |
roger_wee | 0:048a74dd7c3a | 306 | |
roger_wee | 0:048a74dd7c3a | 307 | int main() { |
roger_wee | 0:048a74dd7c3a | 308 | // Initialize control state |
roger_wee | 0:048a74dd7c3a | 309 | controlState = init; |
roger_wee | 0:048a74dd7c3a | 310 | |
roger_wee | 0:048a74dd7c3a | 311 | while(1) |
roger_wee | 0:048a74dd7c3a | 312 | { |
roger_wee | 0:048a74dd7c3a | 313 | // Read user input |
roger_wee | 0:048a74dd7c3a | 314 | readUserInput(); |
roger_wee | 0:048a74dd7c3a | 315 | |
roger_wee | 0:048a74dd7c3a | 316 | // Begin Interface |
roger_wee | 0:048a74dd7c3a | 317 | controlInterface(); |
roger_wee | 0:048a74dd7c3a | 318 | } |
roger_wee | 0:048a74dd7c3a | 319 | } |