Alpesh Bhudia / Mbed 2 deprecated EquatorStrutDigitalMonitor

Merged to branch

Dependencies:   USBDevice mbed EquatorStrutController LightWeightSerialTransmit

Fork of EquatorStrutDigitalMonitor by Stewart Coulden-Smith

main.cpp@15:cd409a54ceec, 2014-08-13 (annotated)

Committer:
pyrostew
Date:
Wed Aug 13 08:45:00 2014 +0000
Revision:
15:cd409a54ceec
Parent:
12:814db1249a19
Child:
16:47d761226df6
Adjusted interrupts to handle new encoder signal processing.

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 15:cd409a54ceec 9 InterruptIn RGHSinFallingInterrupt(P0_13);
pyrostew 15:cd409a54ceec 10 InterruptIn RGHCosFallingInterrupt(P0_14);
pyrostew 0:398432a37ca5 11 InterruptIn HallSensor(P0_2);
pyrostew 0:398432a37ca5 12 DigitalOut ResetLine(P1_29);
pyrostew 0:398432a37ca5 13 PwmOut PhaseA(P0_9);
pyrostew 0:398432a37ca5 14 PwmOut PhaseB(P0_8);
pyrostew 0:398432a37ca5 15 Timer RunningTime;
pyrostew 0:398432a37ca5 16
pyrostew 0:398432a37ca5 17 bool Enabled = false;
pyrostew 12:814db1249a19 18 volatile bool Homing = false;
pyrostew 12:814db1249a19 19 volatile bool HallTriggered = false;
pyrostew 0:398432a37ca5 20
pyrostew 0:398432a37ca5 21 RawSerial pc(P1_27, P1_26);
pyrostew 0:398432a37ca5 22
pyrostew 12:814db1249a19 23 volatile char PinState = 0;
pyrostew 0:398432a37ca5 24
pyrostew 12:814db1249a19 25 volatile int direction = 0;
pyrostew 12:814db1249a19 26 volatile double position = 0.0;
pyrostew 0:398432a37ca5 27 double currentPower = 0.0;
pyrostew 12:814db1249a19 28 volatile int lastTime = 0;
pyrostew 0:398432a37ca5 29
pyrostew 12:814db1249a19 30 const int arraySize = 50;
pyrostew 12:814db1249a19 31 volatile int intteruptPeriodArray[arraySize];
pyrostew 12:814db1249a19 32 volatile int arrayTotal = 0;
pyrostew 12:814db1249a19 33 volatile char arrayPos = 0;
pyrostew 12:814db1249a19 34 volatile int interruptPeriod = 0;
pyrostew 10:088eeae4287c 35
pyrostew 0:398432a37ca5 36 char counter = 0;
pyrostew 0:398432a37ca5 37
pyrostew 15:cd409a54ceec 38 volatile bool SinHigh = false;
pyrostew 15:cd409a54ceec 39 volatile bool CosHigh = false;
pyrostew 15:cd409a54ceec 40
pyrostew 10:088eeae4287c 41 void SmoothingAdd(int input)
pyrostew 10:088eeae4287c 42 {
pyrostew 12:814db1249a19 43 intteruptPeriodArray[arrayPos] = input * direction;
pyrostew 10:088eeae4287c 44
pyrostew 12:814db1249a19 45 if (arrayPos == arraySize-1)
pyrostew 10:088eeae4287c 46 {
pyrostew 10:088eeae4287c 47 arrayPos = 0;
pyrostew 10:088eeae4287c 48 }
pyrostew 10:088eeae4287c 49 else
pyrostew 10:088eeae4287c 50 {
pyrostew 12:814db1249a19 51 arrayPos++;
pyrostew 10:088eeae4287c 52 }
pyrostew 10:088eeae4287c 53 }
pyrostew 10:088eeae4287c 54
pyrostew 10:088eeae4287c 55 int SmoothedInterruptPeriod()
pyrostew 10:088eeae4287c 56 {
pyrostew 12:814db1249a19 57 int arrayTotal = 0;
pyrostew 12:814db1249a19 58
pyrostew 12:814db1249a19 59 for (char i = 0; i < arraySize; i++)
pyrostew 12:814db1249a19 60 {
pyrostew 12:814db1249a19 61 arrayTotal += intteruptPeriodArray[i];
pyrostew 12:814db1249a19 62 }
pyrostew 12:814db1249a19 63
pyrostew 12:814db1249a19 64 return arrayTotal / arraySize;
pyrostew 10:088eeae4287c 65 }
pyrostew 10:088eeae4287c 66
pyrostew 15:cd409a54ceec 67 void RGHSinRisingHandler()
pyrostew 15:cd409a54ceec 68 {
pyrostew 15:cd409a54ceec 69 SinHigh = true;
pyrostew 15:cd409a54ceec 70 }
pyrostew 15:cd409a54ceec 71
pyrostew 15:cd409a54ceec 72 void RGHSinFallingHandler()
pyrostew 15:cd409a54ceec 73 {
pyrostew 15:cd409a54ceec 74 SinHigh = false;
pyrostew 15:cd409a54ceec 75 }
pyrostew 15:cd409a54ceec 76
pyrostew 15:cd409a54ceec 77 void RGHCosRisingHandler()
pyrostew 15:cd409a54ceec 78 {
pyrostew 15:cd409a54ceec 79 if (CosHigh)
pyrostew 0:398432a37ca5 80 {
pyrostew 0:398432a37ca5 81 return;
pyrostew 0:398432a37ca5 82 }
pyrostew 15:cd409a54ceec 83 else if (SinHigh)
pyrostew 15:cd409a54ceec 84 {
pyrostew 15:cd409a54ceec 85 direction = -1;
pyrostew 0:398432a37ca5 86 }
pyrostew 0:398432a37ca5 87 else
pyrostew 0:398432a37ca5 88 {
pyrostew 15:cd409a54ceec 89 direction = 1;
pyrostew 0:398432a37ca5 90 }
pyrostew 15:cd409a54ceec 91
pyrostew 15:cd409a54ceec 92 position += 0.04 * direction;
pyrostew 15:cd409a54ceec 93 SmoothingAdd(RunningTime.read_us() - lastTime);
pyrostew 15:cd409a54ceec 94 lastTime = RunningTime.read_us();
pyrostew 15:cd409a54ceec 95
pyrostew 15:cd409a54ceec 96 CosHigh = true;
pyrostew 0:398432a37ca5 97 }
pyrostew 0:398432a37ca5 98
pyrostew 15:cd409a54ceec 99 void RGHCosFallingHandler()
pyrostew 15:cd409a54ceec 100 {
pyrostew 15:cd409a54ceec 101 CosHigh = false;
pyrostew 0:398432a37ca5 102 }
pyrostew 0:398432a37ca5 103
pyrostew 0:398432a37ca5 104 void SetPower(double power)
pyrostew 0:398432a37ca5 105 {
pyrostew 0:398432a37ca5 106 currentPower = power;
pyrostew 0:398432a37ca5 107 if(!Enabled)
pyrostew 0:398432a37ca5 108 {
pyrostew 0:398432a37ca5 109 return;
pyrostew 0:398432a37ca5 110 }
pyrostew 0:398432a37ca5 111
pyrostew 0:398432a37ca5 112 if (power > 1.0 || power < -1.0)
pyrostew 0:398432a37ca5 113 {
pyrostew 0:398432a37ca5 114 return;
pyrostew 0:398432a37ca5 115 }
pyrostew 0:398432a37ca5 116
pyrostew 0:398432a37ca5 117 PhaseA = (power + 1.0) / 2;
pyrostew 0:398432a37ca5 118 PhaseB = 1.0 - ((power + 1.0) / 2);
pyrostew 0:398432a37ca5 119 }
pyrostew 0:398432a37ca5 120
pyrostew 0:398432a37ca5 121 void Enable()
pyrostew 0:398432a37ca5 122 {
pyrostew 0:398432a37ca5 123 SetPower(0.0);
pyrostew 0:398432a37ca5 124
pyrostew 0:398432a37ca5 125 ResetLine = 1;
pyrostew 0:398432a37ca5 126
pyrostew 0:398432a37ca5 127 Enabled = true;
pyrostew 0:398432a37ca5 128 }
pyrostew 0:398432a37ca5 129
pyrostew 0:398432a37ca5 130 void Disable()
pyrostew 0:398432a37ca5 131 {
pyrostew 0:398432a37ca5 132 ResetLine = 0;
pyrostew 0:398432a37ca5 133
pyrostew 0:398432a37ca5 134 SetPower(0.0);
pyrostew 0:398432a37ca5 135
pyrostew 0:398432a37ca5 136 Enabled = false;
pyrostew 0:398432a37ca5 137 }
pyrostew 0:398432a37ca5 138
pyrostew 0:398432a37ca5 139 void Home()
pyrostew 0:398432a37ca5 140 {
pyrostew 0:398432a37ca5 141 if (!Enabled)
pyrostew 0:398432a37ca5 142 {
pyrostew 0:398432a37ca5 143 Enable();
pyrostew 0:398432a37ca5 144 }
pyrostew 0:398432a37ca5 145
pyrostew 12:814db1249a19 146 if (HallSensorState == 1)
pyrostew 0:398432a37ca5 147 {
pyrostew 12:814db1249a19 148 Homing = true;
pyrostew 12:814db1249a19 149 HallTriggered = false;
pyrostew 12:814db1249a19 150
pyrostew 12:814db1249a19 151 SetPower(-0.6);
pyrostew 12:814db1249a19 152
pyrostew 12:814db1249a19 153 while (!HallTriggered)
pyrostew 12:814db1249a19 154 {
pyrostew 12:814db1249a19 155 wait(0.5);
pyrostew 12:814db1249a19 156 }
pyrostew 0:398432a37ca5 157 }
pyrostew 0:398432a37ca5 158
pyrostew 0:398432a37ca5 159 SetPower(1.0);
pyrostew 0:398432a37ca5 160
pyrostew 0:398432a37ca5 161 while (position < 20.0)
pyrostew 0:398432a37ca5 162 {
pyrostew 0:398432a37ca5 163
pyrostew 0:398432a37ca5 164 }
pyrostew 0:398432a37ca5 165
pyrostew 0:398432a37ca5 166 Homing = true;
pyrostew 0:398432a37ca5 167 HallTriggered = false;
pyrostew 0:398432a37ca5 168
pyrostew 12:814db1249a19 169 SetPower(-0.4);
pyrostew 0:398432a37ca5 170
pyrostew 0:398432a37ca5 171 while (!HallTriggered)
pyrostew 0:398432a37ca5 172 {
pyrostew 0:398432a37ca5 173 wait(0.5);
pyrostew 0:398432a37ca5 174 }
pyrostew 0:398432a37ca5 175 }
pyrostew 0:398432a37ca5 176
pyrostew 1:a33723b70582 177 double GetSpeed()
pyrostew 1:a33723b70582 178 {
pyrostew 12:814db1249a19 179 //if ((RunningTime.read_us() - lastTime) > 1000 || SmoothedInterruptPeriod() == 0)
pyrostew 12:814db1249a19 180 //{
pyrostew 12:814db1249a19 181 // return 0.0;
pyrostew 12:814db1249a19 182 //}
pyrostew 12:814db1249a19 183 return (0.04)/((double)SmoothedInterruptPeriod() / 1000000.0);
pyrostew 12:814db1249a19 184 //return SmoothedInterruptPeriod();
pyrostew 1:a33723b70582 185 }
pyrostew 1:a33723b70582 186
alpesh 6:bfe745b152fa 187 double Error = 0;
alpesh 6:bfe745b152fa 188
pyrostew 12:814db1249a19 189 void SerialOut(double outputValue)
pyrostew 12:814db1249a19 190 {
pyrostew 0:398432a37ca5 191 int outChar = 0;
pyrostew 0:398432a37ca5 192
pyrostew 12:814db1249a19 193 if (outputValue < 0.0)
pyrostew 0:398432a37ca5 194 {
pyrostew 0:398432a37ca5 195 pc.putc('-');
pyrostew 12:814db1249a19 196 outputValue *= -1.0;
pyrostew 0:398432a37ca5 197 }
pyrostew 12:814db1249a19 198 if (outputValue >= 100.0)
pyrostew 0:398432a37ca5 199 {
pyrostew 12:814db1249a19 200 outChar = outputValue / 100;
pyrostew 0:398432a37ca5 201 pc.putc(outChar + 48);
pyrostew 12:814db1249a19 202 outputValue -= outChar * 100.0;
pyrostew 0:398432a37ca5 203 }
pyrostew 12:814db1249a19 204 if (outputValue >= 10.0)
pyrostew 0:398432a37ca5 205 {
pyrostew 12:814db1249a19 206 outChar = outputValue / 10;
pyrostew 0:398432a37ca5 207 pc.putc(outChar + 48);
pyrostew 12:814db1249a19 208 outputValue -= outChar * 10.0;
pyrostew 0:398432a37ca5 209 }
pyrostew 0:398432a37ca5 210 else if(outChar > 0)
pyrostew 0:398432a37ca5 211 {
pyrostew 0:398432a37ca5 212 pc.putc('0');
pyrostew 0:398432a37ca5 213 }
pyrostew 12:814db1249a19 214 if (outputValue >= 1.0)
pyrostew 0:398432a37ca5 215 {
pyrostew 12:814db1249a19 216 outChar = outputValue;
pyrostew 0:398432a37ca5 217 pc.putc(outChar + 48);
pyrostew 12:814db1249a19 218 outputValue -= outChar;
pyrostew 0:398432a37ca5 219 }
pyrostew 0:398432a37ca5 220 else
pyrostew 0:398432a37ca5 221 {
pyrostew 0:398432a37ca5 222 pc.putc('0');
pyrostew 0:398432a37ca5 223 }
pyrostew 12:814db1249a19 224 if (outputValue >= 0.1)
pyrostew 0:398432a37ca5 225 {
pyrostew 0:398432a37ca5 226 pc.putc('.');
pyrostew 12:814db1249a19 227 outChar = outputValue * 10;
pyrostew 0:398432a37ca5 228 pc.putc(outChar + 48);
pyrostew 12:814db1249a19 229 outputValue -= (double)outChar / 10.0;
pyrostew 0:398432a37ca5 230 }
pyrostew 0:398432a37ca5 231 else
pyrostew 0:398432a37ca5 232 {
pyrostew 0:398432a37ca5 233 pc.putc('.');
pyrostew 0:398432a37ca5 234 pc.putc('0');
pyrostew 0:398432a37ca5 235 }
pyrostew 12:814db1249a19 236 if (outputValue >= 0.01)
pyrostew 0:398432a37ca5 237 {
pyrostew 12:814db1249a19 238 outChar = outputValue * 100;
pyrostew 0:398432a37ca5 239 pc.putc(outChar + 48);
pyrostew 12:814db1249a19 240 outputValue -= (double)outChar / 100.0;
pyrostew 0:398432a37ca5 241 }
pyrostew 0:398432a37ca5 242 else
pyrostew 0:398432a37ca5 243 {
pyrostew 0:398432a37ca5 244 pc.putc('0');
pyrostew 0:398432a37ca5 245 }
pyrostew 12:814db1249a19 246 if (outputValue >= 0.001)
pyrostew 0:398432a37ca5 247 {
pyrostew 12:814db1249a19 248 outChar= outputValue * 1000;
pyrostew 0:398432a37ca5 249 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 250 }
pyrostew 12:814db1249a19 251 }
pyrostew 12:814db1249a19 252
pyrostew 12:814db1249a19 253 double KpGain = 0.0;
pyrostew 12:814db1249a19 254 double KiGain = 0.0;
pyrostew 12:814db1249a19 255 double KdGain = 0.0;
pyrostew 12:814db1249a19 256
pyrostew 12:814db1249a19 257 void SerialTransmit()
pyrostew 12:814db1249a19 258 {
pyrostew 12:814db1249a19 259 SerialOut(RunningTime.read());
pyrostew 12:814db1249a19 260
pyrostew 12:814db1249a19 261 pc.putc('\t');
pyrostew 12:814db1249a19 262
pyrostew 12:814db1249a19 263 SerialOut(position);
pyrostew 0:398432a37ca5 264
alpesh 2:d1805e7d46fb 265 pc.putc('\t');
pyrostew 12:814db1249a19 266
pyrostew 12:814db1249a19 267 SerialOut(currentPower);
pyrostew 1:a33723b70582 268
pyrostew 12:814db1249a19 269 pc.putc('\t');
pyrostew 12:814db1249a19 270
pyrostew 12:814db1249a19 271 SerialOut(GetSpeed());
pyrostew 1:a33723b70582 272
alpesh 6:bfe745b152fa 273 pc.putc('\t');
pyrostew 12:814db1249a19 274
pyrostew 12:814db1249a19 275 SerialOut(Error);
pyrostew 12:814db1249a19 276
pyrostew 12:814db1249a19 277 pc.putc('\t');
pyrostew 12:814db1249a19 278
pyrostew 12:814db1249a19 279 SerialOut(KpGain);
alpesh 6:bfe745b152fa 280
pyrostew 12:814db1249a19 281 pc.putc('\t');
pyrostew 12:814db1249a19 282
pyrostew 12:814db1249a19 283 SerialOut(KiGain);
pyrostew 12:814db1249a19 284
pyrostew 12:814db1249a19 285 pc.putc('\t');
pyrostew 12:814db1249a19 286
pyrostew 12:814db1249a19 287 SerialOut(KdGain);
alpesh 6:bfe745b152fa 288
pyrostew 0:398432a37ca5 289 pc.putc(10);
pyrostew 0:398432a37ca5 290 pc.putc(13);
pyrostew 0:398432a37ca5 291 }
pyrostew 0:398432a37ca5 292
pyrostew 0:398432a37ca5 293 void HallEffectFall()
pyrostew 0:398432a37ca5 294 {
pyrostew 0:398432a37ca5 295 RGHSinInterrupt.disable_irq();
pyrostew 0:398432a37ca5 296 RGHCosInterrupt.disable_irq();
pyrostew 15:cd409a54ceec 297 RGHSinFallingInterrupt.disable_irq();
pyrostew 15:cd409a54ceec 298 RGHCosFallingInterrupt.disable_irq();
pyrostew 0:398432a37ca5 299
pyrostew 0:398432a37ca5 300 if (direction < 0)
pyrostew 0:398432a37ca5 301 {
pyrostew 0:398432a37ca5 302 SetPower(0.0);
pyrostew 0:398432a37ca5 303
pyrostew 0:398432a37ca5 304 if (Homing)
pyrostew 0:398432a37ca5 305 {
pyrostew 0:398432a37ca5 306 HallTriggered = true;
pyrostew 0:398432a37ca5 307 Homing = false;
pyrostew 0:398432a37ca5 308 position = 0.0;
pyrostew 0:398432a37ca5 309 }
pyrostew 0:398432a37ca5 310 }
pyrostew 0:398432a37ca5 311 RGHSinInterrupt.enable_irq();
pyrostew 0:398432a37ca5 312 RGHCosInterrupt.enable_irq();
pyrostew 15:cd409a54ceec 313 RGHSinFallingInterrupt.enable_irq();
pyrostew 15:cd409a54ceec 314 RGHCosFallingInterrupt.enable_irq();
pyrostew 0:398432a37ca5 315 }
pyrostew 0:398432a37ca5 316
alpesh 7:4cd7be306626 317 double SetPoint = 50.0; //Millimeter per second
alpesh 6:bfe745b152fa 318
alpesh 2:d1805e7d46fb 319 double KpTerm;
alpesh 3:e693c65b04de 320 double ErrorInt;
alpesh 2:d1805e7d46fb 321 double ErrorDer;
alpesh 2:d1805e7d46fb 322
alpesh 3:e693c65b04de 323 int errorcounter;
alpesh 2:d1805e7d46fb 324 double PreviousError [10];
alpesh 2:d1805e7d46fb 325
alpesh 3:e693c65b04de 326 double PwmChange=0;
alpesh 3:e693c65b04de 327
alpesh 3:e693c65b04de 328 double pwm;
alpesh 3:e693c65b04de 329 double NewPwm;
alpesh 3:e693c65b04de 330
alpesh 3:e693c65b04de 331 int previousTime = 0;
alpesh 3:e693c65b04de 332
alpesh 4:2ec05810bc47 333 void Controller ()
pyrostew 12:814db1249a19 334 {
pyrostew 12:814db1249a19 335 if (position <= SetPoint || 1)
alpesh 4:2ec05810bc47 336 {
pyrostew 12:814db1249a19 337 int timeStep = RunningTime.read_us() - previousTime;
pyrostew 12:814db1249a19 338 previousTime = RunningTime.read_us();
pyrostew 12:814db1249a19 339
pyrostew 12:814db1249a19 340 Error = SetPoint - position;
pyrostew 12:814db1249a19 341
pyrostew 12:814db1249a19 342 KpTerm = Error * KpGain;
pyrostew 12:814db1249a19 343
pyrostew 12:814db1249a19 344 ErrorDer = (Error - PreviousError[errorcounter]) / timeStep;
pyrostew 12:814db1249a19 345
pyrostew 12:814db1249a19 346 ErrorInt = ErrorInt + Error * timeStep;
pyrostew 12:814db1249a19 347
pyrostew 12:814db1249a19 348 NewPwm = (KpTerm + KiGain * ErrorInt + KdGain * ErrorDer);
alpesh 4:2ec05810bc47 349
pyrostew 12:814db1249a19 350 if (NewPwm > 1.0)
pyrostew 12:814db1249a19 351 {
pyrostew 12:814db1249a19 352 pwm = 1.0;
pyrostew 12:814db1249a19 353 }
pyrostew 12:814db1249a19 354
pyrostew 12:814db1249a19 355 else if (NewPwm < -1.0)
pyrostew 12:814db1249a19 356 {
pyrostew 12:814db1249a19 357 pwm = -1.0;
pyrostew 12:814db1249a19 358 }
pyrostew 12:814db1249a19 359
pyrostew 12:814db1249a19 360 else
pyrostew 12:814db1249a19 361 {
pyrostew 12:814db1249a19 362 pwm = NewPwm;
pyrostew 12:814db1249a19 363 }
pyrostew 12:814db1249a19 364
pyrostew 12:814db1249a19 365 SetPower(pwm);
alpesh 4:2ec05810bc47 366
pyrostew 12:814db1249a19 367 errorcounter ++;
pyrostew 12:814db1249a19 368
pyrostew 12:814db1249a19 369 if (errorcounter > 9)
pyrostew 12:814db1249a19 370 {
pyrostew 12:814db1249a19 371 errorcounter = 0;
pyrostew 12:814db1249a19 372 }
pyrostew 12:814db1249a19 373
pyrostew 12:814db1249a19 374 PreviousError[errorcounter] = Error;
alpesh 4:2ec05810bc47 375 }
pyrostew 12:814db1249a19 376 }
alpesh 4:2ec05810bc47 377
pyrostew 0:398432a37ca5 378 int main()
alpesh 2:d1805e7d46fb 379
pyrostew 0:398432a37ca5 380 {
pyrostew 15:cd409a54ceec 381 RGHSinInterrupt.rise(&RGHSinRisingHandler);
pyrostew 15:cd409a54ceec 382 RGHCosInterrupt.rise(&RGHCosRisingHandler);
pyrostew 15:cd409a54ceec 383 RGHSinFallingInterrupt.fall(&RGHSinFallingHandler);
pyrostew 15:cd409a54ceec 384 RGHCosFallingInterrupt.fall(&RGHCosFallingHandler);
pyrostew 0:398432a37ca5 385 HallSensor.fall(&HallEffectFall);
pyrostew 0:398432a37ca5 386 HallSensor.mode(PullUp);
pyrostew 0:398432a37ca5 387
pyrostew 0:398432a37ca5 388 RunningTime.start();
pyrostew 0:398432a37ca5 389
pyrostew 0:398432a37ca5 390 pc.baud(115200);
pyrostew 0:398432a37ca5 391
pyrostew 0:398432a37ca5 392 Home();
alpesh 4:2ec05810bc47 393 //Enable();
pyrostew 0:398432a37ca5 394
alpesh 3:e693c65b04de 395 errorcounter = 0;
alpesh 3:e693c65b04de 396 PreviousError[errorcounter]=0;
alpesh 2:d1805e7d46fb 397
alpesh 3:e693c65b04de 398 previousTime = RunningTime.read_us();
alpesh 3:e693c65b04de 399
pyrostew 15:cd409a54ceec 400 /*while(Enabled)
alpesh 2:d1805e7d46fb 401 {
pyrostew 12:814db1249a19 402 //double pow = 0.0;
pyrostew 12:814db1249a19 403 while(KpGain < 1.0)
pyrostew 0:398432a37ca5 404 {
pyrostew 12:814db1249a19 405 KpGain += 0.01;
pyrostew 12:814db1249a19 406 while(KiGain < 1.0)
pyrostew 0:398432a37ca5 407 {
pyrostew 12:814db1249a19 408 KiGain += 0.01;
alpesh 7:4cd7be306626 409
pyrostew 12:814db1249a19 410 while(KdGain < 1.0)
pyrostew 12:814db1249a19 411 {
pyrostew 12:814db1249a19 412 KdGain += 0.01;
pyrostew 12:814db1249a19 413
pyrostew 12:814db1249a19 414 float iterationStart = RunningTime.read();
pyrostew 12:814db1249a19 415
pyrostew 12:814db1249a19 416 while(RunningTime.read()-iterationStart < 10.0)
pyrostew 12:814db1249a19 417 {
pyrostew 12:814db1249a19 418 SerialTransmit();
pyrostew 12:814db1249a19 419
pyrostew 12:814db1249a19 420 Controller();
pyrostew 12:814db1249a19 421 }
pyrostew 12:814db1249a19 422 }
pyrostew 0:398432a37ca5 423 }
alpesh 7:4cd7be306626 424
alpesh 7:4cd7be306626 425 Home();
pyrostew 0:398432a37ca5 426 }
alpesh 2:d1805e7d46fb 427
alpesh 6:bfe745b152fa 428 Disable();
pyrostew 15:cd409a54ceec 429 }*/
alpesh 7:4cd7be306626 430 }
alpesh 7:4cd7be306626 431
alpesh 7:4cd7be306626 432 /* Change this throttle value range to 1.0 and 0.0 for car speed
alpesh 7:4cd7be306626 433 m_throttlechange = (m_kpGain * m_error + m_kdGain * m_ErrorDer + m_ErrorInt);
alpesh 7:4cd7be306626 434
alpesh 7:4cd7be306626 435 double throttle = m_throttle + m_throttlechange;
alpesh 7:4cd7be306626 436
alpesh 7:4cd7be306626 437 if (new_throttle > 1.0) {
alpesh 7:4cd7be306626 438 m_throttle = 1.0;
alpesh 7:4cd7be306626 439 } else if (new_throttle < 0.0) {
alpesh 7:4cd7be306626 440 m_throttle = 0.0;
alpesh 7:4cd7be306626 441 } else {
alpesh 7:4cd7be306626 442 m_throttle = new_throttle;
alpesh 7:4cd7be306626 443 }*/
alpesh 7:4cd7be306626 444
alpesh 7:4cd7be306626 445
alpesh 7:4cd7be306626 446
alpesh 7:4cd7be306626 447
alpesh 7:4cd7be306626 448