Alpesh Bhudia / Mbed 2 deprecated EquatorStrutDigitalMonitor

Merged to branch

Dependencies:   USBDevice mbed EquatorStrutController LightWeightSerialTransmit

Fork of EquatorStrutDigitalMonitor by Stewart Coulden-Smith

main.cpp@13:18c376e5dc9a, 2014-08-13 (annotated)

Committer:
alpesh
Date:
Wed Aug 13 08:31:24 2014 +0000
Revision:
13:18c376e5dc9a
Parent:
12:814db1249a19
Child:
14:67466da6663d
Implemented 2 PID (Position and Velocity). Also, clamping mechanism for Integral Term

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 0:398432a37ca5 4 DigitalIn RGHSinState(P0_11);
pyrostew 0:398432a37ca5 5 DigitalIn RGHCosState(P0_12);
pyrostew 12:814db1249a19 6 DigitalIn HallSensorState(P0_2);
pyrostew 0:398432a37ca5 7 InterruptIn RGHSinInterrupt(P0_11);
pyrostew 0:398432a37ca5 8 InterruptIn RGHCosInterrupt(P0_12);
pyrostew 0:398432a37ca5 9 InterruptIn HallSensor(P0_2);
pyrostew 0:398432a37ca5 10 DigitalOut ResetLine(P1_29);
pyrostew 0:398432a37ca5 11 PwmOut PhaseA(P0_9);
pyrostew 0:398432a37ca5 12 PwmOut PhaseB(P0_8);
pyrostew 0:398432a37ca5 13 Timer RunningTime;
pyrostew 0:398432a37ca5 14
pyrostew 0:398432a37ca5 15 bool Enabled = false;
pyrostew 12:814db1249a19 16 volatile bool Homing = false;
pyrostew 12:814db1249a19 17 volatile bool HallTriggered = false;
pyrostew 0:398432a37ca5 18
pyrostew 0:398432a37ca5 19 RawSerial pc(P1_27, P1_26);
pyrostew 0:398432a37ca5 20
pyrostew 12:814db1249a19 21 volatile char PinState = 0;
pyrostew 0:398432a37ca5 22
pyrostew 12:814db1249a19 23 volatile int direction = 0;
pyrostew 12:814db1249a19 24 volatile double position = 0.0;
pyrostew 0:398432a37ca5 25 double currentPower = 0.0;
pyrostew 12:814db1249a19 26 volatile int lastTime = 0;
pyrostew 0:398432a37ca5 27
pyrostew 12:814db1249a19 28 const int arraySize = 50;
pyrostew 12:814db1249a19 29 volatile int intteruptPeriodArray[arraySize];
pyrostew 12:814db1249a19 30 volatile int arrayTotal = 0;
pyrostew 12:814db1249a19 31 volatile char arrayPos = 0;
pyrostew 12:814db1249a19 32 volatile int interruptPeriod = 0;
pyrostew 10:088eeae4287c 33
pyrostew 0:398432a37ca5 34 char counter = 0;
pyrostew 0:398432a37ca5 35
pyrostew 10:088eeae4287c 36 void SmoothingAdd(int input)
pyrostew 10:088eeae4287c 37 {
pyrostew 12:814db1249a19 38 //arrayTotal -= intteruptPeriodArray[arrayPos];
pyrostew 12:814db1249a19 39 //arrayTotal += input;
pyrostew 12:814db1249a19 40 intteruptPeriodArray[arrayPos] = input * direction;
pyrostew 10:088eeae4287c 41
pyrostew 12:814db1249a19 42 if (arrayPos == arraySize-1)
pyrostew 10:088eeae4287c 43 {
pyrostew 10:088eeae4287c 44 arrayPos = 0;
pyrostew 10:088eeae4287c 45 }
pyrostew 10:088eeae4287c 46 else
pyrostew 10:088eeae4287c 47 {
pyrostew 12:814db1249a19 48 arrayPos++;
pyrostew 10:088eeae4287c 49 }
pyrostew 12:814db1249a19 50
pyrostew 12:814db1249a19 51 //interruptPeriod = arrayTotal / 15;
pyrostew 10:088eeae4287c 52 }
pyrostew 10:088eeae4287c 53
pyrostew 10:088eeae4287c 54 int SmoothedInterruptPeriod()
pyrostew 10:088eeae4287c 55 {
pyrostew 12:814db1249a19 56 int arrayTotal = 0;
pyrostew 12:814db1249a19 57
pyrostew 12:814db1249a19 58 for (char i = 0; i < arraySize; i++)
pyrostew 12:814db1249a19 59 {
pyrostew 12:814db1249a19 60 arrayTotal += intteruptPeriodArray[i];
pyrostew 12:814db1249a19 61 }
pyrostew 12:814db1249a19 62
pyrostew 12:814db1249a19 63 return arrayTotal / arraySize;
pyrostew 10:088eeae4287c 64 }
pyrostew 10:088eeae4287c 65
pyrostew 0:398432a37ca5 66 void RGHSinHandler()
pyrostew 0:398432a37ca5 67 {
pyrostew 0:398432a37ca5 68 if (PinState == 2)
pyrostew 0:398432a37ca5 69 {
pyrostew 0:398432a37ca5 70 return;
pyrostew 0:398432a37ca5 71 }
pyrostew 0:398432a37ca5 72 else if (PinState == 1)
pyrostew 0:398432a37ca5 73 {
pyrostew 0:398432a37ca5 74 PinState = 0 |(RGHSinState << 1) | RGHCosState;
pyrostew 0:398432a37ca5 75
pyrostew 0:398432a37ca5 76 if(PinState == 3)
pyrostew 0:398432a37ca5 77 {
pyrostew 0:398432a37ca5 78 direction = 1;
pyrostew 0:398432a37ca5 79 position += 0.04 * direction;
pyrostew 10:088eeae4287c 80 SmoothingAdd(RunningTime.read_us() - lastTime);
pyrostew 12:814db1249a19 81 interruptPeriod = RunningTime.read_us() - lastTime;
pyrostew 1:a33723b70582 82 lastTime = RunningTime.read_us();
pyrostew 0:398432a37ca5 83 }
pyrostew 0:398432a37ca5 84 }
pyrostew 0:398432a37ca5 85 else
pyrostew 0:398432a37ca5 86 {
pyrostew 0:398432a37ca5 87 PinState = 0 |(RGHSinState << 1) | RGHCosState;
pyrostew 0:398432a37ca5 88 }
pyrostew 0:398432a37ca5 89 }
pyrostew 0:398432a37ca5 90
pyrostew 0:398432a37ca5 91 void RGHCosHandler()
pyrostew 0:398432a37ca5 92 {
pyrostew 0:398432a37ca5 93 if (PinState == 1)
pyrostew 0:398432a37ca5 94 {
pyrostew 0:398432a37ca5 95 return;
pyrostew 0:398432a37ca5 96 }
pyrostew 0:398432a37ca5 97 else if (PinState == 2)
pyrostew 0:398432a37ca5 98 {
pyrostew 0:398432a37ca5 99 PinState = 0 |(RGHSinState << 1) | RGHCosState;
pyrostew 0:398432a37ca5 100
pyrostew 0:398432a37ca5 101 if (PinState == 3)
pyrostew 0:398432a37ca5 102 {
pyrostew 0:398432a37ca5 103 direction = -1;
pyrostew 0:398432a37ca5 104 position += 0.04 * direction;
pyrostew 10:088eeae4287c 105 SmoothingAdd(RunningTime.read_us() - lastTime);
pyrostew 12:814db1249a19 106 interruptPeriod = RunningTime.read_us() - lastTime;
pyrostew 1:a33723b70582 107 lastTime = RunningTime.read_us();
pyrostew 0:398432a37ca5 108 }
pyrostew 0:398432a37ca5 109 }
pyrostew 0:398432a37ca5 110 else
pyrostew 0:398432a37ca5 111 {
pyrostew 0:398432a37ca5 112 PinState = 0 |(RGHSinState << 1) | RGHCosState;
pyrostew 0:398432a37ca5 113 }
pyrostew 0:398432a37ca5 114 }
pyrostew 0:398432a37ca5 115
pyrostew 0:398432a37ca5 116 void SetPower(double power)
pyrostew 0:398432a37ca5 117 {
pyrostew 0:398432a37ca5 118 currentPower = power;
pyrostew 0:398432a37ca5 119 if(!Enabled)
pyrostew 0:398432a37ca5 120 {
pyrostew 0:398432a37ca5 121 return;
pyrostew 0:398432a37ca5 122 }
pyrostew 0:398432a37ca5 123
pyrostew 0:398432a37ca5 124 if (power > 1.0 || power < -1.0)
pyrostew 0:398432a37ca5 125 {
pyrostew 0:398432a37ca5 126 return;
pyrostew 0:398432a37ca5 127 }
pyrostew 0:398432a37ca5 128
pyrostew 0:398432a37ca5 129 PhaseA = (power + 1.0) / 2;
pyrostew 0:398432a37ca5 130 PhaseB = 1.0 - ((power + 1.0) / 2);
pyrostew 0:398432a37ca5 131 }
pyrostew 0:398432a37ca5 132
pyrostew 0:398432a37ca5 133 void Enable()
pyrostew 0:398432a37ca5 134 {
pyrostew 0:398432a37ca5 135 SetPower(0.0);
pyrostew 0:398432a37ca5 136
pyrostew 0:398432a37ca5 137 ResetLine = 1;
pyrostew 0:398432a37ca5 138
pyrostew 0:398432a37ca5 139 Enabled = true;
pyrostew 0:398432a37ca5 140 }
pyrostew 0:398432a37ca5 141
pyrostew 0:398432a37ca5 142 void Disable()
pyrostew 0:398432a37ca5 143 {
pyrostew 0:398432a37ca5 144 ResetLine = 0;
pyrostew 0:398432a37ca5 145
pyrostew 0:398432a37ca5 146 SetPower(0.0);
pyrostew 0:398432a37ca5 147
pyrostew 0:398432a37ca5 148 Enabled = false;
pyrostew 0:398432a37ca5 149 }
pyrostew 0:398432a37ca5 150
pyrostew 0:398432a37ca5 151 void Home()
pyrostew 0:398432a37ca5 152 {
pyrostew 0:398432a37ca5 153 if (!Enabled)
pyrostew 0:398432a37ca5 154 {
pyrostew 0:398432a37ca5 155 Enable();
pyrostew 0:398432a37ca5 156 }
pyrostew 0:398432a37ca5 157
pyrostew 12:814db1249a19 158 if (HallSensorState == 1)
pyrostew 0:398432a37ca5 159 {
pyrostew 12:814db1249a19 160 Homing = true;
pyrostew 12:814db1249a19 161 HallTriggered = false;
pyrostew 12:814db1249a19 162
pyrostew 12:814db1249a19 163 SetPower(-0.6);
pyrostew 12:814db1249a19 164
pyrostew 12:814db1249a19 165 while (!HallTriggered)
pyrostew 12:814db1249a19 166 {
pyrostew 12:814db1249a19 167 wait(0.5);
pyrostew 12:814db1249a19 168 }
pyrostew 0:398432a37ca5 169 }
pyrostew 0:398432a37ca5 170
pyrostew 0:398432a37ca5 171 SetPower(1.0);
pyrostew 0:398432a37ca5 172
pyrostew 0:398432a37ca5 173 while (position < 20.0)
pyrostew 0:398432a37ca5 174 {
pyrostew 0:398432a37ca5 175
pyrostew 0:398432a37ca5 176 }
pyrostew 0:398432a37ca5 177
pyrostew 0:398432a37ca5 178 Homing = true;
pyrostew 0:398432a37ca5 179 HallTriggered = false;
pyrostew 0:398432a37ca5 180
pyrostew 12:814db1249a19 181 SetPower(-0.4);
pyrostew 0:398432a37ca5 182
pyrostew 0:398432a37ca5 183 while (!HallTriggered)
pyrostew 0:398432a37ca5 184 {
pyrostew 0:398432a37ca5 185 wait(0.5);
pyrostew 0:398432a37ca5 186 }
pyrostew 0:398432a37ca5 187 }
pyrostew 0:398432a37ca5 188
pyrostew 1:a33723b70582 189 double GetSpeed()
pyrostew 1:a33723b70582 190 {
pyrostew 12:814db1249a19 191 //if ((RunningTime.read_us() - lastTime) > 1000 || SmoothedInterruptPeriod() == 0)
pyrostew 12:814db1249a19 192 //{
pyrostew 12:814db1249a19 193 // return 0.0;
pyrostew 12:814db1249a19 194 //}
pyrostew 12:814db1249a19 195 return (0.04)/((double)SmoothedInterruptPeriod() / 1000000.0);
pyrostew 12:814db1249a19 196 //return SmoothedInterruptPeriod();
pyrostew 1:a33723b70582 197 }
pyrostew 1:a33723b70582 198
alpesh 13:18c376e5dc9a 199 double PosError = 0; //This has been defined here as it's being used in the serial transmit function
alpesh 13:18c376e5dc9a 200 double VelError = 0;
alpesh 6:bfe745b152fa 201
pyrostew 12:814db1249a19 202 void SerialOut(double outputValue)
pyrostew 12:814db1249a19 203 {
pyrostew 0:398432a37ca5 204 int outChar = 0;
pyrostew 0:398432a37ca5 205
pyrostew 12:814db1249a19 206 if (outputValue < 0.0)
pyrostew 0:398432a37ca5 207 {
pyrostew 0:398432a37ca5 208 pc.putc('-');
pyrostew 12:814db1249a19 209 outputValue *= -1.0;
pyrostew 0:398432a37ca5 210 }
pyrostew 12:814db1249a19 211 if (outputValue >= 100.0)
pyrostew 0:398432a37ca5 212 {
pyrostew 12:814db1249a19 213 outChar = outputValue / 100;
pyrostew 0:398432a37ca5 214 pc.putc(outChar + 48);
pyrostew 12:814db1249a19 215 outputValue -= outChar * 100.0;
pyrostew 0:398432a37ca5 216 }
pyrostew 12:814db1249a19 217 if (outputValue >= 10.0)
pyrostew 0:398432a37ca5 218 {
pyrostew 12:814db1249a19 219 outChar = outputValue / 10;
pyrostew 0:398432a37ca5 220 pc.putc(outChar + 48);
pyrostew 12:814db1249a19 221 outputValue -= outChar * 10.0;
pyrostew 0:398432a37ca5 222 }
pyrostew 0:398432a37ca5 223 else if(outChar > 0)
pyrostew 0:398432a37ca5 224 {
pyrostew 0:398432a37ca5 225 pc.putc('0');
pyrostew 0:398432a37ca5 226 }
pyrostew 12:814db1249a19 227 if (outputValue >= 1.0)
pyrostew 0:398432a37ca5 228 {
pyrostew 12:814db1249a19 229 outChar = outputValue;
pyrostew 0:398432a37ca5 230 pc.putc(outChar + 48);
pyrostew 12:814db1249a19 231 outputValue -= outChar;
pyrostew 0:398432a37ca5 232 }
pyrostew 0:398432a37ca5 233 else
pyrostew 0:398432a37ca5 234 {
pyrostew 0:398432a37ca5 235 pc.putc('0');
pyrostew 0:398432a37ca5 236 }
pyrostew 12:814db1249a19 237 if (outputValue >= 0.1)
pyrostew 0:398432a37ca5 238 {
pyrostew 0:398432a37ca5 239 pc.putc('.');
pyrostew 12:814db1249a19 240 outChar = outputValue * 10;
pyrostew 0:398432a37ca5 241 pc.putc(outChar + 48);
pyrostew 12:814db1249a19 242 outputValue -= (double)outChar / 10.0;
pyrostew 0:398432a37ca5 243 }
pyrostew 0:398432a37ca5 244 else
pyrostew 0:398432a37ca5 245 {
pyrostew 0:398432a37ca5 246 pc.putc('.');
pyrostew 0:398432a37ca5 247 pc.putc('0');
pyrostew 0:398432a37ca5 248 }
pyrostew 12:814db1249a19 249 if (outputValue >= 0.01)
pyrostew 0:398432a37ca5 250 {
pyrostew 12:814db1249a19 251 outChar = outputValue * 100;
pyrostew 0:398432a37ca5 252 pc.putc(outChar + 48);
pyrostew 12:814db1249a19 253 outputValue -= (double)outChar / 100.0;
pyrostew 0:398432a37ca5 254 }
pyrostew 0:398432a37ca5 255 else
pyrostew 0:398432a37ca5 256 {
pyrostew 0:398432a37ca5 257 pc.putc('0');
pyrostew 0:398432a37ca5 258 }
pyrostew 12:814db1249a19 259 if (outputValue >= 0.001)
pyrostew 0:398432a37ca5 260 {
pyrostew 12:814db1249a19 261 outChar= outputValue * 1000;
pyrostew 0:398432a37ca5 262 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 263 }
pyrostew 12:814db1249a19 264 }
pyrostew 12:814db1249a19 265
alpesh 13:18c376e5dc9a 266 double PosKpGain = 0.0;
alpesh 13:18c376e5dc9a 267 double PosKiGain = 0.0;
alpesh 13:18c376e5dc9a 268 double PosKdGain = 0.0;
alpesh 13:18c376e5dc9a 269
alpesh 13:18c376e5dc9a 270 double VelKpGain = 0.0;
alpesh 13:18c376e5dc9a 271 double VelKiGain = 0.0;
alpesh 13:18c376e5dc9a 272 double VelKdGain = 0.0;
pyrostew 12:814db1249a19 273
pyrostew 12:814db1249a19 274 void SerialTransmit()
pyrostew 12:814db1249a19 275 {
pyrostew 12:814db1249a19 276 SerialOut(RunningTime.read());
pyrostew 12:814db1249a19 277
pyrostew 12:814db1249a19 278 pc.putc('\t');
pyrostew 12:814db1249a19 279
pyrostew 12:814db1249a19 280 SerialOut(position);
pyrostew 0:398432a37ca5 281
alpesh 2:d1805e7d46fb 282 pc.putc('\t');
pyrostew 12:814db1249a19 283
pyrostew 12:814db1249a19 284 SerialOut(currentPower);
pyrostew 1:a33723b70582 285
pyrostew 12:814db1249a19 286 pc.putc('\t');
pyrostew 12:814db1249a19 287
pyrostew 12:814db1249a19 288 SerialOut(GetSpeed());
pyrostew 1:a33723b70582 289
alpesh 6:bfe745b152fa 290 pc.putc('\t');
pyrostew 12:814db1249a19 291
alpesh 13:18c376e5dc9a 292 SerialOut(PosError);
pyrostew 12:814db1249a19 293
pyrostew 12:814db1249a19 294 pc.putc('\t');
pyrostew 12:814db1249a19 295
alpesh 13:18c376e5dc9a 296 SerialOut(PosKpGain);
alpesh 6:bfe745b152fa 297
pyrostew 12:814db1249a19 298 pc.putc('\t');
pyrostew 12:814db1249a19 299
alpesh 13:18c376e5dc9a 300 SerialOut(PosKiGain);
pyrostew 12:814db1249a19 301
pyrostew 12:814db1249a19 302 pc.putc('\t');
pyrostew 12:814db1249a19 303
alpesh 13:18c376e5dc9a 304 SerialOut(PosKdGain);
alpesh 6:bfe745b152fa 305
pyrostew 0:398432a37ca5 306 pc.putc(10);
pyrostew 0:398432a37ca5 307 pc.putc(13);
pyrostew 0:398432a37ca5 308 }
pyrostew 0:398432a37ca5 309
pyrostew 0:398432a37ca5 310 void HallEffectFall()
pyrostew 0:398432a37ca5 311 {
pyrostew 0:398432a37ca5 312 RGHSinInterrupt.disable_irq();
pyrostew 0:398432a37ca5 313 RGHCosInterrupt.disable_irq();
pyrostew 0:398432a37ca5 314
pyrostew 0:398432a37ca5 315 if (direction < 0)
pyrostew 0:398432a37ca5 316 {
pyrostew 0:398432a37ca5 317 SetPower(0.0);
pyrostew 0:398432a37ca5 318
pyrostew 0:398432a37ca5 319 if (Homing)
pyrostew 0:398432a37ca5 320 {
pyrostew 0:398432a37ca5 321 HallTriggered = true;
pyrostew 0:398432a37ca5 322 Homing = false;
pyrostew 0:398432a37ca5 323 position = 0.0;
pyrostew 0:398432a37ca5 324 }
pyrostew 0:398432a37ca5 325 }
pyrostew 0:398432a37ca5 326 RGHSinInterrupt.enable_irq();
pyrostew 0:398432a37ca5 327 RGHCosInterrupt.enable_irq();
pyrostew 0:398432a37ca5 328 }
pyrostew 0:398432a37ca5 329
alpesh 13:18c376e5dc9a 330
alpesh 13:18c376e5dc9a 331 double SetPoint = 50.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 13:18c376e5dc9a 337 double VelProError;
alpesh 13:18c376e5dc9a 338 double VelIntError;
alpesh 13:18c376e5dc9a 339 double VelDifError;
alpesh 2:d1805e7d46fb 340
alpesh 3:e693c65b04de 341 int errorcounter;
alpesh 13:18c376e5dc9a 342 double PosPreviousError [10];
alpesh 13:18c376e5dc9a 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 13:18c376e5dc9a 349 double TargetVelocity;
alpesh 3:e693c65b04de 350
alpesh 13:18c376e5dc9a 351 double PosiState;
alpesh 13:18c376e5dc9a 352 double VeliState;
alpesh 13:18c376e5dc9a 353
alpesh 13:18c376e5dc9a 354 int PreviousTime = 0;
alpesh 13:18c376e5dc9a 355
alpesh 13:18c376e5dc9a 356 double vMax = 60;
alpesh 3:e693c65b04de 357
alpesh 4:2ec05810bc47 358 void Controller ()
pyrostew 12:814db1249a19 359 {
alpesh 13:18c376e5dc9a 360
alpesh 13:18c376e5dc9a 361 ///////////////////////////////////////////////////////////////////////////////////////////////
alpesh 13:18c376e5dc9a 362 //Position PID
alpesh 13:18c376e5dc9a 363 ///////////////////////////////////////////////////////////////////////////////////////////////
alpesh 13:18c376e5dc9a 364
alpesh 13:18c376e5dc9a 365 //if (position <= SetPoint || 1)
alpesh 13:18c376e5dc9a 366
pyrostew 12:814db1249a19 367
alpesh 13:18c376e5dc9a 368 //{
alpesh 13:18c376e5dc9a 369 int timeStep = RunningTime.read_us() - PreviousTime;
alpesh 13:18c376e5dc9a 370 PreviousTime = RunningTime.read_us();
alpesh 13:18c376e5dc9a 371
alpesh 13:18c376e5dc9a 372 double integral_velmax = vMax/VelKiGain;
alpesh 13:18c376e5dc9a 373 double integral_velmin = -vMax/VelKiGain ;
alpesh 13:18c376e5dc9a 374
alpesh 13:18c376e5dc9a 375 PosError = SetPoint - position;
alpesh 13:18c376e5dc9a 376
alpesh 13:18c376e5dc9a 377 PosProError = PosError * PosKpGain;
alpesh 13:18c376e5dc9a 378
alpesh 13:18c376e5dc9a 379 PosDifError = (PosError - PosPreviousError[errorcounter]) / timeStep;
pyrostew 12:814db1249a19 380
alpesh 13:18c376e5dc9a 381
alpesh 13:18c376e5dc9a 382 PosiState += PosError;
alpesh 13:18c376e5dc9a 383
alpesh 13:18c376e5dc9a 384 if (PosiState > integral_velmax)
alpesh 13:18c376e5dc9a 385 {
alpesh 13:18c376e5dc9a 386 PosiState = integral_velmax;
alpesh 13:18c376e5dc9a 387 }
alpesh 13:18c376e5dc9a 388 else if (PosiState < integral_velmin)
alpesh 13:18c376e5dc9a 389 {
alpesh 13:18c376e5dc9a 390 PosiState = integral_velmin;
alpesh 13:18c376e5dc9a 391 }
alpesh 13:18c376e5dc9a 392 PosIntError = PosKiGain * PosiState;
alpesh 13:18c376e5dc9a 393
alpesh 13:18c376e5dc9a 394 TargetVelocity = (PosKpGain * PosError + PosKdGain * PosDifError + PosIntError);
alpesh 13:18c376e5dc9a 395
alpesh 13:18c376e5dc9a 396
alpesh 13:18c376e5dc9a 397 ///////////////////////////////////////////////////////////////////////////////////////////////
alpesh 13:18c376e5dc9a 398 //Velocity PID
alpesh 13:18c376e5dc9a 399 ///////////////////////////////////////////////////////////////////////////////////////////////
alpesh 13:18c376e5dc9a 400
alpesh 13:18c376e5dc9a 401 double integral_pwmmax = 1.0/VelKiGain;
alpesh 13:18c376e5dc9a 402 double integral_pwmmin = -1.0/VelKiGain;
alpesh 13:18c376e5dc9a 403
alpesh 13:18c376e5dc9a 404 VelError = TargetVelocity - GetSpeed();
alpesh 13:18c376e5dc9a 405
alpesh 13:18c376e5dc9a 406 VelProError = VelError * VelKpGain;
alpesh 13:18c376e5dc9a 407
alpesh 13:18c376e5dc9a 408 VelDifError = (VelError - VelPreviousError[errorcounter]) / timeStep;
alpesh 13:18c376e5dc9a 409
alpesh 13:18c376e5dc9a 410 //Calculate the integral state with appropriate limiting
alpesh 13:18c376e5dc9a 411
alpesh 13:18c376e5dc9a 412 VeliState += VelError;
alpesh 13:18c376e5dc9a 413
alpesh 13:18c376e5dc9a 414 if (VeliState > integral_pwmmax)
alpesh 13:18c376e5dc9a 415 {
alpesh 13:18c376e5dc9a 416 VeliState = integral_pwmmax;
alpesh 13:18c376e5dc9a 417 }
alpesh 13:18c376e5dc9a 418 else if (VeliState < integral_pwmmin)
alpesh 13:18c376e5dc9a 419 {
alpesh 13:18c376e5dc9a 420 VeliState = integral_pwmmin;
alpesh 13:18c376e5dc9a 421 }
alpesh 13:18c376e5dc9a 422
alpesh 13:18c376e5dc9a 423 VelIntError = VelKiGain * VeliState;
alpesh 13:18c376e5dc9a 424
alpesh 13:18c376e5dc9a 425 TargetPwm = (VelKpGain * VelError + VelKdGain * VelDifError + VelIntError);
alpesh 13:18c376e5dc9a 426
alpesh 13:18c376e5dc9a 427 if (TargetPwm > 1.0)
pyrostew 12:814db1249a19 428 {
pyrostew 12:814db1249a19 429 pwm = 1.0;
pyrostew 12:814db1249a19 430 }
pyrostew 12:814db1249a19 431
alpesh 13:18c376e5dc9a 432 else if (TargetPwm < -1.0)
pyrostew 12:814db1249a19 433 {
pyrostew 12:814db1249a19 434 pwm = -1.0;
pyrostew 12:814db1249a19 435 }
pyrostew 12:814db1249a19 436
pyrostew 12:814db1249a19 437 else
pyrostew 12:814db1249a19 438 {
alpesh 13:18c376e5dc9a 439 pwm = TargetPwm;
pyrostew 12:814db1249a19 440 }
pyrostew 12:814db1249a19 441
pyrostew 12:814db1249a19 442 SetPower(pwm);
alpesh 4:2ec05810bc47 443
pyrostew 12:814db1249a19 444 errorcounter ++;
pyrostew 12:814db1249a19 445
pyrostew 12:814db1249a19 446 if (errorcounter > 9)
pyrostew 12:814db1249a19 447 {
pyrostew 12:814db1249a19 448 errorcounter = 0;
pyrostew 12:814db1249a19 449 }
pyrostew 12:814db1249a19 450
alpesh 13:18c376e5dc9a 451 PosPreviousError[errorcounter] = PosError;
alpesh 13:18c376e5dc9a 452 VelPreviousError[errorcounter] = VelError;
alpesh 13:18c376e5dc9a 453 // }
alpesh 13:18c376e5dc9a 454
alpesh 13:18c376e5dc9a 455
alpesh 13:18c376e5dc9a 456
pyrostew 12:814db1249a19 457 }
alpesh 4:2ec05810bc47 458
pyrostew 0:398432a37ca5 459 int main()
alpesh 2:d1805e7d46fb 460
pyrostew 0:398432a37ca5 461 {
pyrostew 0:398432a37ca5 462 RGHSinInterrupt.rise(&RGHSinHandler);
pyrostew 0:398432a37ca5 463 RGHCosInterrupt.rise(&RGHCosHandler);
pyrostew 0:398432a37ca5 464 HallSensor.fall(&HallEffectFall);
pyrostew 0:398432a37ca5 465 HallSensor.mode(PullUp);
pyrostew 0:398432a37ca5 466
pyrostew 0:398432a37ca5 467 RunningTime.start();
pyrostew 0:398432a37ca5 468
pyrostew 0:398432a37ca5 469 pc.baud(115200);
pyrostew 0:398432a37ca5 470
pyrostew 0:398432a37ca5 471 Home();
alpesh 4:2ec05810bc47 472 //Enable();
pyrostew 0:398432a37ca5 473
alpesh 3:e693c65b04de 474 errorcounter = 0;
alpesh 13:18c376e5dc9a 475 PosPreviousError[errorcounter]=0;
alpesh 2:d1805e7d46fb 476
alpesh 13:18c376e5dc9a 477 PreviousTime = RunningTime.read_us();
alpesh 3:e693c65b04de 478
alpesh 6:bfe745b152fa 479 while(Enabled)
alpesh 2:d1805e7d46fb 480 {
pyrostew 12:814db1249a19 481 //double pow = 0.0;
alpesh 13:18c376e5dc9a 482 while(PosKpGain < 1.0)
pyrostew 0:398432a37ca5 483 {
alpesh 13:18c376e5dc9a 484 PosKpGain += 0.01;
alpesh 13:18c376e5dc9a 485 while(PosKiGain < 1.0)
pyrostew 0:398432a37ca5 486 {
alpesh 13:18c376e5dc9a 487 PosKiGain += 0.01;
alpesh 7:4cd7be306626 488
alpesh 13:18c376e5dc9a 489 while(PosKdGain < 1.0)
pyrostew 12:814db1249a19 490 {
alpesh 13:18c376e5dc9a 491 PosKdGain += 0.01;
pyrostew 12:814db1249a19 492
pyrostew 12:814db1249a19 493 float iterationStart = RunningTime.read();
pyrostew 12:814db1249a19 494
pyrostew 12:814db1249a19 495 while(RunningTime.read()-iterationStart < 10.0)
pyrostew 12:814db1249a19 496 {
pyrostew 12:814db1249a19 497 SerialTransmit();
pyrostew 12:814db1249a19 498
pyrostew 12:814db1249a19 499 Controller();
pyrostew 12:814db1249a19 500 }
pyrostew 12:814db1249a19 501 }
pyrostew 0:398432a37ca5 502 }
alpesh 7:4cd7be306626 503
alpesh 7:4cd7be306626 504 Home();
pyrostew 0:398432a37ca5 505 }
alpesh 2:d1805e7d46fb 506
alpesh 6:bfe745b152fa 507 Disable();
pyrostew 0:398432a37ca5 508 }
alpesh 7:4cd7be306626 509 }
alpesh 7:4cd7be306626 510
alpesh 7:4cd7be306626 511 /* Change this throttle value range to 1.0 and 0.0 for car speed
alpesh 13:18c376e5dc9a 512 m_throttlechange = (m_kpGain * m_error + m_kdGain * m_PosDifError + m_PosIntError);
alpesh 7:4cd7be306626 513
alpesh 7:4cd7be306626 514 double throttle = m_throttle + m_throttlechange;
alpesh 7:4cd7be306626 515
alpesh 7:4cd7be306626 516 if (new_throttle > 1.0) {
alpesh 7:4cd7be306626 517 m_throttle = 1.0;
alpesh 7:4cd7be306626 518 } else if (new_throttle < 0.0) {
alpesh 7:4cd7be306626 519 m_throttle = 0.0;
alpesh 7:4cd7be306626 520 } else {
alpesh 7:4cd7be306626 521 m_throttle = new_throttle;
alpesh 7:4cd7be306626 522 }*/
alpesh 7:4cd7be306626 523
alpesh 7:4cd7be306626 524
alpesh 7:4cd7be306626 525
alpesh 7:4cd7be306626 526
alpesh 7:4cd7be306626 527