Rogelio Vazquez / Mbed 2 deprecated Control_interface

UCR Robosub manual control / PID tuning interface

Dependencies:   mbed HMC5883L

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?

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 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 }