Alpesh Bhudia / Mbed 2 deprecated EquatorStrutDigitalMonitor

Merged to branch

Dependencies:   USBDevice mbed EquatorStrutController LightWeightSerialTransmit

Fork of EquatorStrutDigitalMonitor by Stewart Coulden-Smith

main.cpp@17:f54cdc9ae52f, 2014-08-14 (annotated)

Committer:
pyrostew
Date:
Thu Aug 14 09:20:50 2014 +0000
Revision:
17:f54cdc9ae52f
Parent:
16:47d761226df6
Child:
18:ab282713f4a7
Working encoder readings with updated speed code.

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