Alpesh Bhudia / Mbed 2 deprecated EquatorStrutDigitalMonitor

Merged to branch

Dependencies:   USBDevice mbed EquatorStrutController LightWeightSerialTransmit

Fork of EquatorStrutDigitalMonitor by Stewart Coulden-Smith

main.cpp@25:0e4bde9e1adc, 2014-08-18 (annotated)

Committer:
pyrostew
Date:
Mon Aug 18 13:24:30 2014 +0000
Revision:
25:0e4bde9e1adc
Parent:
24:214f2d426484
Child:
26:5e4b329defec
Stewart-Mon18.08.14

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