Rogelio Vazquez / Mbed 2 deprecated Control_interface

UCR Robosub manual control / PID tuning interface

Dependencies:   mbed HMC5883L

main.cpp@1:3f291f2f80d3, 2017-07-27 (annotated)

Committer:
roger_wee
Date:
Thu Jul 27 05:45:10 2017 +0000
Revision:
1:3f291f2f80d3
Parent:
0:048a74dd7c3a 
control edit

Who changed what in which revision?

UserRevisionLine numberNew 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 1:3f291f2f80d3 9
roger_wee 1:3f291f2f80d3 10 // (m1) (m2)
roger_wee 1:3f291f2f80d3 11 // | |
roger_wee 1:3f291f2f80d3 12 // | |
roger_wee 1:3f291f2f80d3 13 // (Ltrhust)| |(Rthrust)
roger_wee 1:3f291f2f80d3 14 // | |
roger_wee 1:3f291f2f80d3 15 // | |
roger_wee 1:3f291f2f80d3 16 // (m3) (m4)
roger_wee 1:3f291f2f80d3 17
roger_wee 0:048a74dd7c3a 18 PwmOut m1(D3);
roger_wee 0:048a74dd7c3a 19 PwmOut m2(D4);
roger_wee 0:048a74dd7c3a 20 PwmOut m3(D5);
roger_wee 0:048a74dd7c3a 21 PwmOut m4(D6);
roger_wee 0:048a74dd7c3a 22
roger_wee 1:3f291f2f80d3 23 PwmOut Lthrust(D7);
roger_wee 1:3f291f2f80d3 24 PwmOut Rthrust(D8);
roger_wee 1:3f291f2f80d3 25
roger_wee 0:048a74dd7c3a 26 // Declare mpu6050 and compass object
roger_wee 0:048a74dd7c3a 27 MPU6050 mpu1;
roger_wee 0:048a74dd7c3a 28 HMC5883L compass;
roger_wee 0:048a74dd7c3a 29
roger_wee 1:3f291f2f80d3 30 // Serial communication between STM & Arduino NANO
roger_wee 0:048a74dd7c3a 31 RawSerial device(PA_11, PA_12); //TX RX
roger_wee 0:048a74dd7c3a 32
roger_wee 1:3f291f2f80d3 33 // Serial communication between STM & Raspberry PI
roger_wee 1:3f291f2f80d3 34 RawSerial devicePI(D10,D2);
roger_wee 1:3f291f2f80d3 35
roger_wee 0:048a74dd7c3a 36 //-----------Global Variables------------------//
roger_wee 0:048a74dd7c3a 37 char command = 0;
roger_wee 0:048a74dd7c3a 38 Timer calibrate;
roger_wee 1:3f291f2f80d3 39 Timer data;
roger_wee 0:048a74dd7c3a 40
roger_wee 0:048a74dd7c3a 41 int pwmMax = 1100; // Reversed due to motor orientation
roger_wee 0:048a74dd7c3a 42 int pwmMin = 1500;
roger_wee 0:048a74dd7c3a 43
roger_wee 1:3f291f2f80d3 44 const int IDLE = 1500;
roger_wee 1:3f291f2f80d3 45 const int MAXYAW = 100;
roger_wee 1:3f291f2f80d3 46 const int MAXPITCH = 100;
roger_wee 1:3f291f2f80d3 47 const int MAXROLL = 100;
roger_wee 1:3f291f2f80d3 48
roger_wee 1:3f291f2f80d3 49 int thrust = 0;
roger_wee 1:3f291f2f80d3 50
roger_wee 1:3f291f2f80d3 51 int j = -1; // j == 1 --> FWD ... j == -1 --> REV
roger_wee 1:3f291f2f80d3 52
roger_wee 1:3f291f2f80d3 53
roger_wee 0:048a74dd7c3a 54 // PWM output variable for each motor
roger_wee 0:048a74dd7c3a 55 int m1pwm = pwmMin;
roger_wee 0:048a74dd7c3a 56 int m2pwm = pwmMin;
roger_wee 0:048a74dd7c3a 57 int m3pwm = pwmMin;
roger_wee 0:048a74dd7c3a 58 int m4pwm = pwmMin;
roger_wee 1:3f291f2f80d3 59 int Lthrustpwm = pwmMin;
roger_wee 1:3f291f2f80d3 60 int Rthrustpwm = pwmMin;
roger_wee 0:048a74dd7c3a 61
roger_wee 0:048a74dd7c3a 62 // Individual pid parameters
roger_wee 0:048a74dd7c3a 63 double myYaw, yawPoint, yawOut;
roger_wee 0:048a74dd7c3a 64 double myPitch, pitchPoint, pitchOut;
roger_wee 0:048a74dd7c3a 65 double myRoll, rollPoint, rollOut;
roger_wee 0:048a74dd7c3a 66 double myDepth, depthPoint, depthOut;
roger_wee 0:048a74dd7c3a 67
roger_wee 0:048a74dd7c3a 68 int depth = 1;
roger_wee 0:048a74dd7c3a 69
roger_wee 0:048a74dd7c3a 70 double kpVal = 0;
roger_wee 1:3f291f2f80d3 71 double kiVal = 0;
roger_wee 1:3f291f2f80d3 72 double kdVal = 0;
roger_wee 0:048a74dd7c3a 73
roger_wee 0:048a74dd7c3a 74 //-----------End Global Variables--------------//
roger_wee 0:048a74dd7c3a 75
roger_wee 0:048a74dd7c3a 76
roger_wee 0:048a74dd7c3a 77 //----PID objects------//
roger_wee 0:048a74dd7c3a 78
roger_wee 0:048a74dd7c3a 79 // Input, Output, SetPoint, kp, ki, kd, Controller Direction
roger_wee 0:048a74dd7c3a 80 PID pidy(&myYaw, &yawOut, &yawPoint, 1, 1, 1, DIRECT);
roger_wee 0:048a74dd7c3a 81 PID pidp(&myPitch, &pitchOut, &pitchPoint, 1, 1, 1, DIRECT);
roger_wee 0:048a74dd7c3a 82 PID pidr(&myRoll, &rollOut, &rollPoint, 1, 1, 1, DIRECT);
roger_wee 0:048a74dd7c3a 83 PID pidd(&myDepth, &depthOut, &depthPoint, 1, 1, 1, DIRECT);
roger_wee 0:048a74dd7c3a 84
roger_wee 0:048a74dd7c3a 85 //----End PID objects--//
roger_wee 0:048a74dd7c3a 86
roger_wee 0:048a74dd7c3a 87
roger_wee 0:048a74dd7c3a 88 //-----------Helper functions------------------//
roger_wee 0:048a74dd7c3a 89 void calibrateFilter()
roger_wee 0:048a74dd7c3a 90 {
roger_wee 0:048a74dd7c3a 91 calibrate.start();
roger_wee 1:3f291f2f80d3 92 while(calibrate.read() < 5)
roger_wee 0:048a74dd7c3a 93 {
roger_wee 0:048a74dd7c3a 94 IMUPrintData(mpu1, compass);
roger_wee 0:048a74dd7c3a 95
roger_wee 0:048a74dd7c3a 96 char text[90];
roger_wee 0:048a74dd7c3a 97 sprintf(text, "%f,%f,%f \n", yaw, pitch, roll);
roger_wee 0:048a74dd7c3a 98 pc.printf("%s", text);
roger_wee 0:048a74dd7c3a 99 }
roger_wee 0:048a74dd7c3a 100 calibrate.stop();
roger_wee 0:048a74dd7c3a 101 }
roger_wee 0:048a74dd7c3a 102
roger_wee 0:048a74dd7c3a 103 void updateMotors()
roger_wee 0:048a74dd7c3a 104 {
roger_wee 0:048a74dd7c3a 105 m1.pulsewidth_us(m1pwm);
roger_wee 0:048a74dd7c3a 106 m2.pulsewidth_us(m2pwm);
roger_wee 0:048a74dd7c3a 107 m3.pulsewidth_us(m3pwm);
roger_wee 0:048a74dd7c3a 108 m4.pulsewidth_us(m4pwm);
roger_wee 1:3f291f2f80d3 109 Lthrust.pulsewidth_us(Lthrustpwm);
roger_wee 1:3f291f2f80d3 110 Rthrust.pulsewidth_us(Rthrustpwm);
roger_wee 0:048a74dd7c3a 111 }
roger_wee 0:048a74dd7c3a 112
roger_wee 0:048a74dd7c3a 113 void neutralizeMotors()
roger_wee 0:048a74dd7c3a 114 {
roger_wee 0:048a74dd7c3a 115 m1pwm = pwmMin;
roger_wee 0:048a74dd7c3a 116 m2pwm = pwmMin;
roger_wee 0:048a74dd7c3a 117 m3pwm = pwmMin;
roger_wee 0:048a74dd7c3a 118 m4pwm = pwmMin;
roger_wee 1:3f291f2f80d3 119 Lthrustpwm = pwmMin;
roger_wee 1:3f291f2f80d3 120 Rthrustpwm = pwmMin;
roger_wee 0:048a74dd7c3a 121 updateMotors();
roger_wee 0:048a74dd7c3a 122 }
roger_wee 0:048a74dd7c3a 123
roger_wee 1:3f291f2f80d3 124 void vesselGo()
roger_wee 1:3f291f2f80d3 125 {
roger_wee 1:3f291f2f80d3 126 thrust = 200;
roger_wee 1:3f291f2f80d3 127 }
roger_wee 1:3f291f2f80d3 128 void vesselHover()
roger_wee 1:3f291f2f80d3 129 {
roger_wee 1:3f291f2f80d3 130 thrust = 0;
roger_wee 1:3f291f2f80d3 131 }
roger_wee 1:3f291f2f80d3 132
roger_wee 0:048a74dd7c3a 133 void readUserInput()
roger_wee 0:048a74dd7c3a 134 {
roger_wee 0:048a74dd7c3a 135 if(pc.readable())
roger_wee 0:048a74dd7c3a 136 {
roger_wee 0:048a74dd7c3a 137 command = pc.getc();
roger_wee 0:048a74dd7c3a 138 }
roger_wee 0:048a74dd7c3a 139 }
roger_wee 0:048a74dd7c3a 140
roger_wee 1:3f291f2f80d3 141 void updateYawThrottle()
roger_wee 1:3f291f2f80d3 142 {
roger_wee 1:3f291f2f80d3 143 Lthrustpwm = IDLE + (j*(thrust + yawOut));
roger_wee 1:3f291f2f80d3 144 Rthrustpwm = IDLE + (j*(thrust + (MAXYAW-yawOut)));
roger_wee 1:3f291f2f80d3 145 }
roger_wee 1:3f291f2f80d3 146
roger_wee 0:048a74dd7c3a 147 void initializePIDs()
roger_wee 0:048a74dd7c3a 148 {
roger_wee 0:048a74dd7c3a 149 pidy.SetMode(AUTOMATIC); // Yaw PID
roger_wee 1:3f291f2f80d3 150 pidy.SetOutputLimits(0, MAXYAW);
roger_wee 0:048a74dd7c3a 151
roger_wee 0:048a74dd7c3a 152 pidp.SetMode(AUTOMATIC); // Pitch PID
roger_wee 1:3f291f2f80d3 153 pidp.SetOutputLimits(0, MAXPITCH);
roger_wee 0:048a74dd7c3a 154
roger_wee 0:048a74dd7c3a 155 pidr.SetMode(AUTOMATIC); // Roll PID
roger_wee 1:3f291f2f80d3 156 pidr.SetOutputLimits(0, MAXROLL);
roger_wee 0:048a74dd7c3a 157
roger_wee 0:048a74dd7c3a 158 pidd.SetMode(AUTOMATIC); // Depth PID
roger_wee 1:3f291f2f80d3 159 pidd.SetOutputLimits(0,300);
roger_wee 0:048a74dd7c3a 160
roger_wee 0:048a74dd7c3a 161 depthPoint = 0;
roger_wee 1:3f291f2f80d3 162 pitchPoint = 0;
roger_wee 1:3f291f2f80d3 163 rollPoint = 0;
roger_wee 0:048a74dd7c3a 164 }
roger_wee 0:048a74dd7c3a 165
roger_wee 0:048a74dd7c3a 166 void readDepth()
roger_wee 0:048a74dd7c3a 167 {
roger_wee 0:048a74dd7c3a 168 // Read pressure sensor data if available
roger_wee 0:048a74dd7c3a 169 if (device.readable())
roger_wee 0:048a74dd7c3a 170 {
roger_wee 0:048a74dd7c3a 171 // Receive depth in inches as an integer
roger_wee 0:048a74dd7c3a 172 depth = device.getc();
roger_wee 0:048a74dd7c3a 173
roger_wee 0:048a74dd7c3a 174 // Convert to feet
roger_wee 0:048a74dd7c3a 175 }
roger_wee 0:048a74dd7c3a 176 }
roger_wee 0:048a74dd7c3a 177
roger_wee 0:048a74dd7c3a 178 void displayTelemetry()
roger_wee 0:048a74dd7c3a 179 {
roger_wee 0:048a74dd7c3a 180 char text[90];
roger_wee 1:3f291f2f80d3 181 sprintf(text, "%f,%f,%f,%d,%f \n", yaw, pitch, roll, depth, depthPoint);
roger_wee 0:048a74dd7c3a 182 pc.printf("%s", text);
roger_wee 0:048a74dd7c3a 183 }
roger_wee 0:048a74dd7c3a 184
roger_wee 0:048a74dd7c3a 185 //-----------End Helper functions--------------//
roger_wee 0:048a74dd7c3a 186
roger_wee 0:048a74dd7c3a 187
roger_wee 0:048a74dd7c3a 188
roger_wee 0:048a74dd7c3a 189 //-----------Interface States------------------//
roger_wee 0:048a74dd7c3a 190 enum controlStates { init, idle, manual, preparePid, beginTune } controlState;
roger_wee 0:048a74dd7c3a 191
roger_wee 0:048a74dd7c3a 192 void controlInterface(){
roger_wee 0:048a74dd7c3a 193
roger_wee 0:048a74dd7c3a 194 switch(controlState) //Actions
roger_wee 0:048a74dd7c3a 195 {
roger_wee 0:048a74dd7c3a 196 case init:
roger_wee 0:048a74dd7c3a 197 pc.printf("Initialize sensors \n");
roger_wee 0:048a74dd7c3a 198 IMUinit(mpu1);
roger_wee 0:048a74dd7c3a 199 compass.init();
roger_wee 0:048a74dd7c3a 200
roger_wee 1:3f291f2f80d3 201 pc.printf("Initialirze motors \n");
roger_wee 0:048a74dd7c3a 202 neutralizeMotors();
roger_wee 0:048a74dd7c3a 203
roger_wee 0:048a74dd7c3a 204 pc.printf("Initialize PID objects \n");
roger_wee 0:048a74dd7c3a 205 initializePIDs();
roger_wee 0:048a74dd7c3a 206
roger_wee 0:048a74dd7c3a 207 pc.printf("Calibrate MARG Filter \n");
roger_wee 0:048a74dd7c3a 208 calibrateFilter();
roger_wee 0:048a74dd7c3a 209 break;
roger_wee 0:048a74dd7c3a 210
roger_wee 0:048a74dd7c3a 211 case idle:
roger_wee 0:048a74dd7c3a 212 IMUPrintData(mpu1, compass);
roger_wee 0:048a74dd7c3a 213 readDepth();
roger_wee 0:048a74dd7c3a 214 displayTelemetry();
roger_wee 0:048a74dd7c3a 215 break;
roger_wee 0:048a74dd7c3a 216
roger_wee 0:048a74dd7c3a 217 case manual:
roger_wee 0:048a74dd7c3a 218 //pc.printf("Manual Control \n");
roger_wee 0:048a74dd7c3a 219 switch(command)
roger_wee 0:048a74dd7c3a 220 {
roger_wee 0:048a74dd7c3a 221 case 'N':
roger_wee 0:048a74dd7c3a 222 //pc.printf("Neutralize motors\n");
roger_wee 0:048a74dd7c3a 223 neutralizeMotors();
roger_wee 0:048a74dd7c3a 224 break;
roger_wee 0:048a74dd7c3a 225
roger_wee 0:048a74dd7c3a 226 case 'R':
roger_wee 0:048a74dd7c3a 227 //pc.printf("Reduce Thrust\n");
roger_wee 0:048a74dd7c3a 228 if (m1pwm < pwmMin)
roger_wee 0:048a74dd7c3a 229 {
roger_wee 0:048a74dd7c3a 230 m1pwm++;
roger_wee 0:048a74dd7c3a 231 m2pwm++;
roger_wee 0:048a74dd7c3a 232 m3pwm++;
roger_wee 0:048a74dd7c3a 233 m4pwm++;
roger_wee 0:048a74dd7c3a 234 }
roger_wee 0:048a74dd7c3a 235 break;
roger_wee 0:048a74dd7c3a 236
roger_wee 0:048a74dd7c3a 237 case 'I':
roger_wee 0:048a74dd7c3a 238 //pc.printf("Increase Thrust\n");
roger_wee 0:048a74dd7c3a 239 if (m1pwm > pwmMax)
roger_wee 0:048a74dd7c3a 240 {
roger_wee 0:048a74dd7c3a 241 m1pwm--;
roger_wee 0:048a74dd7c3a 242 m2pwm--;
roger_wee 0:048a74dd7c3a 243 m3pwm--;
roger_wee 0:048a74dd7c3a 244 m4pwm--;
roger_wee 0:048a74dd7c3a 245 }
roger_wee 0:048a74dd7c3a 246 break;
roger_wee 0:048a74dd7c3a 247
roger_wee 0:048a74dd7c3a 248 default:
roger_wee 0:048a74dd7c3a 249 break;
roger_wee 0:048a74dd7c3a 250 }
roger_wee 0:048a74dd7c3a 251 updateMotors();
roger_wee 1:3f291f2f80d3 252 displayTelemetry();
roger_wee 0:048a74dd7c3a 253 break;
roger_wee 0:048a74dd7c3a 254
roger_wee 0:048a74dd7c3a 255 case preparePid:
roger_wee 0:048a74dd7c3a 256 // pc.printf("Prepare PID \n");
roger_wee 0:048a74dd7c3a 257 switch(command)
roger_wee 0:048a74dd7c3a 258 {
roger_wee 1:3f291f2f80d3 259 case 'K': // Increase Kp gain
roger_wee 0:048a74dd7c3a 260 kpVal += .1;
roger_wee 0:048a74dd7c3a 261 break;
roger_wee 0:048a74dd7c3a 262
roger_wee 1:3f291f2f80d3 263 case 'k':
roger_wee 0:048a74dd7c3a 264 if (kpVal > 0 ) // Decreae Kp gain
roger_wee 0:048a74dd7c3a 265 {
roger_wee 0:048a74dd7c3a 266 kpVal -= .1;
roger_wee 0:048a74dd7c3a 267 }
roger_wee 0:048a74dd7c3a 268 break;
roger_wee 0:048a74dd7c3a 269
roger_wee 1:3f291f2f80d3 270 case 'I':
roger_wee 1:3f291f2f80d3 271 kiVal += .1;
roger_wee 1:3f291f2f80d3 272 break;
roger_wee 1:3f291f2f80d3 273
roger_wee 1:3f291f2f80d3 274 case 'i':
roger_wee 1:3f291f2f80d3 275 if (kiVal > 0)
roger_wee 1:3f291f2f80d3 276 {
roger_wee 1:3f291f2f80d3 277 kiVal -= .1;
roger_wee 1:3f291f2f80d3 278 }
roger_wee 1:3f291f2f80d3 279 break;
roger_wee 1:3f291f2f80d3 280
roger_wee 1:3f291f2f80d3 281 case 'D':
roger_wee 1:3f291f2f80d3 282 kdVal += .1;
roger_wee 1:3f291f2f80d3 283 break;
roger_wee 1:3f291f2f80d3 284
roger_wee 1:3f291f2f80d3 285 case 'd':
roger_wee 1:3f291f2f80d3 286 if (kdVal > 0)
roger_wee 1:3f291f2f80d3 287 {
roger_wee 1:3f291f2f80d3 288 kdVal -= .1;
roger_wee 1:3f291f2f80d3 289 }
roger_wee 1:3f291f2f80d3 290 break;
roger_wee 1:3f291f2f80d3 291
roger_wee 0:048a74dd7c3a 292 case 'S': // Increase depth
roger_wee 0:048a74dd7c3a 293 depthPoint++;
roger_wee 0:048a74dd7c3a 294
roger_wee 0:048a74dd7c3a 295 case 's': // Decrease setpoint
roger_wee 0:048a74dd7c3a 296 if(depthPoint > 0)
roger_wee 0:048a74dd7c3a 297 {
roger_wee 0:048a74dd7c3a 298 depthPoint--;
roger_wee 0:048a74dd7c3a 299 }
roger_wee 0:048a74dd7c3a 300 break;
roger_wee 0:048a74dd7c3a 301
roger_wee 0:048a74dd7c3a 302 default:
roger_wee 0:048a74dd7c3a 303 break;
roger_wee 0:048a74dd7c3a 304 }
roger_wee 0:048a74dd7c3a 305
roger_wee 0:048a74dd7c3a 306 // Set tunings
roger_wee 1:3f291f2f80d3 307 pidd.SetTunings(kpVal,kiVal,kdVal); //Set Ki Kd = 0
roger_wee 1:3f291f2f80d3 308 //pidy.SetTunings(kpVal,kiVal,kdVal);
roger_wee 1:3f291f2f80d3 309 //pidp.SetTunings(kpVal,kiVal,kdVal);
roger_wee 1:3f291f2f80d3 310 //pidr.SetTunings(kpVal,kiVal,kdVal);
roger_wee 0:048a74dd7c3a 311
roger_wee 1:3f291f2f80d3 312 if (data.read_ms() % 500 == 0)
roger_wee 1:3f291f2f80d3 313 {
roger_wee 1:3f291f2f80d3 314 // Print Set parameters
roger_wee 1:3f291f2f80d3 315 pc.printf("DepthPoint: %d \n", depthPoint);
roger_wee 1:3f291f2f80d3 316 pc.printf("Kp gain: %d \n", pidd.GetKp());
roger_wee 1:3f291f2f80d3 317 pc.printf("Ki gain: %d \n", pidd.GetKi());
roger_wee 1:3f291f2f80d3 318 pc.printf("Kd gain: %d \n", pidd.GetKd());
roger_wee 1:3f291f2f80d3 319 }
roger_wee 0:048a74dd7c3a 320 break;
roger_wee 0:048a74dd7c3a 321
roger_wee 0:048a74dd7c3a 322 case beginTune:
roger_wee 0:048a74dd7c3a 323 //pc.printf("Tuning process begins.. \n");
roger_wee 0:048a74dd7c3a 324
roger_wee 0:048a74dd7c3a 325 // Sense inertial / depth data
roger_wee 0:048a74dd7c3a 326 IMUPrintData(mpu1, compass);
roger_wee 0:048a74dd7c3a 327 readDepth();
roger_wee 0:048a74dd7c3a 328
roger_wee 0:048a74dd7c3a 329 // Assign feedback variables
roger_wee 0:048a74dd7c3a 330 myDepth = depth;
roger_wee 1:3f291f2f80d3 331 //myYaw = yaw;
roger_wee 1:3f291f2f80d3 332 //myPitch = pitch;
roger_wee 1:3f291f2f80d3 333 //myRoll = Roll;
roger_wee 0:048a74dd7c3a 334
roger_wee 0:048a74dd7c3a 335 // Compute PID
roger_wee 0:048a74dd7c3a 336 pidd.Compute();
roger_wee 1:3f291f2f80d3 337 //pidy.Compute();
roger_wee 1:3f291f2f80d3 338 //pidp.Compute();
roger_wee 1:3f291f2f80d3 339 //pidr.Compute();
roger_wee 0:048a74dd7c3a 340
roger_wee 0:048a74dd7c3a 341 // Output pwm to motors // FIXME : account for pitch, roll and maybe yaw depending on motor configuration
roger_wee 1:3f291f2f80d3 342 m1pwm = IDLE + depthOut; // + pitchOut
roger_wee 1:3f291f2f80d3 343 m2pwm = IDLE + depthOut; // + (MAXPITCH - pitchOut)
roger_wee 1:3f291f2f80d3 344 m3pwm = IDLE + depthOut; // + pitchOut
roger_wee 1:3f291f2f80d3 345 m4pwm = IDLE + depthOut; // + (MAXPITCH - pitchOut)
roger_wee 1:3f291f2f80d3 346
roger_wee 1:3f291f2f80d3 347 //updateYawThrottle();
roger_wee 0:048a74dd7c3a 348 updateMotors();
roger_wee 0:048a74dd7c3a 349
roger_wee 0:048a74dd7c3a 350 // Display telemetry
roger_wee 0:048a74dd7c3a 351 displayTelemetry();
roger_wee 0:048a74dd7c3a 352
roger_wee 0:048a74dd7c3a 353 break;
roger_wee 0:048a74dd7c3a 354
roger_wee 0:048a74dd7c3a 355 default:
roger_wee 0:048a74dd7c3a 356 pc.printf("Error! \n");
roger_wee 0:048a74dd7c3a 357 break;
roger_wee 0:048a74dd7c3a 358 }
roger_wee 0:048a74dd7c3a 359
roger_wee 0:048a74dd7c3a 360 switch(controlState) //Transitions
roger_wee 0:048a74dd7c3a 361 {
roger_wee 0:048a74dd7c3a 362 case init:
roger_wee 0:048a74dd7c3a 363 controlState = idle;
roger_wee 0:048a74dd7c3a 364 break;
roger_wee 0:048a74dd7c3a 365
roger_wee 0:048a74dd7c3a 366 case idle:
roger_wee 0:048a74dd7c3a 367 controlState = (command == 'm') ? manual : ((command == 'p') ? preparePid : idle);
roger_wee 0:048a74dd7c3a 368 break;
roger_wee 0:048a74dd7c3a 369
roger_wee 0:048a74dd7c3a 370 case manual:
roger_wee 0:048a74dd7c3a 371 controlState = (command == 'p') ? preparePid : manual;
roger_wee 0:048a74dd7c3a 372 if (controlState == preparePid)
roger_wee 0:048a74dd7c3a 373 {
roger_wee 0:048a74dd7c3a 374 //pc.printf("Neutralize Motors \n");
roger_wee 0:048a74dd7c3a 375 neutralizeMotors();
roger_wee 0:048a74dd7c3a 376 }
roger_wee 0:048a74dd7c3a 377 break;
roger_wee 0:048a74dd7c3a 378
roger_wee 0:048a74dd7c3a 379 case preparePid:
roger_wee 0:048a74dd7c3a 380 controlState = (command == 'g') ? beginTune : preparePid;
roger_wee 0:048a74dd7c3a 381 break;
roger_wee 0:048a74dd7c3a 382
roger_wee 0:048a74dd7c3a 383 case beginTune:
roger_wee 0:048a74dd7c3a 384 controlState = (command == 't') ? preparePid : beginTune;
roger_wee 0:048a74dd7c3a 385 if (controlState == preparePid)
roger_wee 0:048a74dd7c3a 386 {
roger_wee 0:048a74dd7c3a 387 //pc.printf("Neutralize Motors \n");
roger_wee 0:048a74dd7c3a 388 neutralizeMotors();
roger_wee 0:048a74dd7c3a 389 }
roger_wee 0:048a74dd7c3a 390 break;
roger_wee 0:048a74dd7c3a 391
roger_wee 0:048a74dd7c3a 392 default:
roger_wee 0:048a74dd7c3a 393 break;
roger_wee 0:048a74dd7c3a 394 }
roger_wee 0:048a74dd7c3a 395 }
roger_wee 0:048a74dd7c3a 396
roger_wee 0:048a74dd7c3a 397 //-----------End Interface---------------------//
roger_wee 0:048a74dd7c3a 398
roger_wee 0:048a74dd7c3a 399 int main() {
roger_wee 0:048a74dd7c3a 400 // Initialize control state
roger_wee 0:048a74dd7c3a 401 controlState = init;
roger_wee 1:3f291f2f80d3 402 data.start();
roger_wee 0:048a74dd7c3a 403 while(1)
roger_wee 0:048a74dd7c3a 404 {
roger_wee 0:048a74dd7c3a 405 // Read user input
roger_wee 0:048a74dd7c3a 406 readUserInput();
roger_wee 0:048a74dd7c3a 407
roger_wee 0:048a74dd7c3a 408 // Begin Interface
roger_wee 0:048a74dd7c3a 409 controlInterface();
roger_wee 0:048a74dd7c3a 410 }
roger_wee 0:048a74dd7c3a 411 }