Alpesh Bhudia / Mbed 2 deprecated EquatorStrutDigitalMonitor

Merged to branch

Dependencies:   USBDevice mbed EquatorStrutController LightWeightSerialTransmit

Fork of EquatorStrutDigitalMonitor by Stewart Coulden-Smith

main.cpp@22:9f7dae024a81, 2014-08-15 (annotated)

Committer:
pyrostew
Date:
Fri Aug 15 10:26:49 2014 +0000
Revision:
22:9f7dae024a81
Parent:
20:04432d03b46c
Child:
23:e9e2cd9c1fd1
Compute the correct pwm value to linearise the velocity profile

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 0:398432a37ca5 3
pyrostew 12:814db1249a19 4 DigitalIn HallSensorState(P0_2);
pyrostew 0:398432a37ca5 5 InterruptIn RGHSinInterrupt(P0_11);
pyrostew 0:398432a37ca5 6 InterruptIn RGHCosInterrupt(P0_12);
pyrostew 15:cd409a54ceec 7 InterruptIn RGHSinFallingInterrupt(P0_13);
pyrostew 15:cd409a54ceec 8 InterruptIn RGHCosFallingInterrupt(P0_14);
pyrostew 0:398432a37ca5 9 InterruptIn HallSensor(P0_2);
pyrostew 0:398432a37ca5 10 DigitalOut ResetLine(P1_29);
pyrostew 17:f54cdc9ae52f 11 DigitalOut PulseOut(P1_22);
pyrostew 0:398432a37ca5 12 PwmOut PhaseA(P0_9);
pyrostew 0:398432a37ca5 13 PwmOut PhaseB(P0_8);
pyrostew 0:398432a37ca5 14 Timer RunningTime;
pyrostew 0:398432a37ca5 15
pyrostew 0:398432a37ca5 16 bool Enabled = false;
pyrostew 12:814db1249a19 17 volatile bool Homing = false;
pyrostew 12:814db1249a19 18 volatile bool HallTriggered = false;
pyrostew 0:398432a37ca5 19
pyrostew 0:398432a37ca5 20 RawSerial pc(P1_27, P1_26);
pyrostew 0:398432a37ca5 21
pyrostew 17:f54cdc9ae52f 22 volatile int direction = 1;
pyrostew 17:f54cdc9ae52f 23 volatile int position = 0;
pyrostew 0:398432a37ca5 24 double currentPower = 0.0;
pyrostew 17:f54cdc9ae52f 25 int lastTime = 0;
pyrostew 0:398432a37ca5 26
pyrostew 17:f54cdc9ae52f 27 double SpeedInterval = 0.0;
pyrostew 17:f54cdc9ae52f 28 int LastPosition = 0;
pyrostew 10:088eeae4287c 29
pyrostew 0:398432a37ca5 30 char counter = 0;
pyrostew 0:398432a37ca5 31
pyrostew 15:cd409a54ceec 32 volatile bool SinHigh = false;
pyrostew 15:cd409a54ceec 33 volatile bool CosHigh = false;
pyrostew 17:f54cdc9ae52f 34 volatile bool LastSin = false;
pyrostew 17:f54cdc9ae52f 35 volatile bool LastHigh = false;
pyrostew 15:cd409a54ceec 36
pyrostew 17:f54cdc9ae52f 37 void ActionEvent(bool CurrHigh, bool CurrSin)
pyrostew 17:f54cdc9ae52f 38 {
pyrostew 17:f54cdc9ae52f 39 // Same event again - DO NOTHING
pyrostew 17:f54cdc9ae52f 40 if (CurrHigh == LastHigh && CurrSin == LastSin)
pyrostew 17:f54cdc9ae52f 41 {
pyrostew 17:f54cdc9ae52f 42 return;
pyrostew 17:f54cdc9ae52f 43 }
pyrostew 10:088eeae4287c 44
pyrostew 17:f54cdc9ae52f 45 if (CurrSin != LastSin) // Otherwave
pyrostew 10:088eeae4287c 46 {
pyrostew 17:f54cdc9ae52f 47 // Other wave
pyrostew 17:f54cdc9ae52f 48 if ((CurrSin && CurrHigh == LastHigh) ||
pyrostew 17:f54cdc9ae52f 49 (!CurrSin && CurrHigh != LastHigh))
pyrostew 17:f54cdc9ae52f 50 {
pyrostew 17:f54cdc9ae52f 51 //Forwards
pyrostew 17:f54cdc9ae52f 52 direction = 1;
pyrostew 17:f54cdc9ae52f 53 }
pyrostew 17:f54cdc9ae52f 54 else
pyrostew 17:f54cdc9ae52f 55 {
pyrostew 17:f54cdc9ae52f 56 //Backwards
pyrostew 17:f54cdc9ae52f 57 direction = -1;
pyrostew 17:f54cdc9ae52f 58 }
pyrostew 17:f54cdc9ae52f 59
pyrostew 17:f54cdc9ae52f 60
pyrostew 10:088eeae4287c 61 }
pyrostew 10:088eeae4287c 62 else
pyrostew 10:088eeae4287c 63 {
pyrostew 17:f54cdc9ae52f 64 // Reversal
pyrostew 17:f54cdc9ae52f 65 direction = -direction;
pyrostew 12:814db1249a19 66 }
pyrostew 12:814db1249a19 67
pyrostew 17:f54cdc9ae52f 68 position += direction;
pyrostew 17:f54cdc9ae52f 69
pyrostew 17:f54cdc9ae52f 70 // Set the state for the wave that fired
pyrostew 17:f54cdc9ae52f 71 if(CurrSin) SinHigh = CurrHigh;
pyrostew 17:f54cdc9ae52f 72 else CosHigh = CurrHigh;
pyrostew 17:f54cdc9ae52f 73
pyrostew 17:f54cdc9ae52f 74 // Set the last event values
pyrostew 17:f54cdc9ae52f 75 LastHigh = CurrHigh;
pyrostew 17:f54cdc9ae52f 76 LastSin = CurrSin;
pyrostew 10:088eeae4287c 77 }
pyrostew 10:088eeae4287c 78
pyrostew 15:cd409a54ceec 79 void RGHSinRisingHandler()
pyrostew 15:cd409a54ceec 80 {
pyrostew 17:f54cdc9ae52f 81 PulseOut = 1;
pyrostew 17:f54cdc9ae52f 82 ActionEvent(true, true);
pyrostew 17:f54cdc9ae52f 83 PulseOut = 0;
pyrostew 15:cd409a54ceec 84 }
pyrostew 15:cd409a54ceec 85
pyrostew 15:cd409a54ceec 86 void RGHSinFallingHandler()
pyrostew 15:cd409a54ceec 87 {
pyrostew 17:f54cdc9ae52f 88 ActionEvent(false, true);
pyrostew 15:cd409a54ceec 89 }
pyrostew 15:cd409a54ceec 90
pyrostew 15:cd409a54ceec 91 void RGHCosRisingHandler()
pyrostew 17:f54cdc9ae52f 92 {
pyrostew 17:f54cdc9ae52f 93 ActionEvent(true, false);
pyrostew 0:398432a37ca5 94 }
pyrostew 0:398432a37ca5 95
pyrostew 15:cd409a54ceec 96 void RGHCosFallingHandler()
pyrostew 15:cd409a54ceec 97 {
pyrostew 17:f54cdc9ae52f 98 ActionEvent(false, false);
pyrostew 0:398432a37ca5 99 }
pyrostew 0:398432a37ca5 100
pyrostew 0:398432a37ca5 101 void SetPower(double power)
pyrostew 0:398432a37ca5 102 {
pyrostew 0:398432a37ca5 103 if(!Enabled)
pyrostew 0:398432a37ca5 104 {
pyrostew 0:398432a37ca5 105 return;
pyrostew 0:398432a37ca5 106 }
pyrostew 0:398432a37ca5 107
pyrostew 0:398432a37ca5 108 if (power > 1.0 || power < -1.0)
pyrostew 0:398432a37ca5 109 {
pyrostew 0:398432a37ca5 110 return;
pyrostew 0:398432a37ca5 111 }
pyrostew 0:398432a37ca5 112
pyrostew 22:9f7dae024a81 113
pyrostew 22:9f7dae024a81 114 Linear clamping
pyrostew 22:9f7dae024a81 115
pyrostew 22:9f7dae024a81 116
pyrostew 22:9f7dae024a81 117
pyrostew 22:9f7dae024a81 118 currentPower = power;
pyrostew 22:9f7dae024a81 119
pyrostew 0:398432a37ca5 120 PhaseA = (power + 1.0) / 2;
pyrostew 0:398432a37ca5 121 PhaseB = 1.0 - ((power + 1.0) / 2);
pyrostew 0:398432a37ca5 122 }
pyrostew 0:398432a37ca5 123
pyrostew 0:398432a37ca5 124 void Enable()
pyrostew 0:398432a37ca5 125 {
pyrostew 0:398432a37ca5 126 SetPower(0.0);
pyrostew 0:398432a37ca5 127
pyrostew 0:398432a37ca5 128 ResetLine = 1;
pyrostew 0:398432a37ca5 129
pyrostew 0:398432a37ca5 130 Enabled = true;
pyrostew 0:398432a37ca5 131 }
pyrostew 0:398432a37ca5 132
pyrostew 0:398432a37ca5 133 void Disable()
pyrostew 0:398432a37ca5 134 {
pyrostew 0:398432a37ca5 135 ResetLine = 0;
pyrostew 0:398432a37ca5 136
pyrostew 0:398432a37ca5 137 SetPower(0.0);
pyrostew 0:398432a37ca5 138
pyrostew 0:398432a37ca5 139 Enabled = false;
pyrostew 0:398432a37ca5 140 }
pyrostew 0:398432a37ca5 141
pyrostew 1:a33723b70582 142 double GetSpeed()
pyrostew 1:a33723b70582 143 {
pyrostew 17:f54cdc9ae52f 144 double interval = RunningTime.read() - SpeedInterval;
pyrostew 17:f54cdc9ae52f 145 int positionDiff = position - LastPosition;
pyrostew 17:f54cdc9ae52f 146
pyrostew 17:f54cdc9ae52f 147 SpeedInterval = RunningTime.read();
pyrostew 17:f54cdc9ae52f 148 LastPosition = position;
pyrostew 17:f54cdc9ae52f 149
pyrostew 17:f54cdc9ae52f 150 return (positionDiff * 0.01)/interval;
pyrostew 1:a33723b70582 151 }
pyrostew 1:a33723b70582 152
alpesh 13:18c376e5dc9a 153 double PosError = 0; //This has been defined here as it's being used in the serial transmit function
alpesh 19:a6369257c00f 154 //double VelError = 0;
alpesh 6:bfe745b152fa 155
pyrostew 12:814db1249a19 156 void SerialOut(double outputValue)
pyrostew 12:814db1249a19 157 {
pyrostew 0:398432a37ca5 158 int outChar = 0;
pyrostew 0:398432a37ca5 159
pyrostew 12:814db1249a19 160 if (outputValue < 0.0)
pyrostew 0:398432a37ca5 161 {
pyrostew 0:398432a37ca5 162 pc.putc('-');
pyrostew 12:814db1249a19 163 outputValue *= -1.0;
pyrostew 0:398432a37ca5 164 }
pyrostew 18:ab282713f4a7 165 if (outputValue >= 1000.0)
pyrostew 18:ab282713f4a7 166 {
pyrostew 18:ab282713f4a7 167 outChar = outputValue / 1000;
pyrostew 18:ab282713f4a7 168 pc.putc(outChar + 48);
pyrostew 18:ab282713f4a7 169 outputValue -= outChar * 1000.0;
pyrostew 18:ab282713f4a7 170 }
pyrostew 12:814db1249a19 171 if (outputValue >= 100.0)
pyrostew 0:398432a37ca5 172 {
pyrostew 12:814db1249a19 173 outChar = outputValue / 100;
pyrostew 0:398432a37ca5 174 pc.putc(outChar + 48);
pyrostew 12:814db1249a19 175 outputValue -= outChar * 100.0;
pyrostew 0:398432a37ca5 176 }
pyrostew 18:ab282713f4a7 177 else if(outChar > 0)
pyrostew 18:ab282713f4a7 178 {
pyrostew 18:ab282713f4a7 179 pc.putc('0');
pyrostew 18:ab282713f4a7 180 }
pyrostew 12:814db1249a19 181 if (outputValue >= 10.0)
pyrostew 0:398432a37ca5 182 {
pyrostew 12:814db1249a19 183 outChar = outputValue / 10;
pyrostew 0:398432a37ca5 184 pc.putc(outChar + 48);
pyrostew 12:814db1249a19 185 outputValue -= outChar * 10.0;
pyrostew 0:398432a37ca5 186 }
pyrostew 0:398432a37ca5 187 else if(outChar > 0)
pyrostew 0:398432a37ca5 188 {
pyrostew 0:398432a37ca5 189 pc.putc('0');
pyrostew 0:398432a37ca5 190 }
pyrostew 12:814db1249a19 191 if (outputValue >= 1.0)
pyrostew 0:398432a37ca5 192 {
pyrostew 12:814db1249a19 193 outChar = outputValue;
pyrostew 0:398432a37ca5 194 pc.putc(outChar + 48);
pyrostew 12:814db1249a19 195 outputValue -= outChar;
pyrostew 0:398432a37ca5 196 }
pyrostew 0:398432a37ca5 197 else
pyrostew 0:398432a37ca5 198 {
pyrostew 0:398432a37ca5 199 pc.putc('0');
pyrostew 0:398432a37ca5 200 }
pyrostew 12:814db1249a19 201 if (outputValue >= 0.1)
pyrostew 0:398432a37ca5 202 {
pyrostew 0:398432a37ca5 203 pc.putc('.');
pyrostew 12:814db1249a19 204 outChar = outputValue * 10;
pyrostew 0:398432a37ca5 205 pc.putc(outChar + 48);
pyrostew 12:814db1249a19 206 outputValue -= (double)outChar / 10.0;
pyrostew 0:398432a37ca5 207 }
pyrostew 0:398432a37ca5 208 else
pyrostew 0:398432a37ca5 209 {
pyrostew 0:398432a37ca5 210 pc.putc('.');
pyrostew 0:398432a37ca5 211 pc.putc('0');
pyrostew 0:398432a37ca5 212 }
pyrostew 12:814db1249a19 213 if (outputValue >= 0.01)
pyrostew 0:398432a37ca5 214 {
pyrostew 12:814db1249a19 215 outChar = outputValue * 100;
pyrostew 0:398432a37ca5 216 pc.putc(outChar + 48);
pyrostew 12:814db1249a19 217 outputValue -= (double)outChar / 100.0;
pyrostew 0:398432a37ca5 218 }
pyrostew 0:398432a37ca5 219 else
pyrostew 0:398432a37ca5 220 {
pyrostew 0:398432a37ca5 221 pc.putc('0');
pyrostew 0:398432a37ca5 222 }
pyrostew 12:814db1249a19 223 if (outputValue >= 0.001)
pyrostew 0:398432a37ca5 224 {
pyrostew 12:814db1249a19 225 outChar= outputValue * 1000;
pyrostew 0:398432a37ca5 226 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 227 }
pyrostew 12:814db1249a19 228 }
pyrostew 12:814db1249a19 229
alpesh 13:18c376e5dc9a 230 double PosKpGain = 0.0;
alpesh 13:18c376e5dc9a 231 double PosKiGain = 0.0;
alpesh 13:18c376e5dc9a 232 double PosKdGain = 0.0;
alpesh 13:18c376e5dc9a 233
alpesh 19:a6369257c00f 234 //double VelKpGain = 0.01;
alpesh 19:a6369257c00f 235 //double VelKiGain = 0.0;
alpesh 19:a6369257c00f 236 //double VelKdGain = 0.0;
pyrostew 12:814db1249a19 237
pyrostew 12:814db1249a19 238 void SerialTransmit()
pyrostew 12:814db1249a19 239 {
pyrostew 12:814db1249a19 240 SerialOut(RunningTime.read());
pyrostew 12:814db1249a19 241
pyrostew 12:814db1249a19 242 pc.putc('\t');
pyrostew 12:814db1249a19 243
pyrostew 17:f54cdc9ae52f 244 SerialOut((double)position*0.01);
pyrostew 0:398432a37ca5 245
alpesh 2:d1805e7d46fb 246 pc.putc('\t');
pyrostew 12:814db1249a19 247
pyrostew 12:814db1249a19 248 SerialOut(currentPower);
pyrostew 1:a33723b70582 249
pyrostew 12:814db1249a19 250 pc.putc('\t');
pyrostew 12:814db1249a19 251
pyrostew 12:814db1249a19 252 SerialOut(GetSpeed());
pyrostew 1:a33723b70582 253
alpesh 6:bfe745b152fa 254 pc.putc('\t');
pyrostew 12:814db1249a19 255
alpesh 13:18c376e5dc9a 256 SerialOut(PosError);
pyrostew 12:814db1249a19 257
pyrostew 12:814db1249a19 258 pc.putc('\t');
pyrostew 12:814db1249a19 259
alpesh 13:18c376e5dc9a 260 SerialOut(PosKpGain);
alpesh 6:bfe745b152fa 261
pyrostew 12:814db1249a19 262 pc.putc('\t');
pyrostew 12:814db1249a19 263
alpesh 19:a6369257c00f 264 // SerialOut(VelKpGain);
pyrostew 12:814db1249a19 265
pyrostew 18:ab282713f4a7 266 //pc.putc('\t');
pyrostew 12:814db1249a19 267
pyrostew 18:ab282713f4a7 268 //SerialOut(PosKdGain);
alpesh 6:bfe745b152fa 269
pyrostew 0:398432a37ca5 270 pc.putc(10);
pyrostew 0:398432a37ca5 271 pc.putc(13);
pyrostew 0:398432a37ca5 272 }
pyrostew 0:398432a37ca5 273
pyrostew 17:f54cdc9ae52f 274 void Home()
pyrostew 17:f54cdc9ae52f 275 {
pyrostew 17:f54cdc9ae52f 276 if (!Enabled)
pyrostew 17:f54cdc9ae52f 277 {
pyrostew 17:f54cdc9ae52f 278 Enable();
pyrostew 17:f54cdc9ae52f 279 }
pyrostew 17:f54cdc9ae52f 280
pyrostew 17:f54cdc9ae52f 281 if (HallSensorState == 1)
pyrostew 17:f54cdc9ae52f 282 {
pyrostew 17:f54cdc9ae52f 283 Homing = true;
pyrostew 17:f54cdc9ae52f 284 HallTriggered = false;
pyrostew 17:f54cdc9ae52f 285
pyrostew 17:f54cdc9ae52f 286 SetPower(-0.6);
pyrostew 17:f54cdc9ae52f 287
pyrostew 17:f54cdc9ae52f 288 while (!HallTriggered)
pyrostew 17:f54cdc9ae52f 289 {
pyrostew 18:ab282713f4a7 290 wait(0.1);
pyrostew 17:f54cdc9ae52f 291 }
pyrostew 17:f54cdc9ae52f 292 }
pyrostew 17:f54cdc9ae52f 293
pyrostew 17:f54cdc9ae52f 294 SetPower(1.0);
pyrostew 17:f54cdc9ae52f 295
pyrostew 17:f54cdc9ae52f 296 while (position < 2000)
pyrostew 17:f54cdc9ae52f 297 {
pyrostew 17:f54cdc9ae52f 298 //SerialTransmit();
pyrostew 17:f54cdc9ae52f 299 }
pyrostew 17:f54cdc9ae52f 300
pyrostew 17:f54cdc9ae52f 301 Homing = true;
pyrostew 17:f54cdc9ae52f 302 HallTriggered = false;
pyrostew 17:f54cdc9ae52f 303
pyrostew 17:f54cdc9ae52f 304 SetPower(-0.4);
pyrostew 17:f54cdc9ae52f 305
pyrostew 17:f54cdc9ae52f 306 while (!HallTriggered)
pyrostew 17:f54cdc9ae52f 307 {
pyrostew 18:ab282713f4a7 308 wait(0.1);
pyrostew 17:f54cdc9ae52f 309 }
pyrostew 17:f54cdc9ae52f 310 }
pyrostew 17:f54cdc9ae52f 311
pyrostew 0:398432a37ca5 312 void HallEffectFall()
pyrostew 0:398432a37ca5 313 {
pyrostew 0:398432a37ca5 314 RGHSinInterrupt.disable_irq();
pyrostew 0:398432a37ca5 315 RGHCosInterrupt.disable_irq();
pyrostew 15:cd409a54ceec 316 RGHSinFallingInterrupt.disable_irq();
pyrostew 15:cd409a54ceec 317 RGHCosFallingInterrupt.disable_irq();
pyrostew 0:398432a37ca5 318
pyrostew 0:398432a37ca5 319 if (direction < 0)
pyrostew 0:398432a37ca5 320 {
pyrostew 0:398432a37ca5 321 SetPower(0.0);
pyrostew 0:398432a37ca5 322
pyrostew 0:398432a37ca5 323 if (Homing)
pyrostew 0:398432a37ca5 324 {
pyrostew 0:398432a37ca5 325 HallTriggered = true;
pyrostew 0:398432a37ca5 326 Homing = false;
pyrostew 0:398432a37ca5 327 position = 0.0;
pyrostew 0:398432a37ca5 328 }
pyrostew 0:398432a37ca5 329 }
pyrostew 0:398432a37ca5 330 RGHSinInterrupt.enable_irq();
pyrostew 0:398432a37ca5 331 RGHCosInterrupt.enable_irq();
pyrostew 15:cd409a54ceec 332 RGHSinFallingInterrupt.enable_irq();
pyrostew 15:cd409a54ceec 333 RGHCosFallingInterrupt.enable_irq();
pyrostew 0:398432a37ca5 334 }
pyrostew 0:398432a37ca5 335
alpesh 13:18c376e5dc9a 336
pyrostew 18:ab282713f4a7 337 double SetPoint = 70.0; //Target Position in Millimeter per second
alpesh 6:bfe745b152fa 338
alpesh 13:18c376e5dc9a 339 double PosProError;
alpesh 13:18c376e5dc9a 340 double PosIntError;
alpesh 13:18c376e5dc9a 341 double PosDifError;
alpesh 13:18c376e5dc9a 342
alpesh 19:a6369257c00f 343 //double VelProError;
alpesh 19:a6369257c00f 344 //double VelIntError;
alpesh 19:a6369257c00f 345 //double VelDifError;
alpesh 2:d1805e7d46fb 346
alpesh 3:e693c65b04de 347 int errorcounter;
alpesh 13:18c376e5dc9a 348 double PosPreviousError [10];
alpesh 19:a6369257c00f 349 //double VelPreviousError [10];
alpesh 2:d1805e7d46fb 350
alpesh 3:e693c65b04de 351 double PwmChange=0;
alpesh 3:e693c65b04de 352
alpesh 3:e693c65b04de 353 double pwm;
alpesh 13:18c376e5dc9a 354 double TargetPwm;
alpesh 13:18c376e5dc9a 355 double TargetVelocity;
alpesh 3:e693c65b04de 356
alpesh 13:18c376e5dc9a 357 double PosiState;
alpesh 19:a6369257c00f 358 //double VeliState;
alpesh 13:18c376e5dc9a 359
alpesh 13:18c376e5dc9a 360 int PreviousTime = 0;
alpesh 13:18c376e5dc9a 361
alpesh 20:04432d03b46c 362 double vMax = 300;
alpesh 20:04432d03b46c 363 double vStep = 50;
alpesh 20:04432d03b46c 364 double pStep = 0.5;
pyrostew 22:9f7dae024a81 365 double G1 = vStep / pStep;
pyrostew 22:9f7dae024a81 366 double G2 = (vMax - vStep)/(1.0-pStep);
pyrostew 22:9f7dae024a81 367 double Glin = vMax;
alpesh 3:e693c65b04de 368
alpesh 4:2ec05810bc47 369 void Controller ()
pyrostew 12:814db1249a19 370 {
alpesh 13:18c376e5dc9a 371
alpesh 13:18c376e5dc9a 372 ///////////////////////////////////////////////////////////////////////////////////////////////
alpesh 13:18c376e5dc9a 373 //Position PID
alpesh 13:18c376e5dc9a 374 ///////////////////////////////////////////////////////////////////////////////////////////////
alpesh 13:18c376e5dc9a 375
alpesh 13:18c376e5dc9a 376 //if (position <= SetPoint || 1)
alpesh 13:18c376e5dc9a 377
pyrostew 12:814db1249a19 378
alpesh 13:18c376e5dc9a 379 //{
alpesh 13:18c376e5dc9a 380 int timeStep = RunningTime.read_us() - PreviousTime;
alpesh 13:18c376e5dc9a 381 PreviousTime = RunningTime.read_us();
alpesh 13:18c376e5dc9a 382
alpesh 19:a6369257c00f 383 double integral_velmax = vMax/PosKiGain;
alpesh 19:a6369257c00f 384 double integral_velmin = -vMax/PosKiGain ;
alpesh 13:18c376e5dc9a 385
pyrostew 17:f54cdc9ae52f 386 PosError = SetPoint - (position * 0.01);
alpesh 13:18c376e5dc9a 387
alpesh 13:18c376e5dc9a 388 PosProError = PosError * PosKpGain;
pyrostew 12:814db1249a19 389
alpesh 13:18c376e5dc9a 390 PosDifError = (PosError - PosPreviousError[errorcounter]) / timeStep;
alpesh 13:18c376e5dc9a 391
alpesh 13:18c376e5dc9a 392 PosiState += PosError;
alpesh 13:18c376e5dc9a 393
alpesh 13:18c376e5dc9a 394 if (PosiState > integral_velmax)
alpesh 13:18c376e5dc9a 395 {
alpesh 13:18c376e5dc9a 396 PosiState = integral_velmax;
alpesh 13:18c376e5dc9a 397 }
alpesh 13:18c376e5dc9a 398 else if (PosiState < integral_velmin)
alpesh 13:18c376e5dc9a 399 {
alpesh 13:18c376e5dc9a 400 PosiState = integral_velmin;
alpesh 13:18c376e5dc9a 401 }
alpesh 13:18c376e5dc9a 402 PosIntError = PosKiGain * PosiState;
alpesh 13:18c376e5dc9a 403
alpesh 19:a6369257c00f 404 TargetPwm = (PosKpGain * PosError + PosKdGain * PosDifError + PosIntError);
pyrostew 22:9f7dae024a81 405
alpesh 13:18c376e5dc9a 406
pyrostew 22:9f7dae024a81 407 if (TargetPwm > 1.0)
pyrostew 12:814db1249a19 408 {
pyrostew 22:9f7dae024a81 409 TargetPwm = 1.0;
pyrostew 12:814db1249a19 410 }
pyrostew 12:814db1249a19 411
pyrostew 22:9f7dae024a81 412 else if (TargetPwm < -1.0)
pyrostew 12:814db1249a19 413 {
pyrostew 22:9f7dae024a81 414 TargetPwm = -1.0;
pyrostew 22:9f7dae024a81 415 }
pyrostew 22:9f7dae024a81 416
pyrostew 22:9f7dae024a81 417
pyrostew 22:9f7dae024a81 418 // Compute the corrected pwm value to linearise the velocity profile
pyrostew 22:9f7dae024a81 419 double correctedPwm;
pyrostew 22:9f7dae024a81 420
pyrostew 22:9f7dae024a81 421 if (fabs(TargetPwm) < pStep)
pyrostew 22:9f7dae024a81 422 {
pyrostew 22:9f7dae024a81 423 correctedPwm = (fabs(TargetPwm) * Glin) / G1;
pyrostew 12:814db1249a19 424 }
pyrostew 22:9f7dae024a81 425
pyrostew 22:9f7dae024a81 426 if (fabs(TargetPwm) > pStep)
pyrostew 12:814db1249a19 427 {
pyrostew 22:9f7dae024a81 428 correctedPwm = pStep + (fabs(TargetPwm) - pStep) * Glin / G2;
pyrostew 12:814db1249a19 429 }
pyrostew 22:9f7dae024a81 430
pyrostew 22:9f7dae024a81 431 // Make sure our corrected value has the correct sign.
pyrostew 22:9f7dae024a81 432 if(TargetPwm < 0) correctedPwm *= -1.0;
pyrostew 22:9f7dae024a81 433
pyrostew 22:9f7dae024a81 434 SetPower(correctedPwm);
pyrostew 22:9f7dae024a81 435
pyrostew 22:9f7dae024a81 436
alpesh 4:2ec05810bc47 437
pyrostew 12:814db1249a19 438 errorcounter ++;
pyrostew 12:814db1249a19 439
pyrostew 12:814db1249a19 440 if (errorcounter > 9)
pyrostew 12:814db1249a19 441 {
pyrostew 12:814db1249a19 442 errorcounter = 0;
pyrostew 12:814db1249a19 443 }
pyrostew 12:814db1249a19 444
alpesh 13:18c376e5dc9a 445 PosPreviousError[errorcounter] = PosError;
alpesh 19:a6369257c00f 446 // VelPreviousError[errorcounter] = VelError;
alpesh 13:18c376e5dc9a 447 // }
alpesh 13:18c376e5dc9a 448
pyrostew 12:814db1249a19 449 }
alpesh 4:2ec05810bc47 450
pyrostew 0:398432a37ca5 451 int main()
alpesh 2:d1805e7d46fb 452
pyrostew 0:398432a37ca5 453 {
pyrostew 15:cd409a54ceec 454 RGHSinInterrupt.rise(&RGHSinRisingHandler);
pyrostew 15:cd409a54ceec 455 RGHCosInterrupt.rise(&RGHCosRisingHandler);
pyrostew 15:cd409a54ceec 456 RGHSinFallingInterrupt.fall(&RGHSinFallingHandler);
pyrostew 15:cd409a54ceec 457 RGHCosFallingInterrupt.fall(&RGHCosFallingHandler);
pyrostew 0:398432a37ca5 458 HallSensor.fall(&HallEffectFall);
pyrostew 0:398432a37ca5 459 HallSensor.mode(PullUp);
pyrostew 0:398432a37ca5 460
pyrostew 17:f54cdc9ae52f 461 RGHSinFallingInterrupt.mode(PullNone);
pyrostew 17:f54cdc9ae52f 462 RGHCosFallingInterrupt.mode(PullNone);
pyrostew 17:f54cdc9ae52f 463
pyrostew 0:398432a37ca5 464 RunningTime.start();
pyrostew 0:398432a37ca5 465
pyrostew 0:398432a37ca5 466 pc.baud(115200);
pyrostew 0:398432a37ca5 467
pyrostew 0:398432a37ca5 468 Home();
alpesh 4:2ec05810bc47 469 //Enable();
pyrostew 0:398432a37ca5 470
pyrostew 22:9f7dae024a81 471 errorcounter = 0;
pyrostew 22:9f7dae024a81 472 PosPreviousError[errorcounter]=0;
pyrostew 22:9f7dae024a81 473 PreviousTime = RunningTime.read_us();
pyrostew 22:9f7dae024a81 474
pyrostew 22:9f7dae024a81 475 while(Enabled)
pyrostew 22:9f7dae024a81 476 {
pyrostew 22:9f7dae024a81 477 double pow = 0.4;
pyrostew 22:9f7dae024a81 478 while(pow < 1.0)
pyrostew 22:9f7dae024a81 479 // PosKpGain = 10.0;
pyrostew 22:9f7dae024a81 480 // while(PosKpGain < 50.0)
pyrostew 22:9f7dae024a81 481 {
pyrostew 22:9f7dae024a81 482 pow += 0.05;
pyrostew 22:9f7dae024a81 483 //PosKpGain += 1.0;
pyrostew 22:9f7dae024a81 484
pyrostew 22:9f7dae024a81 485 // VelKpGain = 0.003;
pyrostew 22:9f7dae024a81 486 // while (VelKpGain < 0.008)
pyrostew 22:9f7dae024a81 487 // {
pyrostew 22:9f7dae024a81 488 // VelKpGain += 0.001;
pyrostew 22:9f7dae024a81 489
pyrostew 22:9f7dae024a81 490 float iterationStart = RunningTime.read();
pyrostew 22:9f7dae024a81 491
pyrostew 22:9f7dae024a81 492 while(RunningTime.read()-iterationStart < 10.0)
pyrostew 22:9f7dae024a81 493 {
pyrostew 22:9f7dae024a81 494 SerialTransmit();
pyrostew 22:9f7dae024a81 495
pyrostew 22:9f7dae024a81 496 Controller();
pyrostew 22:9f7dae024a81 497 }
pyrostew 22:9f7dae024a81 498
pyrostew 22:9f7dae024a81 499 Home();
pyrostew 22:9f7dae024a81 500 // }
pyrostew 22:9f7dae024a81 501 }
pyrostew 22:9f7dae024a81 502
pyrostew 22:9f7dae024a81 503 Disable();
pyrostew 22:9f7dae024a81 504 }
pyrostew 22:9f7dae024a81 505 }
pyrostew 22:9f7dae024a81 506
pyrostew 22:9f7dae024a81 507
pyrostew 22:9f7dae024a81 508 /* Change this throttle value range to 1.0 and 0.0 for car speed
pyrostew 22:9f7dae024a81 509 m_throttlechange = (m_kpGain * m_error + m_kdGain * m_PosDifError + m_PosIntError);
pyrostew 22:9f7dae024a81 510
pyrostew 22:9f7dae024a81 511 double throttle = m_throttle + m_throttlechange;
pyrostew 22:9f7dae024a81 512
pyrostew 22:9f7dae024a81 513 if (new_throttle > 1.0) {
pyrostew 22:9f7dae024a81 514 m_throttle = 1.0;
pyrostew 22:9f7dae024a81 515 } else if (new_throttle < 0.0) {
pyrostew 22:9f7dae024a81 516 m_throttle = 0.0;
pyrostew 22:9f7dae024a81 517 } else {
pyrostew 22:9f7dae024a81 518 m_throttle = new_throttle;
pyrostew 22:9f7dae024a81 519 }*/
pyrostew 22:9f7dae024a81 520
pyrostew 22:9f7dae024a81 521
pyrostew 22:9f7dae024a81 522 /*
pyrostew 22:9f7dae024a81 523
pyrostew 22:9f7dae024a81 524
alpesh 3:e693c65b04de 525 errorcounter = 0;
alpesh 13:18c376e5dc9a 526 PosPreviousError[errorcounter]=0;
alpesh 2:d1805e7d46fb 527
alpesh 13:18c376e5dc9a 528 PreviousTime = RunningTime.read_us();
alpesh 3:e693c65b04de 529
pyrostew 17:f54cdc9ae52f 530 while(Enabled)
alpesh 2:d1805e7d46fb 531 {
pyrostew 17:f54cdc9ae52f 532 //double pow = 0.4;
pyrostew 17:f54cdc9ae52f 533 //while(pow < 1.0)
pyrostew 18:ab282713f4a7 534 PosKpGain = 10.0;
pyrostew 18:ab282713f4a7 535 while(PosKpGain < 50.0)
pyrostew 0:398432a37ca5 536 {
pyrostew 17:f54cdc9ae52f 537 //pow += 0.05;
pyrostew 18:ab282713f4a7 538 PosKpGain += 1.0;
pyrostew 17:f54cdc9ae52f 539
alpesh 19:a6369257c00f 540 // VelKpGain = 0.003;
alpesh 19:a6369257c00f 541 // while (VelKpGain < 0.008)
alpesh 19:a6369257c00f 542 // {
alpesh 19:a6369257c00f 543 // VelKpGain += 0.001;
pyrostew 18:ab282713f4a7 544
pyrostew 18:ab282713f4a7 545 float iterationStart = RunningTime.read();
pyrostew 17:f54cdc9ae52f 546
pyrostew 18:ab282713f4a7 547 while(RunningTime.read()-iterationStart < 10.0)
pyrostew 18:ab282713f4a7 548 {
pyrostew 18:ab282713f4a7 549 SerialTransmit();
pyrostew 18:ab282713f4a7 550
pyrostew 18:ab282713f4a7 551 Controller();
pyrostew 18:ab282713f4a7 552 }
pyrostew 18:ab282713f4a7 553
pyrostew 18:ab282713f4a7 554 Home();
alpesh 19:a6369257c00f 555 // }
pyrostew 0:398432a37ca5 556 }
alpesh 2:d1805e7d46fb 557
alpesh 6:bfe745b152fa 558 Disable();
pyrostew 17:f54cdc9ae52f 559 }
alpesh 7:4cd7be306626 560 }
pyrostew 22:9f7dae024a81 561 */
alpesh 7:4cd7be306626 562 /* Change this throttle value range to 1.0 and 0.0 for car speed
alpesh 13:18c376e5dc9a 563 m_throttlechange = (m_kpGain * m_error + m_kdGain * m_PosDifError + m_PosIntError);
alpesh 7:4cd7be306626 564
alpesh 7:4cd7be306626 565 double throttle = m_throttle + m_throttlechange;
alpesh 7:4cd7be306626 566
alpesh 7:4cd7be306626 567 if (new_throttle > 1.0) {
alpesh 7:4cd7be306626 568 m_throttle = 1.0;
alpesh 7:4cd7be306626 569 } else if (new_throttle < 0.0) {
alpesh 7:4cd7be306626 570 m_throttle = 0.0;
alpesh 7:4cd7be306626 571 } else {
alpesh 7:4cd7be306626 572 m_throttle = new_throttle;
alpesh 7:4cd7be306626 573 }*/
alpesh 7:4cd7be306626 574
alpesh 7:4cd7be306626 575
alpesh 7:4cd7be306626 576
alpesh 7:4cd7be306626 577
alpesh 7:4cd7be306626 578