Alpesh Bhudia / Mbed 2 deprecated EquatorStrutDigitalMonitor

Merged to branch

Dependencies:   USBDevice mbed EquatorStrutController LightWeightSerialTransmit

Fork of EquatorStrutDigitalMonitor by Stewart Coulden-Smith

main.cpp@21:b4eb253d3dbb, 2014-08-15 (annotated)

Committer:
pyrostew
Date:
Fri Aug 15 09:15:15 2014 +0000
Revision:
21:b4eb253d3dbb
Parent:
19:a6369257c00f 
Stewart's code before updating

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