Alpesh Bhudia / Mbed 2 deprecated EquatorStrutDigitalMonitor

Merged to branch

Dependencies:   USBDevice mbed EquatorStrutController LightWeightSerialTransmit

Fork of EquatorStrutDigitalMonitor by Stewart Coulden-Smith

main.cpp@12:814db1249a19, 2014-08-13 (annotated)

Committer:
pyrostew
Date:
Wed Aug 13 06:43:02 2014 +0000
Revision:
12:814db1249a19
Parent:
11:b6958b3dbddf
Child:
13:18c376e5dc9a
Child:
15:cd409a54ceec
Changed various things in the homing routine and in the averaging routine.

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 6:bfe745b152fa 199 double Error = 0;
alpesh 6:bfe745b152fa 200
pyrostew 12:814db1249a19 201 void SerialOut(double outputValue)
pyrostew 12:814db1249a19 202 {
pyrostew 0:398432a37ca5 203 int outChar = 0;
pyrostew 0:398432a37ca5 204
pyrostew 12:814db1249a19 205 if (outputValue < 0.0)
pyrostew 0:398432a37ca5 206 {
pyrostew 0:398432a37ca5 207 pc.putc('-');
pyrostew 12:814db1249a19 208 outputValue *= -1.0;
pyrostew 0:398432a37ca5 209 }
pyrostew 12:814db1249a19 210 if (outputValue >= 100.0)
pyrostew 0:398432a37ca5 211 {
pyrostew 12:814db1249a19 212 outChar = outputValue / 100;
pyrostew 0:398432a37ca5 213 pc.putc(outChar + 48);
pyrostew 12:814db1249a19 214 outputValue -= outChar * 100.0;
pyrostew 0:398432a37ca5 215 }
pyrostew 12:814db1249a19 216 if (outputValue >= 10.0)
pyrostew 0:398432a37ca5 217 {
pyrostew 12:814db1249a19 218 outChar = outputValue / 10;
pyrostew 0:398432a37ca5 219 pc.putc(outChar + 48);
pyrostew 12:814db1249a19 220 outputValue -= outChar * 10.0;
pyrostew 0:398432a37ca5 221 }
pyrostew 0:398432a37ca5 222 else if(outChar > 0)
pyrostew 0:398432a37ca5 223 {
pyrostew 0:398432a37ca5 224 pc.putc('0');
pyrostew 0:398432a37ca5 225 }
pyrostew 12:814db1249a19 226 if (outputValue >= 1.0)
pyrostew 0:398432a37ca5 227 {
pyrostew 12:814db1249a19 228 outChar = outputValue;
pyrostew 0:398432a37ca5 229 pc.putc(outChar + 48);
pyrostew 12:814db1249a19 230 outputValue -= outChar;
pyrostew 0:398432a37ca5 231 }
pyrostew 0:398432a37ca5 232 else
pyrostew 0:398432a37ca5 233 {
pyrostew 0:398432a37ca5 234 pc.putc('0');
pyrostew 0:398432a37ca5 235 }
pyrostew 12:814db1249a19 236 if (outputValue >= 0.1)
pyrostew 0:398432a37ca5 237 {
pyrostew 0:398432a37ca5 238 pc.putc('.');
pyrostew 12:814db1249a19 239 outChar = outputValue * 10;
pyrostew 0:398432a37ca5 240 pc.putc(outChar + 48);
pyrostew 12:814db1249a19 241 outputValue -= (double)outChar / 10.0;
pyrostew 0:398432a37ca5 242 }
pyrostew 0:398432a37ca5 243 else
pyrostew 0:398432a37ca5 244 {
pyrostew 0:398432a37ca5 245 pc.putc('.');
pyrostew 0:398432a37ca5 246 pc.putc('0');
pyrostew 0:398432a37ca5 247 }
pyrostew 12:814db1249a19 248 if (outputValue >= 0.01)
pyrostew 0:398432a37ca5 249 {
pyrostew 12:814db1249a19 250 outChar = outputValue * 100;
pyrostew 0:398432a37ca5 251 pc.putc(outChar + 48);
pyrostew 12:814db1249a19 252 outputValue -= (double)outChar / 100.0;
pyrostew 0:398432a37ca5 253 }
pyrostew 0:398432a37ca5 254 else
pyrostew 0:398432a37ca5 255 {
pyrostew 0:398432a37ca5 256 pc.putc('0');
pyrostew 0:398432a37ca5 257 }
pyrostew 12:814db1249a19 258 if (outputValue >= 0.001)
pyrostew 0:398432a37ca5 259 {
pyrostew 12:814db1249a19 260 outChar= outputValue * 1000;
pyrostew 0:398432a37ca5 261 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 262 }
pyrostew 12:814db1249a19 263 }
pyrostew 12:814db1249a19 264
pyrostew 12:814db1249a19 265 double KpGain = 0.0;
pyrostew 12:814db1249a19 266 double KiGain = 0.0;
pyrostew 12:814db1249a19 267 double KdGain = 0.0;
pyrostew 12:814db1249a19 268
pyrostew 12:814db1249a19 269 void SerialTransmit()
pyrostew 12:814db1249a19 270 {
pyrostew 12:814db1249a19 271 SerialOut(RunningTime.read());
pyrostew 12:814db1249a19 272
pyrostew 12:814db1249a19 273 pc.putc('\t');
pyrostew 12:814db1249a19 274
pyrostew 12:814db1249a19 275 SerialOut(position);
pyrostew 0:398432a37ca5 276
alpesh 2:d1805e7d46fb 277 pc.putc('\t');
pyrostew 12:814db1249a19 278
pyrostew 12:814db1249a19 279 SerialOut(currentPower);
pyrostew 1:a33723b70582 280
pyrostew 12:814db1249a19 281 pc.putc('\t');
pyrostew 12:814db1249a19 282
pyrostew 12:814db1249a19 283 SerialOut(GetSpeed());
pyrostew 1:a33723b70582 284
alpesh 6:bfe745b152fa 285 pc.putc('\t');
pyrostew 12:814db1249a19 286
pyrostew 12:814db1249a19 287 SerialOut(Error);
pyrostew 12:814db1249a19 288
pyrostew 12:814db1249a19 289 pc.putc('\t');
pyrostew 12:814db1249a19 290
pyrostew 12:814db1249a19 291 SerialOut(KpGain);
alpesh 6:bfe745b152fa 292
pyrostew 12:814db1249a19 293 pc.putc('\t');
pyrostew 12:814db1249a19 294
pyrostew 12:814db1249a19 295 SerialOut(KiGain);
pyrostew 12:814db1249a19 296
pyrostew 12:814db1249a19 297 pc.putc('\t');
pyrostew 12:814db1249a19 298
pyrostew 12:814db1249a19 299 SerialOut(KdGain);
alpesh 6:bfe745b152fa 300
pyrostew 0:398432a37ca5 301 pc.putc(10);
pyrostew 0:398432a37ca5 302 pc.putc(13);
pyrostew 0:398432a37ca5 303 }
pyrostew 0:398432a37ca5 304
pyrostew 0:398432a37ca5 305 void HallEffectFall()
pyrostew 0:398432a37ca5 306 {
pyrostew 0:398432a37ca5 307 RGHSinInterrupt.disable_irq();
pyrostew 0:398432a37ca5 308 RGHCosInterrupt.disable_irq();
pyrostew 0:398432a37ca5 309
pyrostew 0:398432a37ca5 310 if (direction < 0)
pyrostew 0:398432a37ca5 311 {
pyrostew 0:398432a37ca5 312 SetPower(0.0);
pyrostew 0:398432a37ca5 313
pyrostew 0:398432a37ca5 314 if (Homing)
pyrostew 0:398432a37ca5 315 {
pyrostew 0:398432a37ca5 316 HallTriggered = true;
pyrostew 0:398432a37ca5 317 Homing = false;
pyrostew 0:398432a37ca5 318 position = 0.0;
pyrostew 0:398432a37ca5 319 }
pyrostew 0:398432a37ca5 320 }
pyrostew 0:398432a37ca5 321 RGHSinInterrupt.enable_irq();
pyrostew 0:398432a37ca5 322 RGHCosInterrupt.enable_irq();
pyrostew 0:398432a37ca5 323 }
pyrostew 0:398432a37ca5 324
alpesh 7:4cd7be306626 325 double SetPoint = 50.0; //Millimeter per second
alpesh 6:bfe745b152fa 326
alpesh 2:d1805e7d46fb 327 double KpTerm;
alpesh 3:e693c65b04de 328 double ErrorInt;
alpesh 2:d1805e7d46fb 329 double ErrorDer;
alpesh 2:d1805e7d46fb 330
alpesh 3:e693c65b04de 331 int errorcounter;
alpesh 2:d1805e7d46fb 332 double PreviousError [10];
alpesh 2:d1805e7d46fb 333
alpesh 3:e693c65b04de 334 double PwmChange=0;
alpesh 3:e693c65b04de 335
alpesh 3:e693c65b04de 336 double pwm;
alpesh 3:e693c65b04de 337 double NewPwm;
alpesh 3:e693c65b04de 338
alpesh 3:e693c65b04de 339 int previousTime = 0;
alpesh 3:e693c65b04de 340
alpesh 4:2ec05810bc47 341 void Controller ()
pyrostew 12:814db1249a19 342 {
pyrostew 12:814db1249a19 343 if (position <= SetPoint || 1)
alpesh 4:2ec05810bc47 344 {
pyrostew 12:814db1249a19 345 int timeStep = RunningTime.read_us() - previousTime;
pyrostew 12:814db1249a19 346 previousTime = RunningTime.read_us();
pyrostew 12:814db1249a19 347
pyrostew 12:814db1249a19 348 Error = SetPoint - position;
pyrostew 12:814db1249a19 349
pyrostew 12:814db1249a19 350 KpTerm = Error * KpGain;
pyrostew 12:814db1249a19 351
pyrostew 12:814db1249a19 352 ErrorDer = (Error - PreviousError[errorcounter]) / timeStep;
pyrostew 12:814db1249a19 353
pyrostew 12:814db1249a19 354 ErrorInt = ErrorInt + Error * timeStep;
pyrostew 12:814db1249a19 355
pyrostew 12:814db1249a19 356 NewPwm = (KpTerm + KiGain * ErrorInt + KdGain * ErrorDer);
alpesh 4:2ec05810bc47 357
pyrostew 12:814db1249a19 358 if (NewPwm > 1.0)
pyrostew 12:814db1249a19 359 {
pyrostew 12:814db1249a19 360 pwm = 1.0;
pyrostew 12:814db1249a19 361 }
pyrostew 12:814db1249a19 362
pyrostew 12:814db1249a19 363 else if (NewPwm < -1.0)
pyrostew 12:814db1249a19 364 {
pyrostew 12:814db1249a19 365 pwm = -1.0;
pyrostew 12:814db1249a19 366 }
pyrostew 12:814db1249a19 367
pyrostew 12:814db1249a19 368 else
pyrostew 12:814db1249a19 369 {
pyrostew 12:814db1249a19 370 pwm = NewPwm;
pyrostew 12:814db1249a19 371 }
pyrostew 12:814db1249a19 372
pyrostew 12:814db1249a19 373 SetPower(pwm);
alpesh 4:2ec05810bc47 374
pyrostew 12:814db1249a19 375 errorcounter ++;
pyrostew 12:814db1249a19 376
pyrostew 12:814db1249a19 377 if (errorcounter > 9)
pyrostew 12:814db1249a19 378 {
pyrostew 12:814db1249a19 379 errorcounter = 0;
pyrostew 12:814db1249a19 380 }
pyrostew 12:814db1249a19 381
pyrostew 12:814db1249a19 382 PreviousError[errorcounter] = Error;
alpesh 4:2ec05810bc47 383 }
pyrostew 12:814db1249a19 384 }
alpesh 4:2ec05810bc47 385
pyrostew 0:398432a37ca5 386 int main()
alpesh 2:d1805e7d46fb 387
pyrostew 0:398432a37ca5 388 {
pyrostew 0:398432a37ca5 389 RGHSinInterrupt.rise(&RGHSinHandler);
pyrostew 0:398432a37ca5 390 RGHCosInterrupt.rise(&RGHCosHandler);
pyrostew 0:398432a37ca5 391 HallSensor.fall(&HallEffectFall);
pyrostew 0:398432a37ca5 392 HallSensor.mode(PullUp);
pyrostew 0:398432a37ca5 393
pyrostew 0:398432a37ca5 394 RunningTime.start();
pyrostew 0:398432a37ca5 395
pyrostew 0:398432a37ca5 396 pc.baud(115200);
pyrostew 0:398432a37ca5 397
pyrostew 0:398432a37ca5 398 Home();
alpesh 4:2ec05810bc47 399 //Enable();
pyrostew 0:398432a37ca5 400
alpesh 3:e693c65b04de 401 errorcounter = 0;
alpesh 3:e693c65b04de 402 PreviousError[errorcounter]=0;
alpesh 2:d1805e7d46fb 403
alpesh 3:e693c65b04de 404 previousTime = RunningTime.read_us();
alpesh 3:e693c65b04de 405
alpesh 6:bfe745b152fa 406 while(Enabled)
alpesh 2:d1805e7d46fb 407 {
pyrostew 12:814db1249a19 408 //double pow = 0.0;
pyrostew 12:814db1249a19 409 while(KpGain < 1.0)
pyrostew 0:398432a37ca5 410 {
pyrostew 12:814db1249a19 411 KpGain += 0.01;
pyrostew 12:814db1249a19 412 while(KiGain < 1.0)
pyrostew 0:398432a37ca5 413 {
pyrostew 12:814db1249a19 414 KiGain += 0.01;
alpesh 7:4cd7be306626 415
pyrostew 12:814db1249a19 416 while(KdGain < 1.0)
pyrostew 12:814db1249a19 417 {
pyrostew 12:814db1249a19 418 KdGain += 0.01;
pyrostew 12:814db1249a19 419
pyrostew 12:814db1249a19 420 float iterationStart = RunningTime.read();
pyrostew 12:814db1249a19 421
pyrostew 12:814db1249a19 422 while(RunningTime.read()-iterationStart < 10.0)
pyrostew 12:814db1249a19 423 {
pyrostew 12:814db1249a19 424 SerialTransmit();
pyrostew 12:814db1249a19 425
pyrostew 12:814db1249a19 426 Controller();
pyrostew 12:814db1249a19 427 }
pyrostew 12:814db1249a19 428 }
pyrostew 0:398432a37ca5 429 }
alpesh 7:4cd7be306626 430
alpesh 7:4cd7be306626 431 Home();
pyrostew 0:398432a37ca5 432 }
alpesh 2:d1805e7d46fb 433
alpesh 6:bfe745b152fa 434 Disable();
pyrostew 0:398432a37ca5 435 }
alpesh 7:4cd7be306626 436 }
alpesh 7:4cd7be306626 437
alpesh 7:4cd7be306626 438 /* Change this throttle value range to 1.0 and 0.0 for car speed
alpesh 7:4cd7be306626 439 m_throttlechange = (m_kpGain * m_error + m_kdGain * m_ErrorDer + m_ErrorInt);
alpesh 7:4cd7be306626 440
alpesh 7:4cd7be306626 441 double throttle = m_throttle + m_throttlechange;
alpesh 7:4cd7be306626 442
alpesh 7:4cd7be306626 443 if (new_throttle > 1.0) {
alpesh 7:4cd7be306626 444 m_throttle = 1.0;
alpesh 7:4cd7be306626 445 } else if (new_throttle < 0.0) {
alpesh 7:4cd7be306626 446 m_throttle = 0.0;
alpesh 7:4cd7be306626 447 } else {
alpesh 7:4cd7be306626 448 m_throttle = new_throttle;
alpesh 7:4cd7be306626 449 }*/
alpesh 7:4cd7be306626 450
alpesh 7:4cd7be306626 451
alpesh 7:4cd7be306626 452
alpesh 7:4cd7be306626 453
alpesh 7:4cd7be306626 454