Stewart Coulden-Smith / Mbed 2 deprecated EquatorStrutDigitalMonitor

Equator Strut Controller.

Dependencies:   USBDevice mbed

main.cpp@2:088eeae4287c, 2014-08-07 (annotated)

Committer:
pyrostew
Date:
Thu Aug 07 10:37:03 2014 +0000
Revision:
2:088eeae4287c
Parent:
1:a33723b70582 
Added smoothing algorithm to interrupt period value.

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 2:088eeae4287c 28 int intteruptPeriodArray[15];
pyrostew 2:088eeae4287c 29 int arrayTotal = 0;
pyrostew 2:088eeae4287c 30 char arrayPos = 0;
pyrostew 2:088eeae4287c 31
pyrostew 0:398432a37ca5 32 char counter = 0;
pyrostew 0:398432a37ca5 33
pyrostew 2:088eeae4287c 34 void SmoothingAdd(int input)
pyrostew 2:088eeae4287c 35 {
pyrostew 2:088eeae4287c 36 arrayTotal -= intteruptPeriodArray[arrayPos];
pyrostew 2:088eeae4287c 37 arrayTotal += input;
pyrostew 2:088eeae4287c 38 intteruptPeriodArray[arrayPos] = input;
pyrostew 2:088eeae4287c 39
pyrostew 2:088eeae4287c 40 if (arrayPos == 14)
pyrostew 2:088eeae4287c 41 {
pyrostew 2:088eeae4287c 42 arrayPos = 0;
pyrostew 2:088eeae4287c 43 }
pyrostew 2:088eeae4287c 44 else
pyrostew 2:088eeae4287c 45 {
pyrostew 2:088eeae4287c 46 arrayTotal++;
pyrostew 2:088eeae4287c 47 }
pyrostew 2:088eeae4287c 48 }
pyrostew 2:088eeae4287c 49
pyrostew 2:088eeae4287c 50 int SmoothedInterruptPeriod()
pyrostew 2:088eeae4287c 51 {
pyrostew 2:088eeae4287c 52 return arrayTotal / 15;
pyrostew 2:088eeae4287c 53 }
pyrostew 2:088eeae4287c 54
pyrostew 0:398432a37ca5 55 void RGHSinHandler()
pyrostew 0:398432a37ca5 56 {
pyrostew 0:398432a37ca5 57 if (PinState == 2)
pyrostew 0:398432a37ca5 58 {
pyrostew 0:398432a37ca5 59 return;
pyrostew 0:398432a37ca5 60 }
pyrostew 0:398432a37ca5 61 else if (PinState == 1)
pyrostew 0:398432a37ca5 62 {
pyrostew 0:398432a37ca5 63 PinState = 0 |(RGHSinState << 1) | RGHCosState;
pyrostew 0:398432a37ca5 64
pyrostew 0:398432a37ca5 65 if(PinState == 3)
pyrostew 0:398432a37ca5 66 {
pyrostew 0:398432a37ca5 67 direction = 1;
pyrostew 0:398432a37ca5 68 position += 0.04 * direction;
pyrostew 2:088eeae4287c 69 SmoothingAdd(RunningTime.read_us() - lastTime);
pyrostew 1:a33723b70582 70 lastTime = RunningTime.read_us();
pyrostew 0:398432a37ca5 71 }
pyrostew 0:398432a37ca5 72 }
pyrostew 0:398432a37ca5 73 else
pyrostew 0:398432a37ca5 74 {
pyrostew 0:398432a37ca5 75 PinState = 0 |(RGHSinState << 1) | RGHCosState;
pyrostew 0:398432a37ca5 76 }
pyrostew 0:398432a37ca5 77 }
pyrostew 0:398432a37ca5 78
pyrostew 0:398432a37ca5 79 void RGHCosHandler()
pyrostew 0:398432a37ca5 80 {
pyrostew 0:398432a37ca5 81 if (PinState == 1)
pyrostew 0:398432a37ca5 82 {
pyrostew 0:398432a37ca5 83 return;
pyrostew 0:398432a37ca5 84 }
pyrostew 0:398432a37ca5 85 else if (PinState == 2)
pyrostew 0:398432a37ca5 86 {
pyrostew 0:398432a37ca5 87 PinState = 0 |(RGHSinState << 1) | RGHCosState;
pyrostew 0:398432a37ca5 88
pyrostew 0:398432a37ca5 89 if (PinState == 3)
pyrostew 0:398432a37ca5 90 {
pyrostew 0:398432a37ca5 91 direction = -1;
pyrostew 0:398432a37ca5 92 position += 0.04 * direction;
pyrostew 2:088eeae4287c 93 SmoothingAdd(RunningTime.read_us() - lastTime);
pyrostew 1:a33723b70582 94 lastTime = RunningTime.read_us();
pyrostew 0:398432a37ca5 95 }
pyrostew 0:398432a37ca5 96 }
pyrostew 0:398432a37ca5 97 else
pyrostew 0:398432a37ca5 98 {
pyrostew 0:398432a37ca5 99 PinState = 0 |(RGHSinState << 1) | RGHCosState;
pyrostew 0:398432a37ca5 100 }
pyrostew 0:398432a37ca5 101 }
pyrostew 0:398432a37ca5 102
pyrostew 0:398432a37ca5 103 void SetPower(double power)
pyrostew 0:398432a37ca5 104 {
pyrostew 0:398432a37ca5 105 currentPower = power;
pyrostew 0:398432a37ca5 106 if(!Enabled)
pyrostew 0:398432a37ca5 107 {
pyrostew 0:398432a37ca5 108 return;
pyrostew 0:398432a37ca5 109 }
pyrostew 0:398432a37ca5 110
pyrostew 0:398432a37ca5 111 if (power > 1.0 || power < -1.0)
pyrostew 0:398432a37ca5 112 {
pyrostew 0:398432a37ca5 113 return;
pyrostew 0:398432a37ca5 114 }
pyrostew 0:398432a37ca5 115
pyrostew 0:398432a37ca5 116 PhaseA = (power + 1.0) / 2;
pyrostew 0:398432a37ca5 117 PhaseB = 1.0 - ((power + 1.0) / 2);
pyrostew 0:398432a37ca5 118 }
pyrostew 0:398432a37ca5 119
pyrostew 0:398432a37ca5 120 void Enable()
pyrostew 0:398432a37ca5 121 {
pyrostew 0:398432a37ca5 122 SetPower(0.0);
pyrostew 0:398432a37ca5 123
pyrostew 0:398432a37ca5 124 ResetLine = 1;
pyrostew 0:398432a37ca5 125
pyrostew 0:398432a37ca5 126 Enabled = true;
pyrostew 0:398432a37ca5 127 }
pyrostew 0:398432a37ca5 128
pyrostew 0:398432a37ca5 129 void Disable()
pyrostew 0:398432a37ca5 130 {
pyrostew 0:398432a37ca5 131 ResetLine = 0;
pyrostew 0:398432a37ca5 132
pyrostew 0:398432a37ca5 133 SetPower(0.0);
pyrostew 0:398432a37ca5 134
pyrostew 0:398432a37ca5 135 Enabled = false;
pyrostew 0:398432a37ca5 136 }
pyrostew 0:398432a37ca5 137
pyrostew 0:398432a37ca5 138 void Home()
pyrostew 0:398432a37ca5 139 {
pyrostew 0:398432a37ca5 140 if (!Enabled)
pyrostew 0:398432a37ca5 141 {
pyrostew 0:398432a37ca5 142 Enable();
pyrostew 0:398432a37ca5 143 }
pyrostew 0:398432a37ca5 144
pyrostew 0:398432a37ca5 145 Homing = true;
pyrostew 0:398432a37ca5 146 HallTriggered = false;
pyrostew 0:398432a37ca5 147
pyrostew 0:398432a37ca5 148 SetPower(-1.0);
pyrostew 0:398432a37ca5 149
pyrostew 0:398432a37ca5 150 while (!HallTriggered)
pyrostew 0:398432a37ca5 151 {
pyrostew 0:398432a37ca5 152 wait(0.5);
pyrostew 0:398432a37ca5 153 }
pyrostew 0:398432a37ca5 154
pyrostew 0:398432a37ca5 155 SetPower(1.0);
pyrostew 0:398432a37ca5 156
pyrostew 0:398432a37ca5 157 while (position < 20.0)
pyrostew 0:398432a37ca5 158 {
pyrostew 0:398432a37ca5 159
pyrostew 0:398432a37ca5 160 }
pyrostew 0:398432a37ca5 161
pyrostew 0:398432a37ca5 162 Homing = true;
pyrostew 0:398432a37ca5 163 HallTriggered = false;
pyrostew 0:398432a37ca5 164
pyrostew 0:398432a37ca5 165 SetPower(-0.5);
pyrostew 0:398432a37ca5 166
pyrostew 0:398432a37ca5 167 while (!HallTriggered)
pyrostew 0:398432a37ca5 168 {
pyrostew 0:398432a37ca5 169 wait(0.5);
pyrostew 0:398432a37ca5 170 }
pyrostew 0:398432a37ca5 171 }
pyrostew 0:398432a37ca5 172
pyrostew 1:a33723b70582 173 double GetSpeed()
pyrostew 1:a33723b70582 174 {
pyrostew 1:a33723b70582 175 if ((RunningTime - lastTime) > 10000)
pyrostew 1:a33723b70582 176 {
pyrostew 1:a33723b70582 177 return 0.0;
pyrostew 1:a33723b70582 178 }
pyrostew 2:088eeae4287c 179 return (direction * 0.04)/((double)SmoothedInterruptPeriod() / 1000000.0);
pyrostew 1:a33723b70582 180 }
pyrostew 1:a33723b70582 181
pyrostew 0:398432a37ca5 182 void SerialTransmit()
pyrostew 0:398432a37ca5 183 {
pyrostew 0:398432a37ca5 184 double tempPos = position;
pyrostew 0:398432a37ca5 185 double tempTime = RunningTime.read();
pyrostew 0:398432a37ca5 186 double tempPow = currentPower;
pyrostew 1:a33723b70582 187 double tempSpeed = GetSpeed();
pyrostew 0:398432a37ca5 188 int outChar = 0;
pyrostew 0:398432a37ca5 189
pyrostew 0:398432a37ca5 190 if (tempPos < 0.0)
pyrostew 0:398432a37ca5 191 {
pyrostew 0:398432a37ca5 192 pc.putc('-');
pyrostew 0:398432a37ca5 193 tempPos *= -1;
pyrostew 0:398432a37ca5 194 }
pyrostew 0:398432a37ca5 195 if (tempPos >= 100.0)
pyrostew 0:398432a37ca5 196 {
pyrostew 0:398432a37ca5 197 outChar = tempPos / 100;
pyrostew 0:398432a37ca5 198 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 199 tempPos -= outChar * 100.0;
pyrostew 0:398432a37ca5 200 }
pyrostew 0:398432a37ca5 201 if (tempPos >= 10.0)
pyrostew 0:398432a37ca5 202 {
pyrostew 0:398432a37ca5 203 outChar = tempPos / 10;
pyrostew 0:398432a37ca5 204 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 205 tempPos -= outChar * 10.0;
pyrostew 0:398432a37ca5 206 }
pyrostew 0:398432a37ca5 207 else if(outChar > 0)
pyrostew 0:398432a37ca5 208 {
pyrostew 0:398432a37ca5 209 pc.putc('0');
pyrostew 0:398432a37ca5 210 }
pyrostew 0:398432a37ca5 211 if (tempPos >= 1.0)
pyrostew 0:398432a37ca5 212 {
pyrostew 0:398432a37ca5 213 outChar = tempPos;
pyrostew 0:398432a37ca5 214 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 215 tempPos -= outChar;
pyrostew 0:398432a37ca5 216 }
pyrostew 0:398432a37ca5 217 else
pyrostew 0:398432a37ca5 218 {
pyrostew 0:398432a37ca5 219 pc.putc('0');
pyrostew 0:398432a37ca5 220 }
pyrostew 0:398432a37ca5 221 if (tempPos >= 0.1)
pyrostew 0:398432a37ca5 222 {
pyrostew 0:398432a37ca5 223 pc.putc('.');
pyrostew 0:398432a37ca5 224 outChar = tempPos * 10;
pyrostew 0:398432a37ca5 225 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 226 tempPos -= (double)outChar / 10.0;
pyrostew 0:398432a37ca5 227 }
pyrostew 0:398432a37ca5 228 else
pyrostew 0:398432a37ca5 229 {
pyrostew 0:398432a37ca5 230 pc.putc('.');
pyrostew 0:398432a37ca5 231 pc.putc('0');
pyrostew 0:398432a37ca5 232 }
pyrostew 0:398432a37ca5 233 if (tempPos >= 0.01)
pyrostew 0:398432a37ca5 234 {
pyrostew 0:398432a37ca5 235 outChar = tempPos * 100;
pyrostew 0:398432a37ca5 236 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 237 }
pyrostew 0:398432a37ca5 238
pyrostew 0:398432a37ca5 239 pc.putc(',');
pyrostew 0:398432a37ca5 240
pyrostew 0:398432a37ca5 241 outChar = 0;
pyrostew 0:398432a37ca5 242
pyrostew 0:398432a37ca5 243 if (tempTime >= 100.0)
pyrostew 0:398432a37ca5 244 {
pyrostew 0:398432a37ca5 245 outChar = tempTime / 100;
pyrostew 0:398432a37ca5 246 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 247 tempTime -= outChar * 100.0;
pyrostew 0:398432a37ca5 248 }
pyrostew 0:398432a37ca5 249 if (tempTime >= 10.0)
pyrostew 0:398432a37ca5 250 {
pyrostew 0:398432a37ca5 251 outChar = tempTime / 10;
pyrostew 0:398432a37ca5 252 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 253 tempTime -= outChar * 10.0;
pyrostew 0:398432a37ca5 254 }
pyrostew 0:398432a37ca5 255 else if(outChar > 0)
pyrostew 0:398432a37ca5 256 {
pyrostew 0:398432a37ca5 257 pc.putc('0');
pyrostew 0:398432a37ca5 258 }
pyrostew 0:398432a37ca5 259 if (tempTime >= 1.0)
pyrostew 0:398432a37ca5 260 {
pyrostew 0:398432a37ca5 261 outChar = tempTime;
pyrostew 0:398432a37ca5 262 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 263 tempTime -= outChar;
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.1)
pyrostew 0:398432a37ca5 270 {
pyrostew 0:398432a37ca5 271 pc.putc('.');
pyrostew 0:398432a37ca5 272 outChar = tempTime * 10;
pyrostew 0:398432a37ca5 273 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 274 tempTime -= (double)outChar / 10.0;
pyrostew 0:398432a37ca5 275 }
pyrostew 0:398432a37ca5 276 else
pyrostew 0:398432a37ca5 277 {
pyrostew 0:398432a37ca5 278 pc.putc('.');
pyrostew 0:398432a37ca5 279 pc.putc('0');
pyrostew 0:398432a37ca5 280 }
pyrostew 0:398432a37ca5 281 if (tempTime >= 0.01)
pyrostew 0:398432a37ca5 282 {
pyrostew 0:398432a37ca5 283 outChar = tempTime * 100;
pyrostew 0:398432a37ca5 284 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 285 tempTime -= (double)outChar / 100.0;
pyrostew 0:398432a37ca5 286 }
pyrostew 0:398432a37ca5 287 else
pyrostew 0:398432a37ca5 288 {
pyrostew 0:398432a37ca5 289 pc.putc('0');
pyrostew 0:398432a37ca5 290 }
pyrostew 0:398432a37ca5 291 if (tempTime >= 0.001)
pyrostew 0:398432a37ca5 292 {
pyrostew 0:398432a37ca5 293 outChar= tempTime * 1000;
pyrostew 0:398432a37ca5 294 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 295 }
pyrostew 0:398432a37ca5 296
pyrostew 0:398432a37ca5 297 pc.putc(',');
pyrostew 0:398432a37ca5 298 outChar = 0;
pyrostew 0:398432a37ca5 299
pyrostew 0:398432a37ca5 300 if (tempPow < 0.0)
pyrostew 0:398432a37ca5 301 {
pyrostew 0:398432a37ca5 302 pc.putc('-');
pyrostew 0:398432a37ca5 303 tempPow *= -1;
pyrostew 0:398432a37ca5 304 }
pyrostew 0:398432a37ca5 305 if (tempPow >= 1.0)
pyrostew 0:398432a37ca5 306 {
pyrostew 0:398432a37ca5 307 outChar = tempPow;
pyrostew 0:398432a37ca5 308 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 309 tempPow -= outChar;
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.1)
pyrostew 0:398432a37ca5 316 {
pyrostew 0:398432a37ca5 317 pc.putc('.');
pyrostew 0:398432a37ca5 318 outChar = tempPow * 10;
pyrostew 0:398432a37ca5 319 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 320 tempPow -= (double)outChar / 10.0;
pyrostew 0:398432a37ca5 321 }
pyrostew 0:398432a37ca5 322 else
pyrostew 0:398432a37ca5 323 {
pyrostew 0:398432a37ca5 324 pc.putc('.');
pyrostew 0:398432a37ca5 325 pc.putc('0');
pyrostew 0:398432a37ca5 326 }
pyrostew 0:398432a37ca5 327 if (tempPow >= 0.01)
pyrostew 0:398432a37ca5 328 {
pyrostew 0:398432a37ca5 329 outChar = tempPow * 100;
pyrostew 0:398432a37ca5 330 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 331 tempPow -= (double)outChar / 100.0;
pyrostew 0:398432a37ca5 332 }
pyrostew 0:398432a37ca5 333 else
pyrostew 0:398432a37ca5 334 {
pyrostew 0:398432a37ca5 335 pc.putc('0');
pyrostew 0:398432a37ca5 336 }
pyrostew 0:398432a37ca5 337 if (tempPow >= 0.001)
pyrostew 0:398432a37ca5 338 {
pyrostew 0:398432a37ca5 339 outChar= tempPow * 1000;
pyrostew 0:398432a37ca5 340 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 341 }
pyrostew 0:398432a37ca5 342
pyrostew 1:a33723b70582 343 pc.putc(',');
pyrostew 1:a33723b70582 344 outChar = 0;
pyrostew 1:a33723b70582 345
pyrostew 1:a33723b70582 346 if (tempSpeed < 0.0)
pyrostew 1:a33723b70582 347 {
pyrostew 1:a33723b70582 348 pc.putc('-');
pyrostew 1:a33723b70582 349 tempSpeed *= -1;
pyrostew 1:a33723b70582 350 }
pyrostew 1:a33723b70582 351 if (tempSpeed >= 100.0)
pyrostew 1:a33723b70582 352 {
pyrostew 1:a33723b70582 353 outChar = tempSpeed / 100;
pyrostew 1:a33723b70582 354 pc.putc(outChar + 48);
pyrostew 1:a33723b70582 355 tempSpeed -= outChar * 100.0;
pyrostew 1:a33723b70582 356 }
pyrostew 1:a33723b70582 357 if (tempSpeed >= 10.0)
pyrostew 1:a33723b70582 358 {
pyrostew 1:a33723b70582 359 outChar = tempSpeed / 10;
pyrostew 1:a33723b70582 360 pc.putc(outChar + 48);
pyrostew 1:a33723b70582 361 tempSpeed -= outChar * 10.0;
pyrostew 1:a33723b70582 362 }
pyrostew 1:a33723b70582 363 else if(outChar > 0)
pyrostew 1:a33723b70582 364 {
pyrostew 1:a33723b70582 365 pc.putc('0');
pyrostew 1:a33723b70582 366 }
pyrostew 1:a33723b70582 367 if (tempSpeed >= 1.0)
pyrostew 1:a33723b70582 368 {
pyrostew 1:a33723b70582 369 outChar = tempSpeed;
pyrostew 1:a33723b70582 370 pc.putc(outChar + 48);
pyrostew 1:a33723b70582 371 tempSpeed -= outChar;
pyrostew 1:a33723b70582 372 }
pyrostew 1:a33723b70582 373 else
pyrostew 1:a33723b70582 374 {
pyrostew 1:a33723b70582 375 pc.putc('0');
pyrostew 1:a33723b70582 376 }
pyrostew 1:a33723b70582 377 if (tempSpeed >= 0.1)
pyrostew 1:a33723b70582 378 {
pyrostew 1:a33723b70582 379 pc.putc('.');
pyrostew 1:a33723b70582 380 outChar = tempSpeed * 10;
pyrostew 1:a33723b70582 381 pc.putc(outChar + 48);
pyrostew 1:a33723b70582 382 tempSpeed -= (double)outChar / 10.0;
pyrostew 1:a33723b70582 383 }
pyrostew 1:a33723b70582 384 else
pyrostew 1:a33723b70582 385 {
pyrostew 1:a33723b70582 386 pc.putc('.');
pyrostew 1:a33723b70582 387 pc.putc('0');
pyrostew 1:a33723b70582 388 }
pyrostew 1:a33723b70582 389 if (tempSpeed >= 0.01)
pyrostew 1:a33723b70582 390 {
pyrostew 1:a33723b70582 391 outChar = tempSpeed * 100;
pyrostew 1:a33723b70582 392 pc.putc(outChar + 48);
pyrostew 1:a33723b70582 393 }
pyrostew 1:a33723b70582 394
pyrostew 0:398432a37ca5 395 pc.putc(10);
pyrostew 0:398432a37ca5 396 pc.putc(13);
pyrostew 0:398432a37ca5 397 }
pyrostew 0:398432a37ca5 398
pyrostew 0:398432a37ca5 399 void HallEffectFall()
pyrostew 0:398432a37ca5 400 {
pyrostew 0:398432a37ca5 401 RGHSinInterrupt.disable_irq();
pyrostew 0:398432a37ca5 402 RGHCosInterrupt.disable_irq();
pyrostew 0:398432a37ca5 403
pyrostew 0:398432a37ca5 404 if (direction < 0)
pyrostew 0:398432a37ca5 405 {
pyrostew 0:398432a37ca5 406 SetPower(0.0);
pyrostew 0:398432a37ca5 407
pyrostew 0:398432a37ca5 408 if (Homing)
pyrostew 0:398432a37ca5 409 {
pyrostew 0:398432a37ca5 410 HallTriggered = true;
pyrostew 0:398432a37ca5 411 Homing = false;
pyrostew 0:398432a37ca5 412 position = 0.0;
pyrostew 0:398432a37ca5 413 }
pyrostew 0:398432a37ca5 414 }
pyrostew 0:398432a37ca5 415 RGHSinInterrupt.enable_irq();
pyrostew 0:398432a37ca5 416 RGHCosInterrupt.enable_irq();
pyrostew 0:398432a37ca5 417 }
pyrostew 0:398432a37ca5 418
pyrostew 0:398432a37ca5 419 int main()
pyrostew 0:398432a37ca5 420 {
pyrostew 0:398432a37ca5 421 RGHSinInterrupt.rise(&RGHSinHandler);
pyrostew 0:398432a37ca5 422 RGHCosInterrupt.rise(&RGHCosHandler);
pyrostew 0:398432a37ca5 423 HallSensor.fall(&HallEffectFall);
pyrostew 0:398432a37ca5 424 HallSensor.mode(PullUp);
pyrostew 0:398432a37ca5 425
pyrostew 0:398432a37ca5 426 RunningTime.start();
pyrostew 0:398432a37ca5 427
pyrostew 0:398432a37ca5 428 pc.baud(115200);
pyrostew 0:398432a37ca5 429
pyrostew 0:398432a37ca5 430 Home();
pyrostew 0:398432a37ca5 431
pyrostew 0:398432a37ca5 432 while(1)
pyrostew 0:398432a37ca5 433 {
pyrostew 0:398432a37ca5 434 SerialTransmit();
pyrostew 0:398432a37ca5 435
pyrostew 0:398432a37ca5 436 switch(counter)
pyrostew 0:398432a37ca5 437 {
pyrostew 0:398432a37ca5 438 case 0:
pyrostew 0:398432a37ca5 439 if (position < 200)
pyrostew 0:398432a37ca5 440 {
pyrostew 0:398432a37ca5 441 SetPower(1.0);
pyrostew 0:398432a37ca5 442 }
pyrostew 0:398432a37ca5 443 else
pyrostew 0:398432a37ca5 444 {
pyrostew 0:398432a37ca5 445 counter++;
pyrostew 0:398432a37ca5 446 }
pyrostew 0:398432a37ca5 447 break;
pyrostew 0:398432a37ca5 448 case 1:
pyrostew 0:398432a37ca5 449 if (position > 10)
pyrostew 0:398432a37ca5 450 {
pyrostew 0:398432a37ca5 451 SetPower(-1.0);
pyrostew 0:398432a37ca5 452 }
pyrostew 0:398432a37ca5 453 else
pyrostew 0:398432a37ca5 454 {
pyrostew 0:398432a37ca5 455 counter = 0;
pyrostew 0:398432a37ca5 456 }
pyrostew 0:398432a37ca5 457 break;
pyrostew 0:398432a37ca5 458 }
pyrostew 0:398432a37ca5 459 }
pyrostew 0:398432a37ca5 460 }