Alpesh Bhudia / Mbed 2 deprecated EquatorStrutDigitalMonitor

Merged to branch

Dependencies:   USBDevice mbed EquatorStrutController LightWeightSerialTransmit

Fork of EquatorStrutDigitalMonitor by Stewart Coulden-Smith

main.cpp@20:04432d03b46c, 2014-08-15 (annotated)

Committer:
alpesh
Date:
Fri Aug 15 09:11:50 2014 +0000
Revision:
20:04432d03b46c
Parent:
19:a6369257c00f
Child:
22:9f7dae024a81
Linearisation of the pwm

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 17:f54cdc9ae52f 268 void Home()
pyrostew 17:f54cdc9ae52f 269 {
pyrostew 17:f54cdc9ae52f 270 if (!Enabled)
pyrostew 17:f54cdc9ae52f 271 {
pyrostew 17:f54cdc9ae52f 272 Enable();
pyrostew 17:f54cdc9ae52f 273 }
pyrostew 17:f54cdc9ae52f 274
pyrostew 17:f54cdc9ae52f 275 if (HallSensorState == 1)
pyrostew 17:f54cdc9ae52f 276 {
pyrostew 17:f54cdc9ae52f 277 Homing = true;
pyrostew 17:f54cdc9ae52f 278 HallTriggered = false;
pyrostew 17:f54cdc9ae52f 279
pyrostew 17:f54cdc9ae52f 280 SetPower(-0.6);
pyrostew 17:f54cdc9ae52f 281
pyrostew 17:f54cdc9ae52f 282 while (!HallTriggered)
pyrostew 17:f54cdc9ae52f 283 {
pyrostew 18:ab282713f4a7 284 wait(0.1);
pyrostew 17:f54cdc9ae52f 285 }
pyrostew 17:f54cdc9ae52f 286 }
pyrostew 17:f54cdc9ae52f 287
pyrostew 17:f54cdc9ae52f 288 SetPower(1.0);
pyrostew 17:f54cdc9ae52f 289
pyrostew 17:f54cdc9ae52f 290 while (position < 2000)
pyrostew 17:f54cdc9ae52f 291 {
pyrostew 17:f54cdc9ae52f 292 //SerialTransmit();
pyrostew 17:f54cdc9ae52f 293 }
pyrostew 17:f54cdc9ae52f 294
pyrostew 17:f54cdc9ae52f 295 Homing = true;
pyrostew 17:f54cdc9ae52f 296 HallTriggered = false;
pyrostew 17:f54cdc9ae52f 297
pyrostew 17:f54cdc9ae52f 298 SetPower(-0.4);
pyrostew 17:f54cdc9ae52f 299
pyrostew 17:f54cdc9ae52f 300 while (!HallTriggered)
pyrostew 17:f54cdc9ae52f 301 {
pyrostew 18:ab282713f4a7 302 wait(0.1);
pyrostew 17:f54cdc9ae52f 303 }
pyrostew 17:f54cdc9ae52f 304 }
pyrostew 17:f54cdc9ae52f 305
pyrostew 0:398432a37ca5 306 void HallEffectFall()
pyrostew 0:398432a37ca5 307 {
pyrostew 0:398432a37ca5 308 RGHSinInterrupt.disable_irq();
pyrostew 0:398432a37ca5 309 RGHCosInterrupt.disable_irq();
pyrostew 15:cd409a54ceec 310 RGHSinFallingInterrupt.disable_irq();
pyrostew 15:cd409a54ceec 311 RGHCosFallingInterrupt.disable_irq();
pyrostew 0:398432a37ca5 312
pyrostew 0:398432a37ca5 313 if (direction < 0)
pyrostew 0:398432a37ca5 314 {
pyrostew 0:398432a37ca5 315 SetPower(0.0);
pyrostew 0:398432a37ca5 316
pyrostew 0:398432a37ca5 317 if (Homing)
pyrostew 0:398432a37ca5 318 {
pyrostew 0:398432a37ca5 319 HallTriggered = true;
pyrostew 0:398432a37ca5 320 Homing = false;
pyrostew 0:398432a37ca5 321 position = 0.0;
pyrostew 0:398432a37ca5 322 }
pyrostew 0:398432a37ca5 323 }
pyrostew 0:398432a37ca5 324 RGHSinInterrupt.enable_irq();
pyrostew 0:398432a37ca5 325 RGHCosInterrupt.enable_irq();
pyrostew 15:cd409a54ceec 326 RGHSinFallingInterrupt.enable_irq();
pyrostew 15:cd409a54ceec 327 RGHCosFallingInterrupt.enable_irq();
pyrostew 0:398432a37ca5 328 }
pyrostew 0:398432a37ca5 329
alpesh 13:18c376e5dc9a 330
pyrostew 18:ab282713f4a7 331 double SetPoint = 70.0; //Target Position in Millimeter per second
alpesh 6:bfe745b152fa 332
alpesh 13:18c376e5dc9a 333 double PosProError;
alpesh 13:18c376e5dc9a 334 double PosIntError;
alpesh 13:18c376e5dc9a 335 double PosDifError;
alpesh 13:18c376e5dc9a 336
alpesh 19:a6369257c00f 337 //double VelProError;
alpesh 19:a6369257c00f 338 //double VelIntError;
alpesh 19:a6369257c00f 339 //double VelDifError;
alpesh 2:d1805e7d46fb 340
alpesh 3:e693c65b04de 341 int errorcounter;
alpesh 13:18c376e5dc9a 342 double PosPreviousError [10];
alpesh 19:a6369257c00f 343 //double VelPreviousError [10];
alpesh 2:d1805e7d46fb 344
alpesh 3:e693c65b04de 345 double PwmChange=0;
alpesh 3:e693c65b04de 346
alpesh 3:e693c65b04de 347 double pwm;
alpesh 13:18c376e5dc9a 348 double TargetPwm;
alpesh 20:04432d03b46c 349 double newTargetPwm;
alpesh 13:18c376e5dc9a 350 double TargetVelocity;
alpesh 3:e693c65b04de 351
alpesh 13:18c376e5dc9a 352 double PosiState;
alpesh 19:a6369257c00f 353 //double VeliState;
alpesh 13:18c376e5dc9a 354
alpesh 13:18c376e5dc9a 355 int PreviousTime = 0;
alpesh 13:18c376e5dc9a 356
alpesh 20:04432d03b46c 357 double vMax = 300;
alpesh 20:04432d03b46c 358 double vStep = 50;
alpesh 20:04432d03b46c 359 double pStep = 0.5;
alpesh 3:e693c65b04de 360
alpesh 4:2ec05810bc47 361 void Controller ()
pyrostew 12:814db1249a19 362 {
alpesh 13:18c376e5dc9a 363
alpesh 13:18c376e5dc9a 364 ///////////////////////////////////////////////////////////////////////////////////////////////
alpesh 13:18c376e5dc9a 365 //Position PID
alpesh 13:18c376e5dc9a 366 ///////////////////////////////////////////////////////////////////////////////////////////////
alpesh 13:18c376e5dc9a 367
alpesh 13:18c376e5dc9a 368 //if (position <= SetPoint || 1)
alpesh 13:18c376e5dc9a 369
pyrostew 12:814db1249a19 370
alpesh 13:18c376e5dc9a 371 //{
alpesh 13:18c376e5dc9a 372 int timeStep = RunningTime.read_us() - PreviousTime;
alpesh 13:18c376e5dc9a 373 PreviousTime = RunningTime.read_us();
alpesh 13:18c376e5dc9a 374
alpesh 19:a6369257c00f 375 double integral_velmax = vMax/PosKiGain;
alpesh 19:a6369257c00f 376 double integral_velmin = -vMax/PosKiGain ;
alpesh 13:18c376e5dc9a 377
pyrostew 17:f54cdc9ae52f 378 PosError = SetPoint - (position * 0.01);
alpesh 13:18c376e5dc9a 379
alpesh 13:18c376e5dc9a 380 PosProError = PosError * PosKpGain;
pyrostew 12:814db1249a19 381
alpesh 13:18c376e5dc9a 382 PosDifError = (PosError - PosPreviousError[errorcounter]) / timeStep;
alpesh 13:18c376e5dc9a 383
alpesh 13:18c376e5dc9a 384 PosiState += PosError;
alpesh 13:18c376e5dc9a 385
alpesh 13:18c376e5dc9a 386 if (PosiState > integral_velmax)
alpesh 13:18c376e5dc9a 387 {
alpesh 13:18c376e5dc9a 388 PosiState = integral_velmax;
alpesh 13:18c376e5dc9a 389 }
alpesh 13:18c376e5dc9a 390 else if (PosiState < integral_velmin)
alpesh 13:18c376e5dc9a 391 {
alpesh 13:18c376e5dc9a 392 PosiState = integral_velmin;
alpesh 13:18c376e5dc9a 393 }
alpesh 13:18c376e5dc9a 394 PosIntError = PosKiGain * PosiState;
alpesh 13:18c376e5dc9a 395
alpesh 19:a6369257c00f 396 TargetPwm = (PosKpGain * PosError + PosKdGain * PosDifError + PosIntError);
alpesh 19:a6369257c00f 397
alpesh 20:04432d03b46c 398 if (TargetPwm < pStep)
alpesh 13:18c376e5dc9a 399
alpesh 20:04432d03b46c 400 {
alpesh 20:04432d03b46c 401 newTargetPwm = (TargetPwm * 300) / 100;
alpesh 20:04432d03b46c 402 }
alpesh 13:18c376e5dc9a 403
alpesh 20:04432d03b46c 404 if (TargetPwm > pStep)
alpesh 20:04432d03b46c 405 {
alpesh 20:04432d03b46c 406 newTargetPwm = pStep + (TargetPwm - pStep) * 100 / 500;
alpesh 20:04432d03b46c 407 }
alpesh 13:18c376e5dc9a 408
alpesh 20:04432d03b46c 409 if (newTargetPwm > 1.0)
pyrostew 12:814db1249a19 410 {
pyrostew 12:814db1249a19 411 pwm = 1.0;
pyrostew 12:814db1249a19 412 }
pyrostew 12:814db1249a19 413
alpesh 20:04432d03b46c 414 else if (newTargetPwm < -1.0)
pyrostew 12:814db1249a19 415 {
pyrostew 12:814db1249a19 416 pwm = -1.0;
pyrostew 12:814db1249a19 417 }
pyrostew 12:814db1249a19 418
pyrostew 12:814db1249a19 419 else
pyrostew 12:814db1249a19 420 {
alpesh 20:04432d03b46c 421 pwm = newTargetPwm;
pyrostew 12:814db1249a19 422 }
pyrostew 12:814db1249a19 423
pyrostew 12:814db1249a19 424 SetPower(pwm);
alpesh 4:2ec05810bc47 425
pyrostew 12:814db1249a19 426 errorcounter ++;
pyrostew 12:814db1249a19 427
pyrostew 12:814db1249a19 428 if (errorcounter > 9)
pyrostew 12:814db1249a19 429 {
pyrostew 12:814db1249a19 430 errorcounter = 0;
pyrostew 12:814db1249a19 431 }
pyrostew 12:814db1249a19 432
alpesh 13:18c376e5dc9a 433 PosPreviousError[errorcounter] = PosError;
alpesh 19:a6369257c00f 434 // VelPreviousError[errorcounter] = VelError;
alpesh 13:18c376e5dc9a 435 // }
alpesh 13:18c376e5dc9a 436
pyrostew 12:814db1249a19 437 }
alpesh 4:2ec05810bc47 438
pyrostew 0:398432a37ca5 439 int main()
alpesh 2:d1805e7d46fb 440
pyrostew 0:398432a37ca5 441 {
pyrostew 15:cd409a54ceec 442 RGHSinInterrupt.rise(&RGHSinRisingHandler);
pyrostew 15:cd409a54ceec 443 RGHCosInterrupt.rise(&RGHCosRisingHandler);
pyrostew 15:cd409a54ceec 444 RGHSinFallingInterrupt.fall(&RGHSinFallingHandler);
pyrostew 15:cd409a54ceec 445 RGHCosFallingInterrupt.fall(&RGHCosFallingHandler);
pyrostew 0:398432a37ca5 446 HallSensor.fall(&HallEffectFall);
pyrostew 0:398432a37ca5 447 HallSensor.mode(PullUp);
pyrostew 0:398432a37ca5 448
pyrostew 17:f54cdc9ae52f 449 RGHSinFallingInterrupt.mode(PullNone);
pyrostew 17:f54cdc9ae52f 450 RGHCosFallingInterrupt.mode(PullNone);
pyrostew 17:f54cdc9ae52f 451
pyrostew 0:398432a37ca5 452 RunningTime.start();
pyrostew 0:398432a37ca5 453
pyrostew 0:398432a37ca5 454 pc.baud(115200);
pyrostew 0:398432a37ca5 455
pyrostew 0:398432a37ca5 456 Home();
alpesh 4:2ec05810bc47 457 //Enable();
pyrostew 0:398432a37ca5 458
alpesh 3:e693c65b04de 459 errorcounter = 0;
alpesh 13:18c376e5dc9a 460 PosPreviousError[errorcounter]=0;
alpesh 2:d1805e7d46fb 461
alpesh 13:18c376e5dc9a 462 PreviousTime = RunningTime.read_us();
alpesh 3:e693c65b04de 463
pyrostew 17:f54cdc9ae52f 464 while(Enabled)
alpesh 2:d1805e7d46fb 465 {
pyrostew 17:f54cdc9ae52f 466 //double pow = 0.4;
pyrostew 17:f54cdc9ae52f 467 //while(pow < 1.0)
pyrostew 18:ab282713f4a7 468 PosKpGain = 10.0;
pyrostew 18:ab282713f4a7 469 while(PosKpGain < 50.0)
pyrostew 0:398432a37ca5 470 {
pyrostew 17:f54cdc9ae52f 471 //pow += 0.05;
pyrostew 18:ab282713f4a7 472 PosKpGain += 1.0;
pyrostew 17:f54cdc9ae52f 473
alpesh 19:a6369257c00f 474 // VelKpGain = 0.003;
alpesh 19:a6369257c00f 475 // while (VelKpGain < 0.008)
alpesh 19:a6369257c00f 476 // {
alpesh 19:a6369257c00f 477 // VelKpGain += 0.001;
pyrostew 18:ab282713f4a7 478
pyrostew 18:ab282713f4a7 479 float iterationStart = RunningTime.read();
pyrostew 17:f54cdc9ae52f 480
pyrostew 18:ab282713f4a7 481 while(RunningTime.read()-iterationStart < 10.0)
pyrostew 18:ab282713f4a7 482 {
pyrostew 18:ab282713f4a7 483 SerialTransmit();
pyrostew 18:ab282713f4a7 484
pyrostew 18:ab282713f4a7 485 Controller();
pyrostew 18:ab282713f4a7 486 }
pyrostew 18:ab282713f4a7 487
pyrostew 18:ab282713f4a7 488 Home();
alpesh 19:a6369257c00f 489 // }
pyrostew 0:398432a37ca5 490 }
alpesh 2:d1805e7d46fb 491
alpesh 6:bfe745b152fa 492 Disable();
pyrostew 17:f54cdc9ae52f 493 }
alpesh 7:4cd7be306626 494 }
alpesh 7:4cd7be306626 495
alpesh 7:4cd7be306626 496 /* Change this throttle value range to 1.0 and 0.0 for car speed
alpesh 13:18c376e5dc9a 497 m_throttlechange = (m_kpGain * m_error + m_kdGain * m_PosDifError + m_PosIntError);
alpesh 7:4cd7be306626 498
alpesh 7:4cd7be306626 499 double throttle = m_throttle + m_throttlechange;
alpesh 7:4cd7be306626 500
alpesh 7:4cd7be306626 501 if (new_throttle > 1.0) {
alpesh 7:4cd7be306626 502 m_throttle = 1.0;
alpesh 7:4cd7be306626 503 } else if (new_throttle < 0.0) {
alpesh 7:4cd7be306626 504 m_throttle = 0.0;
alpesh 7:4cd7be306626 505 } else {
alpesh 7:4cd7be306626 506 m_throttle = new_throttle;
alpesh 7:4cd7be306626 507 }*/
alpesh 7:4cd7be306626 508
alpesh 7:4cd7be306626 509
alpesh 7:4cd7be306626 510
alpesh 7:4cd7be306626 511
alpesh 7:4cd7be306626 512