Alpesh Bhudia / Mbed 2 deprecated EquatorStrutDigitalMonitor

Merged to branch

Dependencies:   USBDevice mbed EquatorStrutController LightWeightSerialTransmit

Fork of EquatorStrutDigitalMonitor by Stewart Coulden-Smith

main.cpp@26:5e4b329defec, 2014-08-20 (annotated)

Committer:
pyrostew
Date:
Wed Aug 20 08:36:08 2014 +0000
Revision:
26:5e4b329defec
Parent:
25:0e4bde9e1adc
Child:
27:1d55ebab6214
Moved strut control code back into the EquatorStrutController Library and removed all none essential commented out code.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pyrostew 0:398432a37ca5 1 #include "mbed.h"
pyrostew 0:398432a37ca5 2 #include "RawSerial.h"
pyrostew 26:5e4b329defec 3 #include "EquatorStrutController.h"
pyrostew 0:398432a37ca5 4
pyrostew 0:398432a37ca5 5 Timer RunningTime;
pyrostew 0:398432a37ca5 6
pyrostew 0:398432a37ca5 7 RawSerial pc(P1_27, P1_26);
pyrostew 0:398432a37ca5 8
pyrostew 23:e9e2cd9c1fd1 9 double vMax = 300;
pyrostew 23:e9e2cd9c1fd1 10 double vStep = 50;
pyrostew 23:e9e2cd9c1fd1 11 double pStep = 0.5;
pyrostew 23:e9e2cd9c1fd1 12 double G1 = vStep / pStep;
pyrostew 23:e9e2cd9c1fd1 13 double G2 = (vMax - vStep)/(1.0-pStep);
pyrostew 23:e9e2cd9c1fd1 14 double Glin = vMax;
pyrostew 23:e9e2cd9c1fd1 15 double pLinStep = pStep * G1 / Glin;
pyrostew 23:e9e2cd9c1fd1 16 double TargetPwm;
pyrostew 23:e9e2cd9c1fd1 17
pyrostew 0:398432a37ca5 18 char counter = 0;
pyrostew 0:398432a37ca5 19
pyrostew 26:5e4b329defec 20 EquatorStrut strut;
pyrostew 15:cd409a54ceec 21
pyrostew 26:5e4b329defec 22 void LinearizePower(double power)
pyrostew 15:cd409a54ceec 23 {
pyrostew 23:e9e2cd9c1fd1 24 // Compute the corrected pwm value to linearise the velocity profile
pyrostew 23:e9e2cd9c1fd1 25 double correctedPwm;
pyrostew 22:9f7dae024a81 26
pyrostew 25:0e4bde9e1adc 27 if (fabs(power) < 1.0)
pyrostew 23:e9e2cd9c1fd1 28 {
pyrostew 25:0e4bde9e1adc 29 if (fabs(power) < pLinStep)
pyrostew 25:0e4bde9e1adc 30 {
pyrostew 25:0e4bde9e1adc 31 correctedPwm = (fabs(power) * Glin) / G1;
pyrostew 25:0e4bde9e1adc 32 }
pyrostew 25:0e4bde9e1adc 33 else
pyrostew 25:0e4bde9e1adc 34 {
pyrostew 25:0e4bde9e1adc 35 correctedPwm = pStep + (fabs(power) - pLinStep) * Glin / G2;
pyrostew 25:0e4bde9e1adc 36 }
pyrostew 23:e9e2cd9c1fd1 37 }
pyrostew 23:e9e2cd9c1fd1 38 else
pyrostew 23:e9e2cd9c1fd1 39 {
pyrostew 25:0e4bde9e1adc 40 correctedPwm = 1.0;
pyrostew 23:e9e2cd9c1fd1 41 }
pyrostew 22:9f7dae024a81 42
pyrostew 23:e9e2cd9c1fd1 43 // Make sure our corrected value has the correct sign.
pyrostew 23:e9e2cd9c1fd1 44 if(power < 0) correctedPwm *= -1.0;
pyrostew 23:e9e2cd9c1fd1 45
pyrostew 26:5e4b329defec 46 if (correctedPwm > 1.0)
pyrostew 26:5e4b329defec 47 {
pyrostew 26:5e4b329defec 48 correctedPwm = 1.0;
pyrostew 26:5e4b329defec 49 }
pyrostew 0:398432a37ca5 50
pyrostew 26:5e4b329defec 51 else if (correctedPwm < -1.0)
pyrostew 26:5e4b329defec 52 {
pyrostew 26:5e4b329defec 53 correctedPwm = -1.0;
pyrostew 26:5e4b329defec 54 }
pyrostew 0:398432a37ca5 55
pyrostew 26:5e4b329defec 56 strut.SetPower(correctedPwm);
pyrostew 1:a33723b70582 57 }
pyrostew 1:a33723b70582 58
alpesh 13:18c376e5dc9a 59 double PosError = 0; //This has been defined here as it's being used in the serial transmit function
alpesh 6:bfe745b152fa 60
pyrostew 12:814db1249a19 61 void SerialOut(double outputValue)
pyrostew 12:814db1249a19 62 {
pyrostew 0:398432a37ca5 63 int outChar = 0;
pyrostew 0:398432a37ca5 64
pyrostew 12:814db1249a19 65 if (outputValue < 0.0)
pyrostew 0:398432a37ca5 66 {
pyrostew 0:398432a37ca5 67 pc.putc('-');
pyrostew 12:814db1249a19 68 outputValue *= -1.0;
pyrostew 0:398432a37ca5 69 }
pyrostew 18:ab282713f4a7 70 if (outputValue >= 1000.0)
pyrostew 18:ab282713f4a7 71 {
pyrostew 18:ab282713f4a7 72 outChar = outputValue / 1000;
pyrostew 18:ab282713f4a7 73 pc.putc(outChar + 48);
pyrostew 18:ab282713f4a7 74 outputValue -= outChar * 1000.0;
pyrostew 18:ab282713f4a7 75 }
pyrostew 12:814db1249a19 76 if (outputValue >= 100.0)
pyrostew 0:398432a37ca5 77 {
pyrostew 12:814db1249a19 78 outChar = outputValue / 100;
pyrostew 0:398432a37ca5 79 pc.putc(outChar + 48);
pyrostew 12:814db1249a19 80 outputValue -= outChar * 100.0;
pyrostew 0:398432a37ca5 81 }
pyrostew 18:ab282713f4a7 82 else if(outChar > 0)
pyrostew 18:ab282713f4a7 83 {
pyrostew 18:ab282713f4a7 84 pc.putc('0');
pyrostew 18:ab282713f4a7 85 }
pyrostew 12:814db1249a19 86 if (outputValue >= 10.0)
pyrostew 0:398432a37ca5 87 {
pyrostew 12:814db1249a19 88 outChar = outputValue / 10;
pyrostew 0:398432a37ca5 89 pc.putc(outChar + 48);
pyrostew 12:814db1249a19 90 outputValue -= outChar * 10.0;
pyrostew 0:398432a37ca5 91 }
pyrostew 0:398432a37ca5 92 else if(outChar > 0)
pyrostew 0:398432a37ca5 93 {
pyrostew 0:398432a37ca5 94 pc.putc('0');
pyrostew 0:398432a37ca5 95 }
pyrostew 12:814db1249a19 96 if (outputValue >= 1.0)
pyrostew 0:398432a37ca5 97 {
pyrostew 12:814db1249a19 98 outChar = outputValue;
pyrostew 0:398432a37ca5 99 pc.putc(outChar + 48);
pyrostew 12:814db1249a19 100 outputValue -= outChar;
pyrostew 0:398432a37ca5 101 }
pyrostew 0:398432a37ca5 102 else
pyrostew 0:398432a37ca5 103 {
pyrostew 0:398432a37ca5 104 pc.putc('0');
pyrostew 0:398432a37ca5 105 }
pyrostew 12:814db1249a19 106 if (outputValue >= 0.1)
pyrostew 0:398432a37ca5 107 {
pyrostew 0:398432a37ca5 108 pc.putc('.');
pyrostew 12:814db1249a19 109 outChar = outputValue * 10;
pyrostew 0:398432a37ca5 110 pc.putc(outChar + 48);
pyrostew 12:814db1249a19 111 outputValue -= (double)outChar / 10.0;
pyrostew 0:398432a37ca5 112 }
pyrostew 0:398432a37ca5 113 else
pyrostew 0:398432a37ca5 114 {
pyrostew 0:398432a37ca5 115 pc.putc('.');
pyrostew 0:398432a37ca5 116 pc.putc('0');
pyrostew 0:398432a37ca5 117 }
pyrostew 12:814db1249a19 118 if (outputValue >= 0.01)
pyrostew 0:398432a37ca5 119 {
pyrostew 12:814db1249a19 120 outChar = outputValue * 100;
pyrostew 0:398432a37ca5 121 pc.putc(outChar + 48);
pyrostew 12:814db1249a19 122 outputValue -= (double)outChar / 100.0;
pyrostew 0:398432a37ca5 123 }
pyrostew 0:398432a37ca5 124 else
pyrostew 0:398432a37ca5 125 {
pyrostew 0:398432a37ca5 126 pc.putc('0');
pyrostew 0:398432a37ca5 127 }
pyrostew 12:814db1249a19 128 if (outputValue >= 0.001)
pyrostew 0:398432a37ca5 129 {
pyrostew 12:814db1249a19 130 outChar= outputValue * 1000;
pyrostew 0:398432a37ca5 131 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 132 }
pyrostew 12:814db1249a19 133 }
pyrostew 12:814db1249a19 134
alpesh 13:18c376e5dc9a 135 double PosKpGain = 0.0;
alpesh 13:18c376e5dc9a 136 double PosKiGain = 0.0;
alpesh 13:18c376e5dc9a 137 double PosKdGain = 0.0;
alpesh 13:18c376e5dc9a 138
pyrostew 12:814db1249a19 139 void SerialTransmit()
pyrostew 12:814db1249a19 140 {
pyrostew 12:814db1249a19 141 SerialOut(RunningTime.read());
pyrostew 12:814db1249a19 142
pyrostew 12:814db1249a19 143 pc.putc('\t');
pyrostew 12:814db1249a19 144
pyrostew 26:5e4b329defec 145 SerialOut(strut.GetPosition());
pyrostew 0:398432a37ca5 146
alpesh 2:d1805e7d46fb 147 pc.putc('\t');
pyrostew 12:814db1249a19 148
pyrostew 26:5e4b329defec 149 SerialOut(strut.CurrentPower());
pyrostew 1:a33723b70582 150
pyrostew 12:814db1249a19 151 pc.putc('\t');
pyrostew 12:814db1249a19 152
pyrostew 26:5e4b329defec 153 SerialOut(strut.CurrentSpeed());
pyrostew 1:a33723b70582 154
alpesh 6:bfe745b152fa 155 pc.putc('\t');
pyrostew 12:814db1249a19 156
alpesh 13:18c376e5dc9a 157 SerialOut(PosError);
pyrostew 12:814db1249a19 158
pyrostew 12:814db1249a19 159 pc.putc('\t');
pyrostew 12:814db1249a19 160
alpesh 13:18c376e5dc9a 161 SerialOut(PosKpGain);
alpesh 6:bfe745b152fa 162
pyrostew 12:814db1249a19 163 pc.putc('\t');
pyrostew 12:814db1249a19 164
pyrostew 0:398432a37ca5 165 pc.putc(10);
pyrostew 0:398432a37ca5 166 pc.putc(13);
pyrostew 0:398432a37ca5 167 }
pyrostew 0:398432a37ca5 168
pyrostew 23:e9e2cd9c1fd1 169 void SerialNewFile()
pyrostew 23:e9e2cd9c1fd1 170 {
pyrostew 23:e9e2cd9c1fd1 171 pc.putc(28);
pyrostew 23:e9e2cd9c1fd1 172 pc.putc(10);
pyrostew 23:e9e2cd9c1fd1 173 pc.putc(13);
pyrostew 23:e9e2cd9c1fd1 174 }
pyrostew 23:e9e2cd9c1fd1 175
pyrostew 26:5e4b329defec 176 double SetPoint = 150.0; //Target Position in Millimeters
alpesh 6:bfe745b152fa 177
alpesh 13:18c376e5dc9a 178 double PosProError;
alpesh 13:18c376e5dc9a 179 double PosIntError;
alpesh 13:18c376e5dc9a 180 double PosDifError;
alpesh 13:18c376e5dc9a 181
alpesh 3:e693c65b04de 182 int errorcounter;
alpesh 13:18c376e5dc9a 183 double PosPreviousError [10];
alpesh 2:d1805e7d46fb 184
alpesh 3:e693c65b04de 185 double PwmChange=0;
alpesh 3:e693c65b04de 186
alpesh 3:e693c65b04de 187 double pwm;
pyrostew 23:e9e2cd9c1fd1 188
alpesh 13:18c376e5dc9a 189 double TargetVelocity;
alpesh 3:e693c65b04de 190
alpesh 13:18c376e5dc9a 191 double PosiState;
alpesh 13:18c376e5dc9a 192
alpesh 13:18c376e5dc9a 193 int PreviousTime = 0;
alpesh 13:18c376e5dc9a 194
alpesh 4:2ec05810bc47 195 void Controller ()
pyrostew 12:814db1249a19 196 {
alpesh 13:18c376e5dc9a 197 ///////////////////////////////////////////////////////////////////////////////////////////////
alpesh 13:18c376e5dc9a 198 //Position PID
alpesh 13:18c376e5dc9a 199 ///////////////////////////////////////////////////////////////////////////////////////////////
pyrostew 26:5e4b329defec 200 int timeStep = RunningTime.read_us() - PreviousTime;
pyrostew 26:5e4b329defec 201 PreviousTime = RunningTime.read_us();
pyrostew 12:814db1249a19 202
pyrostew 26:5e4b329defec 203 double integral_velmax = vMax/PosKiGain;
pyrostew 26:5e4b329defec 204 double integral_velmin = -vMax/PosKiGain ;
pyrostew 26:5e4b329defec 205
pyrostew 26:5e4b329defec 206 PosError = SetPoint - (strut.GetPosition());
alpesh 13:18c376e5dc9a 207
pyrostew 26:5e4b329defec 208 PosProError = PosError * PosKpGain;
pyrostew 12:814db1249a19 209
pyrostew 26:5e4b329defec 210 PosDifError = (PosError - PosPreviousError[errorcounter]) / timeStep;
pyrostew 26:5e4b329defec 211
pyrostew 26:5e4b329defec 212 PosiState += PosError;
pyrostew 26:5e4b329defec 213
pyrostew 26:5e4b329defec 214 if (PosiState > integral_velmax)
pyrostew 26:5e4b329defec 215 {
pyrostew 26:5e4b329defec 216 PosiState = integral_velmax;
pyrostew 26:5e4b329defec 217 }
pyrostew 26:5e4b329defec 218 else if (PosiState < integral_velmin)
pyrostew 26:5e4b329defec 219 {
pyrostew 26:5e4b329defec 220 PosiState = integral_velmin;
pyrostew 26:5e4b329defec 221 }
pyrostew 26:5e4b329defec 222 PosIntError = PosKiGain * PosiState;
pyrostew 26:5e4b329defec 223
pyrostew 26:5e4b329defec 224 TargetPwm = (PosKpGain * PosError + PosKdGain * PosDifError + PosIntError);
pyrostew 22:9f7dae024a81 225
pyrostew 26:5e4b329defec 226
pyrostew 26:5e4b329defec 227 if (TargetPwm > 1.0)
pyrostew 26:5e4b329defec 228 {
pyrostew 26:5e4b329defec 229 TargetPwm = 1.0;
pyrostew 26:5e4b329defec 230 }
pyrostew 26:5e4b329defec 231
pyrostew 26:5e4b329defec 232 else if (TargetPwm < -1.0)
pyrostew 26:5e4b329defec 233 {
pyrostew 26:5e4b329defec 234 TargetPwm = -1.0;
pyrostew 26:5e4b329defec 235 }
pyrostew 12:814db1249a19 236
pyrostew 26:5e4b329defec 237 strut.SetPower(TargetPwm);
pyrostew 26:5e4b329defec 238
pyrostew 26:5e4b329defec 239 errorcounter++;
pyrostew 12:814db1249a19 240
pyrostew 26:5e4b329defec 241 if (errorcounter > 9)
pyrostew 26:5e4b329defec 242 {
pyrostew 26:5e4b329defec 243 errorcounter = 0;
pyrostew 26:5e4b329defec 244 }
pyrostew 12:814db1249a19 245
pyrostew 26:5e4b329defec 246 PosPreviousError[errorcounter] = PosError;
pyrostew 12:814db1249a19 247 }
alpesh 4:2ec05810bc47 248
pyrostew 0:398432a37ca5 249 int main()
alpesh 2:d1805e7d46fb 250
pyrostew 26:5e4b329defec 251 {
pyrostew 0:398432a37ca5 252 RunningTime.start();
pyrostew 0:398432a37ca5 253
pyrostew 0:398432a37ca5 254 pc.baud(115200);
pyrostew 0:398432a37ca5 255
pyrostew 26:5e4b329defec 256 strut.Home();
alpesh 4:2ec05810bc47 257 //Enable();
pyrostew 0:398432a37ca5 258
pyrostew 26:5e4b329defec 259 errorcounter = 0;
pyrostew 22:9f7dae024a81 260 PosPreviousError[errorcounter]=0;
pyrostew 22:9f7dae024a81 261 PreviousTime = RunningTime.read_us();
pyrostew 22:9f7dae024a81 262
pyrostew 26:5e4b329defec 263 while(strut.IsEnabled())
pyrostew 22:9f7dae024a81 264 {
pyrostew 26:5e4b329defec 265 int counter = 0;
pyrostew 26:5e4b329defec 266 //double pow = 0.0;
pyrostew 26:5e4b329defec 267 //while(pow < 1.0)
pyrostew 26:5e4b329defec 268 PosKpGain = 0.0;
pyrostew 26:5e4b329defec 269 while(PosKpGain < 1.0)
pyrostew 22:9f7dae024a81 270 {
pyrostew 26:5e4b329defec 271 counter++;
pyrostew 22:9f7dae024a81 272
pyrostew 26:5e4b329defec 273 //pow += 0.05;
pyrostew 26:5e4b329defec 274 PosKpGain += 0.01;
pyrostew 26:5e4b329defec 275
pyrostew 26:5e4b329defec 276 SerialNewFile();
pyrostew 26:5e4b329defec 277
pyrostew 26:5e4b329defec 278 RunningTime.reset();
pyrostew 22:9f7dae024a81 279
pyrostew 26:5e4b329defec 280 float iterationStart = RunningTime.read();
pyrostew 26:5e4b329defec 281
pyrostew 26:5e4b329defec 282 while(RunningTime.read()-iterationStart < 10.0)
pyrostew 26:5e4b329defec 283 {
pyrostew 26:5e4b329defec 284 SerialTransmit();
pyrostew 26:5e4b329defec 285
pyrostew 26:5e4b329defec 286 Controller();
pyrostew 26:5e4b329defec 287 }
pyrostew 22:9f7dae024a81 288
pyrostew 26:5e4b329defec 289 //SetPower(pow);
pyrostew 26:5e4b329defec 290
pyrostew 26:5e4b329defec 291 //while(position < 21000 && RunningTime.read()-iterationStart < 30.0)
pyrostew 26:5e4b329defec 292 //{
pyrostew 26:5e4b329defec 293 // SerialTransmit();
pyrostew 26:5e4b329defec 294 //}
pyrostew 26:5e4b329defec 295
pyrostew 26:5e4b329defec 296 //iterationStart = RunningTime.read();
pyrostew 26:5e4b329defec 297 //SerialNewFile();
pyrostew 26:5e4b329defec 298
pyrostew 26:5e4b329defec 299 //SetPower(-pow);
pyrostew 26:5e4b329defec 300
pyrostew 26:5e4b329defec 301 //while(position > 1000 && RunningTime.read()-iterationStart < 30.0)
pyrostew 26:5e4b329defec 302 //{
pyrostew 26:5e4b329defec 303 // SerialTransmit();
pyrostew 26:5e4b329defec 304 //}
pyrostew 23:e9e2cd9c1fd1 305
pyrostew 26:5e4b329defec 306 //SerialNewFile();
pyrostew 26:5e4b329defec 307 strut.Home();
pyrostew 26:5e4b329defec 308
pyrostew 26:5e4b329defec 309 if (counter == 10)
pyrostew 26:5e4b329defec 310 {
pyrostew 26:5e4b329defec 311 counter = 0;
pyrostew 26:5e4b329defec 312 strut.Disable();
pyrostew 26:5e4b329defec 313 wait(60);
pyrostew 26:5e4b329defec 314 strut.Enable();
pyrostew 26:5e4b329defec 315 }
pyrostew 22:9f7dae024a81 316 }
pyrostew 22:9f7dae024a81 317
pyrostew 26:5e4b329defec 318 strut.Disable();
pyrostew 22:9f7dae024a81 319 }
pyrostew 26:5e4b329defec 320 }