Stewart Coulden-Smith / Mbed 2 deprecated EquatorStrutDigitalMonitor

Equator Strut Controller.

Dependencies:   USBDevice mbed

main.cpp@1:a33723b70582, 2014-08-05 (annotated)

Committer:
pyrostew
Date:
Tue Aug 05 13:09:58 2014 +0000
Revision:
1:a33723b70582
Parent:
0:398432a37ca5
Child:
2:088eeae4287c
Added GetSpeed function and now outputting speed on the serial interface.

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
pyrostew 0:398432a37ca5 140 SetPower(-0.5);
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 {
pyrostew 1:a33723b70582 150 if ((RunningTime - lastTime) > 10000)
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
pyrostew 0:398432a37ca5 157 void SerialTransmit()
pyrostew 0:398432a37ca5 158 {
pyrostew 0:398432a37ca5 159 double tempPos = position;
pyrostew 0:398432a37ca5 160 double tempTime = RunningTime.read();
pyrostew 0:398432a37ca5 161 double tempPow = currentPower;
pyrostew 1:a33723b70582 162 double tempSpeed = GetSpeed();
pyrostew 0:398432a37ca5 163 int outChar = 0;
pyrostew 0:398432a37ca5 164
pyrostew 0:398432a37ca5 165 if (tempPos < 0.0)
pyrostew 0:398432a37ca5 166 {
pyrostew 0:398432a37ca5 167 pc.putc('-');
pyrostew 0:398432a37ca5 168 tempPos *= -1;
pyrostew 0:398432a37ca5 169 }
pyrostew 0:398432a37ca5 170 if (tempPos >= 100.0)
pyrostew 0:398432a37ca5 171 {
pyrostew 0:398432a37ca5 172 outChar = tempPos / 100;
pyrostew 0:398432a37ca5 173 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 174 tempPos -= outChar * 100.0;
pyrostew 0:398432a37ca5 175 }
pyrostew 0:398432a37ca5 176 if (tempPos >= 10.0)
pyrostew 0:398432a37ca5 177 {
pyrostew 0:398432a37ca5 178 outChar = tempPos / 10;
pyrostew 0:398432a37ca5 179 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 180 tempPos -= outChar * 10.0;
pyrostew 0:398432a37ca5 181 }
pyrostew 0:398432a37ca5 182 else if(outChar > 0)
pyrostew 0:398432a37ca5 183 {
pyrostew 0:398432a37ca5 184 pc.putc('0');
pyrostew 0:398432a37ca5 185 }
pyrostew 0:398432a37ca5 186 if (tempPos >= 1.0)
pyrostew 0:398432a37ca5 187 {
pyrostew 0:398432a37ca5 188 outChar = tempPos;
pyrostew 0:398432a37ca5 189 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 190 tempPos -= outChar;
pyrostew 0:398432a37ca5 191 }
pyrostew 0:398432a37ca5 192 else
pyrostew 0:398432a37ca5 193 {
pyrostew 0:398432a37ca5 194 pc.putc('0');
pyrostew 0:398432a37ca5 195 }
pyrostew 0:398432a37ca5 196 if (tempPos >= 0.1)
pyrostew 0:398432a37ca5 197 {
pyrostew 0:398432a37ca5 198 pc.putc('.');
pyrostew 0:398432a37ca5 199 outChar = tempPos * 10;
pyrostew 0:398432a37ca5 200 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 201 tempPos -= (double)outChar / 10.0;
pyrostew 0:398432a37ca5 202 }
pyrostew 0:398432a37ca5 203 else
pyrostew 0:398432a37ca5 204 {
pyrostew 0:398432a37ca5 205 pc.putc('.');
pyrostew 0:398432a37ca5 206 pc.putc('0');
pyrostew 0:398432a37ca5 207 }
pyrostew 0:398432a37ca5 208 if (tempPos >= 0.01)
pyrostew 0:398432a37ca5 209 {
pyrostew 0:398432a37ca5 210 outChar = tempPos * 100;
pyrostew 0:398432a37ca5 211 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 212 }
pyrostew 0:398432a37ca5 213
pyrostew 0:398432a37ca5 214 pc.putc(',');
pyrostew 0:398432a37ca5 215
pyrostew 0:398432a37ca5 216 outChar = 0;
pyrostew 0:398432a37ca5 217
pyrostew 0:398432a37ca5 218 if (tempTime >= 100.0)
pyrostew 0:398432a37ca5 219 {
pyrostew 0:398432a37ca5 220 outChar = tempTime / 100;
pyrostew 0:398432a37ca5 221 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 222 tempTime -= outChar * 100.0;
pyrostew 0:398432a37ca5 223 }
pyrostew 0:398432a37ca5 224 if (tempTime >= 10.0)
pyrostew 0:398432a37ca5 225 {
pyrostew 0:398432a37ca5 226 outChar = tempTime / 10;
pyrostew 0:398432a37ca5 227 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 228 tempTime -= outChar * 10.0;
pyrostew 0:398432a37ca5 229 }
pyrostew 0:398432a37ca5 230 else if(outChar > 0)
pyrostew 0:398432a37ca5 231 {
pyrostew 0:398432a37ca5 232 pc.putc('0');
pyrostew 0:398432a37ca5 233 }
pyrostew 0:398432a37ca5 234 if (tempTime >= 1.0)
pyrostew 0:398432a37ca5 235 {
pyrostew 0:398432a37ca5 236 outChar = tempTime;
pyrostew 0:398432a37ca5 237 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 238 tempTime -= outChar;
pyrostew 0:398432a37ca5 239 }
pyrostew 0:398432a37ca5 240 else
pyrostew 0:398432a37ca5 241 {
pyrostew 0:398432a37ca5 242 pc.putc('0');
pyrostew 0:398432a37ca5 243 }
pyrostew 0:398432a37ca5 244 if (tempTime >= 0.1)
pyrostew 0:398432a37ca5 245 {
pyrostew 0:398432a37ca5 246 pc.putc('.');
pyrostew 0:398432a37ca5 247 outChar = tempTime * 10;
pyrostew 0:398432a37ca5 248 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 249 tempTime -= (double)outChar / 10.0;
pyrostew 0:398432a37ca5 250 }
pyrostew 0:398432a37ca5 251 else
pyrostew 0:398432a37ca5 252 {
pyrostew 0:398432a37ca5 253 pc.putc('.');
pyrostew 0:398432a37ca5 254 pc.putc('0');
pyrostew 0:398432a37ca5 255 }
pyrostew 0:398432a37ca5 256 if (tempTime >= 0.01)
pyrostew 0:398432a37ca5 257 {
pyrostew 0:398432a37ca5 258 outChar = tempTime * 100;
pyrostew 0:398432a37ca5 259 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 260 tempTime -= (double)outChar / 100.0;
pyrostew 0:398432a37ca5 261 }
pyrostew 0:398432a37ca5 262 else
pyrostew 0:398432a37ca5 263 {
pyrostew 0:398432a37ca5 264 pc.putc('0');
pyrostew 0:398432a37ca5 265 }
pyrostew 0:398432a37ca5 266 if (tempTime >= 0.001)
pyrostew 0:398432a37ca5 267 {
pyrostew 0:398432a37ca5 268 outChar= tempTime * 1000;
pyrostew 0:398432a37ca5 269 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 270 }
pyrostew 0:398432a37ca5 271
pyrostew 0:398432a37ca5 272 pc.putc(',');
pyrostew 0:398432a37ca5 273 outChar = 0;
pyrostew 0:398432a37ca5 274
pyrostew 0:398432a37ca5 275 if (tempPow < 0.0)
pyrostew 0:398432a37ca5 276 {
pyrostew 0:398432a37ca5 277 pc.putc('-');
pyrostew 0:398432a37ca5 278 tempPow *= -1;
pyrostew 0:398432a37ca5 279 }
pyrostew 0:398432a37ca5 280 if (tempPow >= 1.0)
pyrostew 0:398432a37ca5 281 {
pyrostew 0:398432a37ca5 282 outChar = tempPow;
pyrostew 0:398432a37ca5 283 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 284 tempPow -= outChar;
pyrostew 0:398432a37ca5 285 }
pyrostew 0:398432a37ca5 286 else
pyrostew 0:398432a37ca5 287 {
pyrostew 0:398432a37ca5 288 pc.putc('0');
pyrostew 0:398432a37ca5 289 }
pyrostew 0:398432a37ca5 290 if (tempPow >= 0.1)
pyrostew 0:398432a37ca5 291 {
pyrostew 0:398432a37ca5 292 pc.putc('.');
pyrostew 0:398432a37ca5 293 outChar = tempPow * 10;
pyrostew 0:398432a37ca5 294 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 295 tempPow -= (double)outChar / 10.0;
pyrostew 0:398432a37ca5 296 }
pyrostew 0:398432a37ca5 297 else
pyrostew 0:398432a37ca5 298 {
pyrostew 0:398432a37ca5 299 pc.putc('.');
pyrostew 0:398432a37ca5 300 pc.putc('0');
pyrostew 0:398432a37ca5 301 }
pyrostew 0:398432a37ca5 302 if (tempPow >= 0.01)
pyrostew 0:398432a37ca5 303 {
pyrostew 0:398432a37ca5 304 outChar = tempPow * 100;
pyrostew 0:398432a37ca5 305 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 306 tempPow -= (double)outChar / 100.0;
pyrostew 0:398432a37ca5 307 }
pyrostew 0:398432a37ca5 308 else
pyrostew 0:398432a37ca5 309 {
pyrostew 0:398432a37ca5 310 pc.putc('0');
pyrostew 0:398432a37ca5 311 }
pyrostew 0:398432a37ca5 312 if (tempPow >= 0.001)
pyrostew 0:398432a37ca5 313 {
pyrostew 0:398432a37ca5 314 outChar= tempPow * 1000;
pyrostew 0:398432a37ca5 315 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 316 }
pyrostew 0:398432a37ca5 317
pyrostew 1:a33723b70582 318 pc.putc(',');
pyrostew 1:a33723b70582 319 outChar = 0;
pyrostew 1:a33723b70582 320
pyrostew 1:a33723b70582 321 if (tempSpeed < 0.0)
pyrostew 1:a33723b70582 322 {
pyrostew 1:a33723b70582 323 pc.putc('-');
pyrostew 1:a33723b70582 324 tempSpeed *= -1;
pyrostew 1:a33723b70582 325 }
pyrostew 1:a33723b70582 326 if (tempSpeed >= 100.0)
pyrostew 1:a33723b70582 327 {
pyrostew 1:a33723b70582 328 outChar = tempSpeed / 100;
pyrostew 1:a33723b70582 329 pc.putc(outChar + 48);
pyrostew 1:a33723b70582 330 tempSpeed -= outChar * 100.0;
pyrostew 1:a33723b70582 331 }
pyrostew 1:a33723b70582 332 if (tempSpeed >= 10.0)
pyrostew 1:a33723b70582 333 {
pyrostew 1:a33723b70582 334 outChar = tempSpeed / 10;
pyrostew 1:a33723b70582 335 pc.putc(outChar + 48);
pyrostew 1:a33723b70582 336 tempSpeed -= outChar * 10.0;
pyrostew 1:a33723b70582 337 }
pyrostew 1:a33723b70582 338 else if(outChar > 0)
pyrostew 1:a33723b70582 339 {
pyrostew 1:a33723b70582 340 pc.putc('0');
pyrostew 1:a33723b70582 341 }
pyrostew 1:a33723b70582 342 if (tempSpeed >= 1.0)
pyrostew 1:a33723b70582 343 {
pyrostew 1:a33723b70582 344 outChar = tempSpeed;
pyrostew 1:a33723b70582 345 pc.putc(outChar + 48);
pyrostew 1:a33723b70582 346 tempSpeed -= outChar;
pyrostew 1:a33723b70582 347 }
pyrostew 1:a33723b70582 348 else
pyrostew 1:a33723b70582 349 {
pyrostew 1:a33723b70582 350 pc.putc('0');
pyrostew 1:a33723b70582 351 }
pyrostew 1:a33723b70582 352 if (tempSpeed >= 0.1)
pyrostew 1:a33723b70582 353 {
pyrostew 1:a33723b70582 354 pc.putc('.');
pyrostew 1:a33723b70582 355 outChar = tempSpeed * 10;
pyrostew 1:a33723b70582 356 pc.putc(outChar + 48);
pyrostew 1:a33723b70582 357 tempSpeed -= (double)outChar / 10.0;
pyrostew 1:a33723b70582 358 }
pyrostew 1:a33723b70582 359 else
pyrostew 1:a33723b70582 360 {
pyrostew 1:a33723b70582 361 pc.putc('.');
pyrostew 1:a33723b70582 362 pc.putc('0');
pyrostew 1:a33723b70582 363 }
pyrostew 1:a33723b70582 364 if (tempSpeed >= 0.01)
pyrostew 1:a33723b70582 365 {
pyrostew 1:a33723b70582 366 outChar = tempSpeed * 100;
pyrostew 1:a33723b70582 367 pc.putc(outChar + 48);
pyrostew 1:a33723b70582 368 }
pyrostew 1:a33723b70582 369
pyrostew 0:398432a37ca5 370 pc.putc(10);
pyrostew 0:398432a37ca5 371 pc.putc(13);
pyrostew 0:398432a37ca5 372 }
pyrostew 0:398432a37ca5 373
pyrostew 0:398432a37ca5 374 void HallEffectFall()
pyrostew 0:398432a37ca5 375 {
pyrostew 0:398432a37ca5 376 RGHSinInterrupt.disable_irq();
pyrostew 0:398432a37ca5 377 RGHCosInterrupt.disable_irq();
pyrostew 0:398432a37ca5 378
pyrostew 0:398432a37ca5 379 if (direction < 0)
pyrostew 0:398432a37ca5 380 {
pyrostew 0:398432a37ca5 381 SetPower(0.0);
pyrostew 0:398432a37ca5 382
pyrostew 0:398432a37ca5 383 if (Homing)
pyrostew 0:398432a37ca5 384 {
pyrostew 0:398432a37ca5 385 HallTriggered = true;
pyrostew 0:398432a37ca5 386 Homing = false;
pyrostew 0:398432a37ca5 387 position = 0.0;
pyrostew 0:398432a37ca5 388 }
pyrostew 0:398432a37ca5 389 }
pyrostew 0:398432a37ca5 390 RGHSinInterrupt.enable_irq();
pyrostew 0:398432a37ca5 391 RGHCosInterrupt.enable_irq();
pyrostew 0:398432a37ca5 392 }
pyrostew 0:398432a37ca5 393
pyrostew 0:398432a37ca5 394 int main()
pyrostew 0:398432a37ca5 395 {
pyrostew 0:398432a37ca5 396 RGHSinInterrupt.rise(&RGHSinHandler);
pyrostew 0:398432a37ca5 397 RGHCosInterrupt.rise(&RGHCosHandler);
pyrostew 0:398432a37ca5 398 HallSensor.fall(&HallEffectFall);
pyrostew 0:398432a37ca5 399 HallSensor.mode(PullUp);
pyrostew 0:398432a37ca5 400
pyrostew 0:398432a37ca5 401 RunningTime.start();
pyrostew 0:398432a37ca5 402
pyrostew 0:398432a37ca5 403 pc.baud(115200);
pyrostew 0:398432a37ca5 404
pyrostew 0:398432a37ca5 405 Home();
pyrostew 0:398432a37ca5 406
pyrostew 0:398432a37ca5 407 while(1)
pyrostew 0:398432a37ca5 408 {
pyrostew 0:398432a37ca5 409 SerialTransmit();
pyrostew 0:398432a37ca5 410
pyrostew 0:398432a37ca5 411 switch(counter)
pyrostew 0:398432a37ca5 412 {
pyrostew 0:398432a37ca5 413 case 0:
pyrostew 0:398432a37ca5 414 if (position < 200)
pyrostew 0:398432a37ca5 415 {
pyrostew 0:398432a37ca5 416 SetPower(1.0);
pyrostew 0:398432a37ca5 417 }
pyrostew 0:398432a37ca5 418 else
pyrostew 0:398432a37ca5 419 {
pyrostew 0:398432a37ca5 420 counter++;
pyrostew 0:398432a37ca5 421 }
pyrostew 0:398432a37ca5 422 break;
pyrostew 0:398432a37ca5 423 case 1:
pyrostew 0:398432a37ca5 424 if (position > 10)
pyrostew 0:398432a37ca5 425 {
pyrostew 0:398432a37ca5 426 SetPower(-1.0);
pyrostew 0:398432a37ca5 427 }
pyrostew 0:398432a37ca5 428 else
pyrostew 0:398432a37ca5 429 {
pyrostew 0:398432a37ca5 430 counter = 0;
pyrostew 0:398432a37ca5 431 }
pyrostew 0:398432a37ca5 432 break;
pyrostew 0:398432a37ca5 433 }
pyrostew 0:398432a37ca5 434 }
pyrostew 0:398432a37ca5 435 }