Alpesh Bhudia / Mbed 2 deprecated EquatorStrutDigitalMonitor

Merged to branch

Dependencies:   USBDevice mbed EquatorStrutController LightWeightSerialTransmit

Fork of EquatorStrutDigitalMonitor by Stewart Coulden-Smith

main.cpp@9:ee84a9697de0, 2014-08-11 (annotated)

Committer:
alpesh
Date:
Mon Aug 11 13:48:04 2014 +0000
Revision:
9:ee84a9697de0
Parent:
8:7f6e81140f27
Child:
11:b6958b3dbddf
Adding Iterations and testing Proportional 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 0:398432a37ca5 6 InterruptIn RGHSinInterrupt(P0_11);
pyrostew 0:398432a37ca5 7 InterruptIn RGHCosInterrupt(P0_12);
pyrostew 0:398432a37ca5 8 InterruptIn HallSensor(P0_2);
pyrostew 0:398432a37ca5 9 DigitalOut ResetLine(P1_29);
pyrostew 0:398432a37ca5 10 PwmOut PhaseA(P0_9);
pyrostew 0:398432a37ca5 11 PwmOut PhaseB(P0_8);
pyrostew 0:398432a37ca5 12 Timer RunningTime;
pyrostew 0:398432a37ca5 13
pyrostew 0:398432a37ca5 14 bool Enabled = false;
pyrostew 0:398432a37ca5 15 bool Homing = false;
pyrostew 0:398432a37ca5 16 bool HallTriggered = false;
pyrostew 0:398432a37ca5 17
pyrostew 0:398432a37ca5 18 RawSerial pc(P1_27, P1_26);
pyrostew 0:398432a37ca5 19
pyrostew 0:398432a37ca5 20 char PinState = 0;
pyrostew 0:398432a37ca5 21
pyrostew 0:398432a37ca5 22 int direction = 0;
pyrostew 0:398432a37ca5 23 double position = 0.0;
pyrostew 0:398432a37ca5 24 double currentPower = 0.0;
pyrostew 1:a33723b70582 25 int interruptPeriod = 0;
pyrostew 1:a33723b70582 26 int lastTime = 0;
pyrostew 0:398432a37ca5 27
pyrostew 0:398432a37ca5 28 char counter = 0;
pyrostew 0:398432a37ca5 29
pyrostew 0:398432a37ca5 30 void RGHSinHandler()
pyrostew 0:398432a37ca5 31 {
pyrostew 0:398432a37ca5 32 if (PinState == 2)
pyrostew 0:398432a37ca5 33 {
pyrostew 0:398432a37ca5 34 return;
pyrostew 0:398432a37ca5 35 }
pyrostew 0:398432a37ca5 36 else if (PinState == 1)
pyrostew 0:398432a37ca5 37 {
pyrostew 0:398432a37ca5 38 PinState = 0 |(RGHSinState << 1) | RGHCosState;
pyrostew 0:398432a37ca5 39
pyrostew 0:398432a37ca5 40 if(PinState == 3)
pyrostew 0:398432a37ca5 41 {
pyrostew 0:398432a37ca5 42 direction = 1;
pyrostew 0:398432a37ca5 43 position += 0.04 * direction;
pyrostew 1:a33723b70582 44 interruptPeriod = RunningTime.read_us() - lastTime;
pyrostew 1:a33723b70582 45 lastTime = RunningTime.read_us();
pyrostew 0:398432a37ca5 46 }
pyrostew 0:398432a37ca5 47 }
pyrostew 0:398432a37ca5 48 else
pyrostew 0:398432a37ca5 49 {
pyrostew 0:398432a37ca5 50 PinState = 0 |(RGHSinState << 1) | RGHCosState;
pyrostew 0:398432a37ca5 51 }
pyrostew 0:398432a37ca5 52 }
pyrostew 0:398432a37ca5 53
pyrostew 0:398432a37ca5 54 void RGHCosHandler()
pyrostew 0:398432a37ca5 55 {
pyrostew 0:398432a37ca5 56 if (PinState == 1)
pyrostew 0:398432a37ca5 57 {
pyrostew 0:398432a37ca5 58 return;
pyrostew 0:398432a37ca5 59 }
pyrostew 0:398432a37ca5 60 else if (PinState == 2)
pyrostew 0:398432a37ca5 61 {
pyrostew 0:398432a37ca5 62 PinState = 0 |(RGHSinState << 1) | RGHCosState;
pyrostew 0:398432a37ca5 63
pyrostew 0:398432a37ca5 64 if (PinState == 3)
pyrostew 0:398432a37ca5 65 {
pyrostew 0:398432a37ca5 66 direction = -1;
pyrostew 0:398432a37ca5 67 position += 0.04 * direction;
pyrostew 1:a33723b70582 68 interruptPeriod = RunningTime.read_us() - lastTime;
pyrostew 1:a33723b70582 69 lastTime = RunningTime.read_us();
pyrostew 0:398432a37ca5 70 }
pyrostew 0:398432a37ca5 71 }
pyrostew 0:398432a37ca5 72 else
pyrostew 0:398432a37ca5 73 {
pyrostew 0:398432a37ca5 74 PinState = 0 |(RGHSinState << 1) | RGHCosState;
pyrostew 0:398432a37ca5 75 }
pyrostew 0:398432a37ca5 76 }
pyrostew 0:398432a37ca5 77
pyrostew 0:398432a37ca5 78 void SetPower(double power)
pyrostew 0:398432a37ca5 79 {
pyrostew 0:398432a37ca5 80 currentPower = power;
pyrostew 0:398432a37ca5 81 if(!Enabled)
pyrostew 0:398432a37ca5 82 {
pyrostew 0:398432a37ca5 83 return;
pyrostew 0:398432a37ca5 84 }
pyrostew 0:398432a37ca5 85
pyrostew 0:398432a37ca5 86 if (power > 1.0 || power < -1.0)
pyrostew 0:398432a37ca5 87 {
pyrostew 0:398432a37ca5 88 return;
pyrostew 0:398432a37ca5 89 }
pyrostew 0:398432a37ca5 90
pyrostew 0:398432a37ca5 91 PhaseA = (power + 1.0) / 2;
pyrostew 0:398432a37ca5 92 PhaseB = 1.0 - ((power + 1.0) / 2);
pyrostew 0:398432a37ca5 93 }
pyrostew 0:398432a37ca5 94
pyrostew 0:398432a37ca5 95 void Enable()
pyrostew 0:398432a37ca5 96 {
pyrostew 0:398432a37ca5 97 SetPower(0.0);
pyrostew 0:398432a37ca5 98
pyrostew 0:398432a37ca5 99 ResetLine = 1;
pyrostew 0:398432a37ca5 100
pyrostew 0:398432a37ca5 101 Enabled = true;
pyrostew 0:398432a37ca5 102 }
pyrostew 0:398432a37ca5 103
pyrostew 0:398432a37ca5 104 void Disable()
pyrostew 0:398432a37ca5 105 {
pyrostew 0:398432a37ca5 106 ResetLine = 0;
pyrostew 0:398432a37ca5 107
pyrostew 0:398432a37ca5 108 SetPower(0.0);
pyrostew 0:398432a37ca5 109
pyrostew 0:398432a37ca5 110 Enabled = false;
pyrostew 0:398432a37ca5 111 }
pyrostew 0:398432a37ca5 112
pyrostew 0:398432a37ca5 113 void Home()
pyrostew 0:398432a37ca5 114 {
pyrostew 0:398432a37ca5 115 if (!Enabled)
pyrostew 0:398432a37ca5 116 {
pyrostew 0:398432a37ca5 117 Enable();
pyrostew 0:398432a37ca5 118 }
pyrostew 0:398432a37ca5 119
pyrostew 0:398432a37ca5 120 Homing = true;
pyrostew 0:398432a37ca5 121 HallTriggered = false;
pyrostew 0:398432a37ca5 122
pyrostew 0:398432a37ca5 123 SetPower(-1.0);
pyrostew 0:398432a37ca5 124
pyrostew 0:398432a37ca5 125 while (!HallTriggered)
pyrostew 0:398432a37ca5 126 {
pyrostew 0:398432a37ca5 127 wait(0.5);
pyrostew 0:398432a37ca5 128 }
pyrostew 0:398432a37ca5 129
pyrostew 0:398432a37ca5 130 SetPower(1.0);
pyrostew 0:398432a37ca5 131
pyrostew 0:398432a37ca5 132 while (position < 20.0)
pyrostew 0:398432a37ca5 133 {
pyrostew 0:398432a37ca5 134
pyrostew 0:398432a37ca5 135 }
pyrostew 0:398432a37ca5 136
pyrostew 0:398432a37ca5 137 Homing = true;
pyrostew 0:398432a37ca5 138 HallTriggered = false;
pyrostew 0:398432a37ca5 139
alpesh 7:4cd7be306626 140 SetPower(-0.7);
pyrostew 0:398432a37ca5 141
pyrostew 0:398432a37ca5 142 while (!HallTriggered)
pyrostew 0:398432a37ca5 143 {
pyrostew 0:398432a37ca5 144 wait(0.5);
pyrostew 0:398432a37ca5 145 }
pyrostew 0:398432a37ca5 146 }
pyrostew 0:398432a37ca5 147
pyrostew 1:a33723b70582 148 double GetSpeed()
pyrostew 1:a33723b70582 149 {
alpesh 5:4468f8ff5ec6 150 if ((RunningTime - lastTime) > 10000 || interruptPeriod == 0)
pyrostew 1:a33723b70582 151 {
pyrostew 1:a33723b70582 152 return 0.0;
pyrostew 1:a33723b70582 153 }
pyrostew 1:a33723b70582 154 return (direction * 0.04)/((double)interruptPeriod / 1000000.0);
pyrostew 1:a33723b70582 155 }
pyrostew 1:a33723b70582 156
alpesh 6:bfe745b152fa 157 double Error = 0;
alpesh 6:bfe745b152fa 158
pyrostew 0:398432a37ca5 159 void SerialTransmit()
pyrostew 0:398432a37ca5 160 {
pyrostew 0:398432a37ca5 161 double tempPos = position;
pyrostew 0:398432a37ca5 162 double tempTime = RunningTime.read();
pyrostew 0:398432a37ca5 163 double tempPow = currentPower;
pyrostew 1:a33723b70582 164 double tempSpeed = GetSpeed();
alpesh 6:bfe745b152fa 165 double tempError = Error;
pyrostew 0:398432a37ca5 166 int outChar = 0;
pyrostew 0:398432a37ca5 167
pyrostew 0:398432a37ca5 168 if (tempPos < 0.0)
pyrostew 0:398432a37ca5 169 {
pyrostew 0:398432a37ca5 170 pc.putc('-');
pyrostew 0:398432a37ca5 171 tempPos *= -1;
pyrostew 0:398432a37ca5 172 }
pyrostew 0:398432a37ca5 173 if (tempPos >= 100.0)
pyrostew 0:398432a37ca5 174 {
pyrostew 0:398432a37ca5 175 outChar = tempPos / 100;
pyrostew 0:398432a37ca5 176 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 177 tempPos -= outChar * 100.0;
pyrostew 0:398432a37ca5 178 }
pyrostew 0:398432a37ca5 179 if (tempPos >= 10.0)
pyrostew 0:398432a37ca5 180 {
pyrostew 0:398432a37ca5 181 outChar = tempPos / 10;
pyrostew 0:398432a37ca5 182 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 183 tempPos -= outChar * 10.0;
pyrostew 0:398432a37ca5 184 }
pyrostew 0:398432a37ca5 185 else if(outChar > 0)
pyrostew 0:398432a37ca5 186 {
pyrostew 0:398432a37ca5 187 pc.putc('0');
pyrostew 0:398432a37ca5 188 }
pyrostew 0:398432a37ca5 189 if (tempPos >= 1.0)
pyrostew 0:398432a37ca5 190 {
pyrostew 0:398432a37ca5 191 outChar = tempPos;
pyrostew 0:398432a37ca5 192 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 193 tempPos -= outChar;
pyrostew 0:398432a37ca5 194 }
pyrostew 0:398432a37ca5 195 else
pyrostew 0:398432a37ca5 196 {
pyrostew 0:398432a37ca5 197 pc.putc('0');
pyrostew 0:398432a37ca5 198 }
pyrostew 0:398432a37ca5 199 if (tempPos >= 0.1)
pyrostew 0:398432a37ca5 200 {
pyrostew 0:398432a37ca5 201 pc.putc('.');
pyrostew 0:398432a37ca5 202 outChar = tempPos * 10;
pyrostew 0:398432a37ca5 203 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 204 tempPos -= (double)outChar / 10.0;
pyrostew 0:398432a37ca5 205 }
pyrostew 0:398432a37ca5 206 else
pyrostew 0:398432a37ca5 207 {
pyrostew 0:398432a37ca5 208 pc.putc('.');
pyrostew 0:398432a37ca5 209 pc.putc('0');
pyrostew 0:398432a37ca5 210 }
pyrostew 0:398432a37ca5 211 if (tempPos >= 0.01)
pyrostew 0:398432a37ca5 212 {
pyrostew 0:398432a37ca5 213 outChar = tempPos * 100;
pyrostew 0:398432a37ca5 214 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 215 }
pyrostew 0:398432a37ca5 216
alpesh 2:d1805e7d46fb 217 pc.putc('\t');
pyrostew 0:398432a37ca5 218
pyrostew 0:398432a37ca5 219 outChar = 0;
pyrostew 0:398432a37ca5 220
pyrostew 0:398432a37ca5 221 if (tempTime >= 100.0)
pyrostew 0:398432a37ca5 222 {
pyrostew 0:398432a37ca5 223 outChar = tempTime / 100;
pyrostew 0:398432a37ca5 224 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 225 tempTime -= outChar * 100.0;
pyrostew 0:398432a37ca5 226 }
pyrostew 0:398432a37ca5 227 if (tempTime >= 10.0)
pyrostew 0:398432a37ca5 228 {
pyrostew 0:398432a37ca5 229 outChar = tempTime / 10;
pyrostew 0:398432a37ca5 230 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 231 tempTime -= outChar * 10.0;
pyrostew 0:398432a37ca5 232 }
pyrostew 0:398432a37ca5 233 else if(outChar > 0)
pyrostew 0:398432a37ca5 234 {
pyrostew 0:398432a37ca5 235 pc.putc('0');
pyrostew 0:398432a37ca5 236 }
pyrostew 0:398432a37ca5 237 if (tempTime >= 1.0)
pyrostew 0:398432a37ca5 238 {
pyrostew 0:398432a37ca5 239 outChar = tempTime;
pyrostew 0:398432a37ca5 240 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 241 tempTime -= outChar;
pyrostew 0:398432a37ca5 242 }
pyrostew 0:398432a37ca5 243 else
pyrostew 0:398432a37ca5 244 {
pyrostew 0:398432a37ca5 245 pc.putc('0');
pyrostew 0:398432a37ca5 246 }
pyrostew 0:398432a37ca5 247 if (tempTime >= 0.1)
pyrostew 0:398432a37ca5 248 {
pyrostew 0:398432a37ca5 249 pc.putc('.');
pyrostew 0:398432a37ca5 250 outChar = tempTime * 10;
pyrostew 0:398432a37ca5 251 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 252 tempTime -= (double)outChar / 10.0;
pyrostew 0:398432a37ca5 253 }
pyrostew 0:398432a37ca5 254 else
pyrostew 0:398432a37ca5 255 {
pyrostew 0:398432a37ca5 256 pc.putc('.');
pyrostew 0:398432a37ca5 257 pc.putc('0');
pyrostew 0:398432a37ca5 258 }
pyrostew 0:398432a37ca5 259 if (tempTime >= 0.01)
pyrostew 0:398432a37ca5 260 {
pyrostew 0:398432a37ca5 261 outChar = tempTime * 100;
pyrostew 0:398432a37ca5 262 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 263 tempTime -= (double)outChar / 100.0;
pyrostew 0:398432a37ca5 264 }
pyrostew 0:398432a37ca5 265 else
pyrostew 0:398432a37ca5 266 {
pyrostew 0:398432a37ca5 267 pc.putc('0');
pyrostew 0:398432a37ca5 268 }
pyrostew 0:398432a37ca5 269 if (tempTime >= 0.001)
pyrostew 0:398432a37ca5 270 {
pyrostew 0:398432a37ca5 271 outChar= tempTime * 1000;
pyrostew 0:398432a37ca5 272 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 273 }
pyrostew 0:398432a37ca5 274
alpesh 2:d1805e7d46fb 275 pc.putc('\t');
pyrostew 0:398432a37ca5 276 outChar = 0;
pyrostew 0:398432a37ca5 277
pyrostew 0:398432a37ca5 278 if (tempPow < 0.0)
pyrostew 0:398432a37ca5 279 {
pyrostew 0:398432a37ca5 280 pc.putc('-');
pyrostew 0:398432a37ca5 281 tempPow *= -1;
pyrostew 0:398432a37ca5 282 }
pyrostew 0:398432a37ca5 283 if (tempPow >= 1.0)
pyrostew 0:398432a37ca5 284 {
pyrostew 0:398432a37ca5 285 outChar = tempPow;
pyrostew 0:398432a37ca5 286 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 287 tempPow -= outChar;
pyrostew 0:398432a37ca5 288 }
pyrostew 0:398432a37ca5 289 else
pyrostew 0:398432a37ca5 290 {
pyrostew 0:398432a37ca5 291 pc.putc('0');
pyrostew 0:398432a37ca5 292 }
pyrostew 0:398432a37ca5 293 if (tempPow >= 0.1)
pyrostew 0:398432a37ca5 294 {
pyrostew 0:398432a37ca5 295 pc.putc('.');
pyrostew 0:398432a37ca5 296 outChar = tempPow * 10;
pyrostew 0:398432a37ca5 297 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 298 tempPow -= (double)outChar / 10.0;
pyrostew 0:398432a37ca5 299 }
pyrostew 0:398432a37ca5 300 else
pyrostew 0:398432a37ca5 301 {
pyrostew 0:398432a37ca5 302 pc.putc('.');
pyrostew 0:398432a37ca5 303 pc.putc('0');
pyrostew 0:398432a37ca5 304 }
pyrostew 0:398432a37ca5 305 if (tempPow >= 0.01)
pyrostew 0:398432a37ca5 306 {
pyrostew 0:398432a37ca5 307 outChar = tempPow * 100;
pyrostew 0:398432a37ca5 308 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 309 tempPow -= (double)outChar / 100.0;
pyrostew 0:398432a37ca5 310 }
pyrostew 0:398432a37ca5 311 else
pyrostew 0:398432a37ca5 312 {
pyrostew 0:398432a37ca5 313 pc.putc('0');
pyrostew 0:398432a37ca5 314 }
pyrostew 0:398432a37ca5 315 if (tempPow >= 0.001)
pyrostew 0:398432a37ca5 316 {
pyrostew 0:398432a37ca5 317 outChar= tempPow * 1000;
pyrostew 0:398432a37ca5 318 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 319 }
pyrostew 0:398432a37ca5 320
alpesh 2:d1805e7d46fb 321 pc.putc('\t');
pyrostew 1:a33723b70582 322 outChar = 0;
pyrostew 1:a33723b70582 323
pyrostew 1:a33723b70582 324 if (tempSpeed < 0.0)
pyrostew 1:a33723b70582 325 {
pyrostew 1:a33723b70582 326 pc.putc('-');
pyrostew 1:a33723b70582 327 tempSpeed *= -1;
pyrostew 1:a33723b70582 328 }
pyrostew 1:a33723b70582 329 if (tempSpeed >= 100.0)
pyrostew 1:a33723b70582 330 {
pyrostew 1:a33723b70582 331 outChar = tempSpeed / 100;
pyrostew 1:a33723b70582 332 pc.putc(outChar + 48);
pyrostew 1:a33723b70582 333 tempSpeed -= outChar * 100.0;
pyrostew 1:a33723b70582 334 }
pyrostew 1:a33723b70582 335 if (tempSpeed >= 10.0)
pyrostew 1:a33723b70582 336 {
pyrostew 1:a33723b70582 337 outChar = tempSpeed / 10;
pyrostew 1:a33723b70582 338 pc.putc(outChar + 48);
pyrostew 1:a33723b70582 339 tempSpeed -= outChar * 10.0;
pyrostew 1:a33723b70582 340 }
pyrostew 1:a33723b70582 341 else if(outChar > 0)
pyrostew 1:a33723b70582 342 {
pyrostew 1:a33723b70582 343 pc.putc('0');
pyrostew 1:a33723b70582 344 }
pyrostew 1:a33723b70582 345 if (tempSpeed >= 1.0)
pyrostew 1:a33723b70582 346 {
pyrostew 1:a33723b70582 347 outChar = tempSpeed;
pyrostew 1:a33723b70582 348 pc.putc(outChar + 48);
pyrostew 1:a33723b70582 349 tempSpeed -= outChar;
pyrostew 1:a33723b70582 350 }
pyrostew 1:a33723b70582 351 else
pyrostew 1:a33723b70582 352 {
pyrostew 1:a33723b70582 353 pc.putc('0');
pyrostew 1:a33723b70582 354 }
pyrostew 1:a33723b70582 355 if (tempSpeed >= 0.1)
pyrostew 1:a33723b70582 356 {
pyrostew 1:a33723b70582 357 pc.putc('.');
pyrostew 1:a33723b70582 358 outChar = tempSpeed * 10;
pyrostew 1:a33723b70582 359 pc.putc(outChar + 48);
pyrostew 1:a33723b70582 360 tempSpeed -= (double)outChar / 10.0;
pyrostew 1:a33723b70582 361 }
pyrostew 1:a33723b70582 362 else
pyrostew 1:a33723b70582 363 {
pyrostew 1:a33723b70582 364 pc.putc('.');
pyrostew 1:a33723b70582 365 pc.putc('0');
pyrostew 1:a33723b70582 366 }
pyrostew 1:a33723b70582 367 if (tempSpeed >= 0.01)
pyrostew 1:a33723b70582 368 {
pyrostew 1:a33723b70582 369 outChar = tempSpeed * 100;
pyrostew 1:a33723b70582 370 pc.putc(outChar + 48);
pyrostew 1:a33723b70582 371 }
pyrostew 1:a33723b70582 372
alpesh 6:bfe745b152fa 373 pc.putc('\t');
alpesh 6:bfe745b152fa 374 outChar = 0;
alpesh 6:bfe745b152fa 375
alpesh 6:bfe745b152fa 376 if (tempError < 0.0)
alpesh 6:bfe745b152fa 377 {
alpesh 6:bfe745b152fa 378 pc.putc('-');
alpesh 6:bfe745b152fa 379 tempError *= -1;
alpesh 6:bfe745b152fa 380 }
alpesh 6:bfe745b152fa 381 if (tempError >= 100.0)
alpesh 6:bfe745b152fa 382 {
alpesh 6:bfe745b152fa 383 outChar = tempError / 100;
alpesh 6:bfe745b152fa 384 pc.putc(outChar + 48);
alpesh 6:bfe745b152fa 385 tempError -= outChar * 100.0;
alpesh 6:bfe745b152fa 386 }
alpesh 6:bfe745b152fa 387 if (tempError >= 10.0)
alpesh 6:bfe745b152fa 388 {
alpesh 6:bfe745b152fa 389 outChar = tempError / 10;
alpesh 6:bfe745b152fa 390 pc.putc(outChar + 48);
alpesh 6:bfe745b152fa 391 tempError -= outChar * 10.0;
alpesh 6:bfe745b152fa 392 }
alpesh 6:bfe745b152fa 393 else if(outChar > 0)
alpesh 6:bfe745b152fa 394 {
alpesh 6:bfe745b152fa 395 pc.putc('0');
alpesh 6:bfe745b152fa 396 }
alpesh 6:bfe745b152fa 397 if (tempError >= 1.0)
alpesh 6:bfe745b152fa 398 {
alpesh 6:bfe745b152fa 399 outChar = tempError;
alpesh 6:bfe745b152fa 400 pc.putc(outChar + 48);
alpesh 6:bfe745b152fa 401 tempError -= outChar;
alpesh 6:bfe745b152fa 402 }
alpesh 6:bfe745b152fa 403 else
alpesh 6:bfe745b152fa 404 {
alpesh 6:bfe745b152fa 405 pc.putc('0');
alpesh 6:bfe745b152fa 406 }
alpesh 6:bfe745b152fa 407 if (tempError >= 0.1)
alpesh 6:bfe745b152fa 408 {
alpesh 6:bfe745b152fa 409 pc.putc('.');
alpesh 6:bfe745b152fa 410 outChar = tempError * 10;
alpesh 6:bfe745b152fa 411 pc.putc(outChar + 48);
alpesh 6:bfe745b152fa 412 tempError -= (double)outChar / 10.0;
alpesh 6:bfe745b152fa 413 }
alpesh 6:bfe745b152fa 414 else
alpesh 6:bfe745b152fa 415 {
alpesh 6:bfe745b152fa 416 pc.putc('.');
alpesh 6:bfe745b152fa 417 pc.putc('0');
alpesh 6:bfe745b152fa 418 }
alpesh 6:bfe745b152fa 419 if (tempError >= 0.01)
alpesh 6:bfe745b152fa 420 {
alpesh 6:bfe745b152fa 421 outChar = tempError * 100;
alpesh 6:bfe745b152fa 422 pc.putc(outChar + 48);
alpesh 6:bfe745b152fa 423 }
alpesh 6:bfe745b152fa 424
pyrostew 0:398432a37ca5 425 pc.putc(10);
pyrostew 0:398432a37ca5 426 pc.putc(13);
pyrostew 0:398432a37ca5 427 }
pyrostew 0:398432a37ca5 428
pyrostew 0:398432a37ca5 429 void HallEffectFall()
pyrostew 0:398432a37ca5 430 {
pyrostew 0:398432a37ca5 431 RGHSinInterrupt.disable_irq();
pyrostew 0:398432a37ca5 432 RGHCosInterrupt.disable_irq();
pyrostew 0:398432a37ca5 433
pyrostew 0:398432a37ca5 434 if (direction < 0)
pyrostew 0:398432a37ca5 435 {
pyrostew 0:398432a37ca5 436 SetPower(0.0);
pyrostew 0:398432a37ca5 437
pyrostew 0:398432a37ca5 438 if (Homing)
pyrostew 0:398432a37ca5 439 {
pyrostew 0:398432a37ca5 440 HallTriggered = true;
pyrostew 0:398432a37ca5 441 Homing = false;
pyrostew 0:398432a37ca5 442 position = 0.0;
pyrostew 0:398432a37ca5 443 }
pyrostew 0:398432a37ca5 444 }
pyrostew 0:398432a37ca5 445 RGHSinInterrupt.enable_irq();
pyrostew 0:398432a37ca5 446 RGHCosInterrupt.enable_irq();
pyrostew 0:398432a37ca5 447 }
pyrostew 0:398432a37ca5 448
alpesh 7:4cd7be306626 449 double SetPoint = 50.0; //Millimeter per second
alpesh 6:bfe745b152fa 450
alpesh 2:d1805e7d46fb 451 double KpTerm;
alpesh 3:e693c65b04de 452 double ErrorInt;
alpesh 2:d1805e7d46fb 453 double ErrorDer;
alpesh 2:d1805e7d46fb 454
alpesh 7:4cd7be306626 455 double KpGain = 0.0;
alpesh 2:d1805e7d46fb 456 double KiGain = 0.0;
alpesh 2:d1805e7d46fb 457 double KdGain = 0.0;
alpesh 2:d1805e7d46fb 458
alpesh 3:e693c65b04de 459 int errorcounter;
alpesh 2:d1805e7d46fb 460 double PreviousError [10];
alpesh 2:d1805e7d46fb 461
alpesh 3:e693c65b04de 462 double PwmChange=0;
alpesh 3:e693c65b04de 463
alpesh 3:e693c65b04de 464 double pwm;
alpesh 3:e693c65b04de 465 double NewPwm;
alpesh 3:e693c65b04de 466
alpesh 3:e693c65b04de 467 int previousTime = 0;
alpesh 3:e693c65b04de 468
alpesh 4:2ec05810bc47 469 void Controller ()
alpesh 4:2ec05810bc47 470 {
alpesh 7:4cd7be306626 471 if (position <= SetPoint || 1)
alpesh 4:2ec05810bc47 472 {
alpesh 4:2ec05810bc47 473 int timeStep = RunningTime.read_us() - previousTime;
alpesh 4:2ec05810bc47 474 previousTime = RunningTime.read_us();
alpesh 4:2ec05810bc47 475
alpesh 7:4cd7be306626 476 Error = SetPoint - position;
alpesh 4:2ec05810bc47 477
alpesh 4:2ec05810bc47 478 KpTerm = Error * KpGain;
alpesh 4:2ec05810bc47 479
alpesh 6:bfe745b152fa 480 ErrorDer = (Error - PreviousError[errorcounter]) / timeStep;
alpesh 4:2ec05810bc47 481
alpesh 4:2ec05810bc47 482 ErrorInt = ErrorInt + Error * timeStep;
alpesh 4:2ec05810bc47 483
alpesh 7:4cd7be306626 484 NewPwm = (KpTerm + KiGain * ErrorInt + KdGain * ErrorDer);
alpesh 4:2ec05810bc47 485
alpesh 4:2ec05810bc47 486 if (NewPwm > 1.0)
alpesh 4:2ec05810bc47 487 {
alpesh 4:2ec05810bc47 488 pwm = 1.0;
alpesh 4:2ec05810bc47 489 }
alpesh 4:2ec05810bc47 490
alpesh 5:4468f8ff5ec6 491 else if (NewPwm < -1.0)
alpesh 4:2ec05810bc47 492 {
alpesh 5:4468f8ff5ec6 493 pwm = -1.0;
alpesh 4:2ec05810bc47 494 }
alpesh 4:2ec05810bc47 495
alpesh 4:2ec05810bc47 496 else
alpesh 4:2ec05810bc47 497 {
alpesh 4:2ec05810bc47 498 pwm = NewPwm;
alpesh 4:2ec05810bc47 499 }
alpesh 4:2ec05810bc47 500
alpesh 4:2ec05810bc47 501 SetPower(pwm);
alpesh 4:2ec05810bc47 502
alpesh 4:2ec05810bc47 503 errorcounter ++;
alpesh 4:2ec05810bc47 504
alpesh 6:bfe745b152fa 505 if (errorcounter > 9)
alpesh 4:2ec05810bc47 506 {
alpesh 6:bfe745b152fa 507 errorcounter = 0;
alpesh 4:2ec05810bc47 508 }
alpesh 4:2ec05810bc47 509
alpesh 4:2ec05810bc47 510 PreviousError[errorcounter] = Error;
alpesh 4:2ec05810bc47 511 }
alpesh 4:2ec05810bc47 512 }
alpesh 4:2ec05810bc47 513
pyrostew 0:398432a37ca5 514 int main()
alpesh 2:d1805e7d46fb 515
pyrostew 0:398432a37ca5 516 {
pyrostew 0:398432a37ca5 517 RGHSinInterrupt.rise(&RGHSinHandler);
pyrostew 0:398432a37ca5 518 RGHCosInterrupt.rise(&RGHCosHandler);
pyrostew 0:398432a37ca5 519 HallSensor.fall(&HallEffectFall);
pyrostew 0:398432a37ca5 520 HallSensor.mode(PullUp);
pyrostew 0:398432a37ca5 521
pyrostew 0:398432a37ca5 522 RunningTime.start();
pyrostew 0:398432a37ca5 523
pyrostew 0:398432a37ca5 524 pc.baud(115200);
pyrostew 0:398432a37ca5 525
alpesh 4:2ec05810bc47 526 Home();
alpesh 4:2ec05810bc47 527 //Enable();
pyrostew 0:398432a37ca5 528
alpesh 3:e693c65b04de 529 errorcounter = 0;
alpesh 3:e693c65b04de 530 PreviousError[errorcounter]=0;
alpesh 2:d1805e7d46fb 531
alpesh 3:e693c65b04de 532 previousTime = RunningTime.read_us();
alpesh 3:e693c65b04de 533
alpesh 6:bfe745b152fa 534 while(Enabled)
alpesh 2:d1805e7d46fb 535 {
alpesh 9:ee84a9697de0 536 double pow = 0.0;
alpesh 9:ee84a9697de0 537 for (int i = 0; i < 20; i++)
alpesh 3:e693c65b04de 538 {
alpesh 7:4cd7be306626 539 //KpGain += 0.1;
alpesh 7:4cd7be306626 540 float iterationStart = RunningTime.read();
alpesh 7:4cd7be306626 541
alpesh 7:4cd7be306626 542 pow += 0.05;
alpesh 7:4cd7be306626 543
alpesh 9:ee84a9697de0 544 if (pow > 1.0)
alpesh 9:ee84a9697de0 545 {
alpesh 9:ee84a9697de0 546 pow = 1.0;
alpesh 9:ee84a9697de0 547 }
alpesh 9:ee84a9697de0 548
alpesh 7:4cd7be306626 549 //while(RunningTime.read()-iterationStart < 10.0)
alpesh 7:4cd7be306626 550 //{
alpesh 7:4cd7be306626 551 // SerialTransmit();
alpesh 7:4cd7be306626 552
alpesh 7:4cd7be306626 553 // Controller();
alpesh 7:4cd7be306626 554 //}
alpesh 5:4468f8ff5ec6 555
alpesh 7:4cd7be306626 556 while(RunningTime.read() - iterationStart < 30.0 && position < 210.0)
alpesh 7:4cd7be306626 557 {
alpesh 7:4cd7be306626 558 SerialTransmit();
alpesh 7:4cd7be306626 559 SetPower(pow);
alpesh 7:4cd7be306626 560 }
alpesh 7:4cd7be306626 561
alpesh 7:4cd7be306626 562 iterationStart = RunningTime.read();
alpesh 7:4cd7be306626 563
alpesh 7:4cd7be306626 564 while(RunningTime.read() - iterationStart < 30.0 && position > 10.0)
alpesh 7:4cd7be306626 565 {
alpesh 7:4cd7be306626 566 SerialTransmit();
alpesh 7:4cd7be306626 567 SetPower(-pow);
alpesh 7:4cd7be306626 568 }
alpesh 7:4cd7be306626 569
alpesh 7:4cd7be306626 570 Home();
alpesh 4:2ec05810bc47 571 }
alpesh 2:d1805e7d46fb 572
alpesh 6:bfe745b152fa 573 Disable();
pyrostew 0:398432a37ca5 574 }
alpesh 7:4cd7be306626 575 }
alpesh 7:4cd7be306626 576
alpesh 7:4cd7be306626 577 /* Change this throttle value range to 1.0 and 0.0 for car speed
alpesh 7:4cd7be306626 578 m_throttlechange = (m_kpGain * m_error + m_kdGain * m_ErrorDer + m_ErrorInt);
alpesh 7:4cd7be306626 579
alpesh 7:4cd7be306626 580 double throttle = m_throttle + m_throttlechange;
alpesh 7:4cd7be306626 581
alpesh 7:4cd7be306626 582 if (new_throttle > 1.0) {
alpesh 7:4cd7be306626 583 m_throttle = 1.0;
alpesh 7:4cd7be306626 584 } else if (new_throttle < 0.0) {
alpesh 7:4cd7be306626 585 m_throttle = 0.0;
alpesh 7:4cd7be306626 586 } else {
alpesh 7:4cd7be306626 587 m_throttle = new_throttle;
alpesh 7:4cd7be306626 588 }*/
alpesh 7:4cd7be306626 589
alpesh 7:4cd7be306626 590
alpesh 7:4cd7be306626 591
alpesh 7:4cd7be306626 592
alpesh 7:4cd7be306626 593