Alpesh Bhudia / Mbed 2 deprecated EquatorStrutDigitalMonitor

Merged to branch

Dependencies:   USBDevice mbed EquatorStrutController LightWeightSerialTransmit

Fork of EquatorStrutDigitalMonitor by Stewart Coulden-Smith

main.cpp@24:214f2d426484, 2014-08-15 (annotated)

Committer:
pyrostew
Date:
Fri Aug 15 14:25:56 2014 +0000
Revision:
24:214f2d426484
Parent:
23:e9e2cd9c1fd1
Child:
25:0e4bde9e1adc
Not Relevant

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