Stewart Coulden-Smith / Mbed 2 deprecated EquatorStrutDigitalMonitor

Equator Strut Controller.

Dependencies:   USBDevice mbed

main.cpp@0:398432a37ca5, 2014-08-05 (annotated)

Committer:
pyrostew
Date:
Tue Aug 05 11:23:39 2014 +0000
Revision:
0:398432a37ca5
Child:
1:a33723b70582
Basic Equator Strut controller, monitors strut position, provides homing function and speed control of the strut motor. Outputs information via a Raw Serial connection on P1_27.

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 0:398432a37ca5 25
pyrostew 0:398432a37ca5 26 char counter = 0;
pyrostew 0:398432a37ca5 27
pyrostew 0:398432a37ca5 28 void RGHSinHandler()
pyrostew 0:398432a37ca5 29 {
pyrostew 0:398432a37ca5 30 if (PinState == 2)
pyrostew 0:398432a37ca5 31 {
pyrostew 0:398432a37ca5 32 return;
pyrostew 0:398432a37ca5 33 }
pyrostew 0:398432a37ca5 34 else if (PinState == 1)
pyrostew 0:398432a37ca5 35 {
pyrostew 0:398432a37ca5 36 PinState = 0 |(RGHSinState << 1) | RGHCosState;
pyrostew 0:398432a37ca5 37
pyrostew 0:398432a37ca5 38 if(PinState == 3)
pyrostew 0:398432a37ca5 39 {
pyrostew 0:398432a37ca5 40 direction = 1;
pyrostew 0:398432a37ca5 41 position += 0.04 * direction;
pyrostew 0:398432a37ca5 42 }
pyrostew 0:398432a37ca5 43 }
pyrostew 0:398432a37ca5 44 else
pyrostew 0:398432a37ca5 45 {
pyrostew 0:398432a37ca5 46 PinState = 0 |(RGHSinState << 1) | RGHCosState;
pyrostew 0:398432a37ca5 47 }
pyrostew 0:398432a37ca5 48 }
pyrostew 0:398432a37ca5 49
pyrostew 0:398432a37ca5 50 void RGHCosHandler()
pyrostew 0:398432a37ca5 51 {
pyrostew 0:398432a37ca5 52 if (PinState == 1)
pyrostew 0:398432a37ca5 53 {
pyrostew 0:398432a37ca5 54 return;
pyrostew 0:398432a37ca5 55 }
pyrostew 0:398432a37ca5 56 else if (PinState == 2)
pyrostew 0:398432a37ca5 57 {
pyrostew 0:398432a37ca5 58 PinState = 0 |(RGHSinState << 1) | RGHCosState;
pyrostew 0:398432a37ca5 59
pyrostew 0:398432a37ca5 60 if (PinState == 3)
pyrostew 0:398432a37ca5 61 {
pyrostew 0:398432a37ca5 62 direction = -1;
pyrostew 0:398432a37ca5 63 position += 0.04 * direction;
pyrostew 0:398432a37ca5 64 }
pyrostew 0:398432a37ca5 65 }
pyrostew 0:398432a37ca5 66 else
pyrostew 0:398432a37ca5 67 {
pyrostew 0:398432a37ca5 68 PinState = 0 |(RGHSinState << 1) | RGHCosState;
pyrostew 0:398432a37ca5 69 }
pyrostew 0:398432a37ca5 70 }
pyrostew 0:398432a37ca5 71
pyrostew 0:398432a37ca5 72 void SetPower(double power)
pyrostew 0:398432a37ca5 73 {
pyrostew 0:398432a37ca5 74 currentPower = power;
pyrostew 0:398432a37ca5 75 if(!Enabled)
pyrostew 0:398432a37ca5 76 {
pyrostew 0:398432a37ca5 77 return;
pyrostew 0:398432a37ca5 78 }
pyrostew 0:398432a37ca5 79
pyrostew 0:398432a37ca5 80 if (power > 1.0 || power < -1.0)
pyrostew 0:398432a37ca5 81 {
pyrostew 0:398432a37ca5 82 return;
pyrostew 0:398432a37ca5 83 }
pyrostew 0:398432a37ca5 84
pyrostew 0:398432a37ca5 85 PhaseA = (power + 1.0) / 2;
pyrostew 0:398432a37ca5 86 PhaseB = 1.0 - ((power + 1.0) / 2);
pyrostew 0:398432a37ca5 87 }
pyrostew 0:398432a37ca5 88
pyrostew 0:398432a37ca5 89 void Enable()
pyrostew 0:398432a37ca5 90 {
pyrostew 0:398432a37ca5 91 SetPower(0.0);
pyrostew 0:398432a37ca5 92
pyrostew 0:398432a37ca5 93 ResetLine = 1;
pyrostew 0:398432a37ca5 94
pyrostew 0:398432a37ca5 95 Enabled = true;
pyrostew 0:398432a37ca5 96 }
pyrostew 0:398432a37ca5 97
pyrostew 0:398432a37ca5 98 void Disable()
pyrostew 0:398432a37ca5 99 {
pyrostew 0:398432a37ca5 100 ResetLine = 0;
pyrostew 0:398432a37ca5 101
pyrostew 0:398432a37ca5 102 SetPower(0.0);
pyrostew 0:398432a37ca5 103
pyrostew 0:398432a37ca5 104 Enabled = false;
pyrostew 0:398432a37ca5 105 }
pyrostew 0:398432a37ca5 106
pyrostew 0:398432a37ca5 107 void Home()
pyrostew 0:398432a37ca5 108 {
pyrostew 0:398432a37ca5 109 if (!Enabled)
pyrostew 0:398432a37ca5 110 {
pyrostew 0:398432a37ca5 111 Enable();
pyrostew 0:398432a37ca5 112 }
pyrostew 0:398432a37ca5 113
pyrostew 0:398432a37ca5 114 Homing = true;
pyrostew 0:398432a37ca5 115 HallTriggered = false;
pyrostew 0:398432a37ca5 116
pyrostew 0:398432a37ca5 117 SetPower(-1.0);
pyrostew 0:398432a37ca5 118
pyrostew 0:398432a37ca5 119 while (!HallTriggered)
pyrostew 0:398432a37ca5 120 {
pyrostew 0:398432a37ca5 121 wait(0.5);
pyrostew 0:398432a37ca5 122 }
pyrostew 0:398432a37ca5 123
pyrostew 0:398432a37ca5 124 SetPower(1.0);
pyrostew 0:398432a37ca5 125
pyrostew 0:398432a37ca5 126 while (position < 20.0)
pyrostew 0:398432a37ca5 127 {
pyrostew 0:398432a37ca5 128
pyrostew 0:398432a37ca5 129 }
pyrostew 0:398432a37ca5 130
pyrostew 0:398432a37ca5 131 Homing = true;
pyrostew 0:398432a37ca5 132 HallTriggered = false;
pyrostew 0:398432a37ca5 133
pyrostew 0:398432a37ca5 134 SetPower(-0.5);
pyrostew 0:398432a37ca5 135
pyrostew 0:398432a37ca5 136 while (!HallTriggered)
pyrostew 0:398432a37ca5 137 {
pyrostew 0:398432a37ca5 138 wait(0.5);
pyrostew 0:398432a37ca5 139 }
pyrostew 0:398432a37ca5 140 }
pyrostew 0:398432a37ca5 141
pyrostew 0:398432a37ca5 142 void SerialTransmit()
pyrostew 0:398432a37ca5 143 {
pyrostew 0:398432a37ca5 144 double tempPos = position;
pyrostew 0:398432a37ca5 145 double tempTime = RunningTime.read();
pyrostew 0:398432a37ca5 146 double tempPow = currentPower;
pyrostew 0:398432a37ca5 147 int outChar = 0;
pyrostew 0:398432a37ca5 148
pyrostew 0:398432a37ca5 149 if (tempPos < 0.0)
pyrostew 0:398432a37ca5 150 {
pyrostew 0:398432a37ca5 151 pc.putc('-');
pyrostew 0:398432a37ca5 152 tempPos *= -1;
pyrostew 0:398432a37ca5 153 }
pyrostew 0:398432a37ca5 154 if (tempPos >= 100.0)
pyrostew 0:398432a37ca5 155 {
pyrostew 0:398432a37ca5 156 outChar = tempPos / 100;
pyrostew 0:398432a37ca5 157 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 158 tempPos -= outChar * 100.0;
pyrostew 0:398432a37ca5 159 }
pyrostew 0:398432a37ca5 160 if (tempPos >= 10.0)
pyrostew 0:398432a37ca5 161 {
pyrostew 0:398432a37ca5 162 outChar = tempPos / 10;
pyrostew 0:398432a37ca5 163 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 164 tempPos -= outChar * 10.0;
pyrostew 0:398432a37ca5 165 }
pyrostew 0:398432a37ca5 166 else if(outChar > 0)
pyrostew 0:398432a37ca5 167 {
pyrostew 0:398432a37ca5 168 pc.putc('0');
pyrostew 0:398432a37ca5 169 }
pyrostew 0:398432a37ca5 170 if (tempPos >= 1.0)
pyrostew 0:398432a37ca5 171 {
pyrostew 0:398432a37ca5 172 outChar = tempPos;
pyrostew 0:398432a37ca5 173 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 174 tempPos -= outChar;
pyrostew 0:398432a37ca5 175 }
pyrostew 0:398432a37ca5 176 else
pyrostew 0:398432a37ca5 177 {
pyrostew 0:398432a37ca5 178 pc.putc('0');
pyrostew 0:398432a37ca5 179 }
pyrostew 0:398432a37ca5 180 if (tempPos >= 0.1)
pyrostew 0:398432a37ca5 181 {
pyrostew 0:398432a37ca5 182 pc.putc('.');
pyrostew 0:398432a37ca5 183 outChar = tempPos * 10;
pyrostew 0:398432a37ca5 184 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 185 tempPos -= (double)outChar / 10.0;
pyrostew 0:398432a37ca5 186 }
pyrostew 0:398432a37ca5 187 else
pyrostew 0:398432a37ca5 188 {
pyrostew 0:398432a37ca5 189 pc.putc('.');
pyrostew 0:398432a37ca5 190 pc.putc('0');
pyrostew 0:398432a37ca5 191 }
pyrostew 0:398432a37ca5 192 if (tempPos >= 0.01)
pyrostew 0:398432a37ca5 193 {
pyrostew 0:398432a37ca5 194 outChar = tempPos * 100;
pyrostew 0:398432a37ca5 195 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 196 }
pyrostew 0:398432a37ca5 197
pyrostew 0:398432a37ca5 198 pc.putc(',');
pyrostew 0:398432a37ca5 199
pyrostew 0:398432a37ca5 200 outChar = 0;
pyrostew 0:398432a37ca5 201
pyrostew 0:398432a37ca5 202 if (tempTime >= 100.0)
pyrostew 0:398432a37ca5 203 {
pyrostew 0:398432a37ca5 204 outChar = tempTime / 100;
pyrostew 0:398432a37ca5 205 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 206 tempTime -= outChar * 100.0;
pyrostew 0:398432a37ca5 207 }
pyrostew 0:398432a37ca5 208 if (tempTime >= 10.0)
pyrostew 0:398432a37ca5 209 {
pyrostew 0:398432a37ca5 210 outChar = tempTime / 10;
pyrostew 0:398432a37ca5 211 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 212 tempTime -= outChar * 10.0;
pyrostew 0:398432a37ca5 213 }
pyrostew 0:398432a37ca5 214 else if(outChar > 0)
pyrostew 0:398432a37ca5 215 {
pyrostew 0:398432a37ca5 216 pc.putc('0');
pyrostew 0:398432a37ca5 217 }
pyrostew 0:398432a37ca5 218 if (tempTime >= 1.0)
pyrostew 0:398432a37ca5 219 {
pyrostew 0:398432a37ca5 220 outChar = tempTime;
pyrostew 0:398432a37ca5 221 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 222 tempTime -= outChar;
pyrostew 0:398432a37ca5 223 }
pyrostew 0:398432a37ca5 224 else
pyrostew 0:398432a37ca5 225 {
pyrostew 0:398432a37ca5 226 pc.putc('0');
pyrostew 0:398432a37ca5 227 }
pyrostew 0:398432a37ca5 228 if (tempTime >= 0.1)
pyrostew 0:398432a37ca5 229 {
pyrostew 0:398432a37ca5 230 pc.putc('.');
pyrostew 0:398432a37ca5 231 outChar = tempTime * 10;
pyrostew 0:398432a37ca5 232 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 233 tempTime -= (double)outChar / 10.0;
pyrostew 0:398432a37ca5 234 }
pyrostew 0:398432a37ca5 235 else
pyrostew 0:398432a37ca5 236 {
pyrostew 0:398432a37ca5 237 pc.putc('.');
pyrostew 0:398432a37ca5 238 pc.putc('0');
pyrostew 0:398432a37ca5 239 }
pyrostew 0:398432a37ca5 240 if (tempTime >= 0.01)
pyrostew 0:398432a37ca5 241 {
pyrostew 0:398432a37ca5 242 outChar = tempTime * 100;
pyrostew 0:398432a37ca5 243 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 244 tempTime -= (double)outChar / 100.0;
pyrostew 0:398432a37ca5 245 }
pyrostew 0:398432a37ca5 246 else
pyrostew 0:398432a37ca5 247 {
pyrostew 0:398432a37ca5 248 pc.putc('0');
pyrostew 0:398432a37ca5 249 }
pyrostew 0:398432a37ca5 250 if (tempTime >= 0.001)
pyrostew 0:398432a37ca5 251 {
pyrostew 0:398432a37ca5 252 outChar= tempTime * 1000;
pyrostew 0:398432a37ca5 253 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 254 }
pyrostew 0:398432a37ca5 255
pyrostew 0:398432a37ca5 256 pc.putc(',');
pyrostew 0:398432a37ca5 257 outChar = 0;
pyrostew 0:398432a37ca5 258
pyrostew 0:398432a37ca5 259 if (tempPow < 0.0)
pyrostew 0:398432a37ca5 260 {
pyrostew 0:398432a37ca5 261 pc.putc('-');
pyrostew 0:398432a37ca5 262 tempPow *= -1;
pyrostew 0:398432a37ca5 263 }
pyrostew 0:398432a37ca5 264 if (tempPow >= 1.0)
pyrostew 0:398432a37ca5 265 {
pyrostew 0:398432a37ca5 266 outChar = tempPow;
pyrostew 0:398432a37ca5 267 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 268 tempPow -= outChar;
pyrostew 0:398432a37ca5 269 }
pyrostew 0:398432a37ca5 270 else
pyrostew 0:398432a37ca5 271 {
pyrostew 0:398432a37ca5 272 pc.putc('0');
pyrostew 0:398432a37ca5 273 }
pyrostew 0:398432a37ca5 274 if (tempPow >= 0.1)
pyrostew 0:398432a37ca5 275 {
pyrostew 0:398432a37ca5 276 pc.putc('.');
pyrostew 0:398432a37ca5 277 outChar = tempPow * 10;
pyrostew 0:398432a37ca5 278 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 279 tempPow -= (double)outChar / 10.0;
pyrostew 0:398432a37ca5 280 }
pyrostew 0:398432a37ca5 281 else
pyrostew 0:398432a37ca5 282 {
pyrostew 0:398432a37ca5 283 pc.putc('.');
pyrostew 0:398432a37ca5 284 pc.putc('0');
pyrostew 0:398432a37ca5 285 }
pyrostew 0:398432a37ca5 286 if (tempPow >= 0.01)
pyrostew 0:398432a37ca5 287 {
pyrostew 0:398432a37ca5 288 outChar = tempPow * 100;
pyrostew 0:398432a37ca5 289 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 290 tempPow -= (double)outChar / 100.0;
pyrostew 0:398432a37ca5 291 }
pyrostew 0:398432a37ca5 292 else
pyrostew 0:398432a37ca5 293 {
pyrostew 0:398432a37ca5 294 pc.putc('0');
pyrostew 0:398432a37ca5 295 }
pyrostew 0:398432a37ca5 296 if (tempPow >= 0.001)
pyrostew 0:398432a37ca5 297 {
pyrostew 0:398432a37ca5 298 outChar= tempPow * 1000;
pyrostew 0:398432a37ca5 299 pc.putc(outChar + 48);
pyrostew 0:398432a37ca5 300 }
pyrostew 0:398432a37ca5 301
pyrostew 0:398432a37ca5 302 pc.putc(10);
pyrostew 0:398432a37ca5 303 pc.putc(13);
pyrostew 0:398432a37ca5 304 }
pyrostew 0:398432a37ca5 305
pyrostew 0:398432a37ca5 306 void HallEffectFall()
pyrostew 0:398432a37ca5 307 {
pyrostew 0:398432a37ca5 308 RGHSinInterrupt.disable_irq();
pyrostew 0:398432a37ca5 309 RGHCosInterrupt.disable_irq();
pyrostew 0:398432a37ca5 310
pyrostew 0:398432a37ca5 311 if (direction < 0)
pyrostew 0:398432a37ca5 312 {
pyrostew 0:398432a37ca5 313 SetPower(0.0);
pyrostew 0:398432a37ca5 314
pyrostew 0:398432a37ca5 315 if (Homing)
pyrostew 0:398432a37ca5 316 {
pyrostew 0:398432a37ca5 317 HallTriggered = true;
pyrostew 0:398432a37ca5 318 Homing = false;
pyrostew 0:398432a37ca5 319 position = 0.0;
pyrostew 0:398432a37ca5 320 }
pyrostew 0:398432a37ca5 321 }
pyrostew 0:398432a37ca5 322 RGHSinInterrupt.enable_irq();
pyrostew 0:398432a37ca5 323 RGHCosInterrupt.enable_irq();
pyrostew 0:398432a37ca5 324 }
pyrostew 0:398432a37ca5 325
pyrostew 0:398432a37ca5 326 int main()
pyrostew 0:398432a37ca5 327 {
pyrostew 0:398432a37ca5 328 RGHSinInterrupt.rise(&RGHSinHandler);
pyrostew 0:398432a37ca5 329 RGHCosInterrupt.rise(&RGHCosHandler);
pyrostew 0:398432a37ca5 330 HallSensor.fall(&HallEffectFall);
pyrostew 0:398432a37ca5 331 HallSensor.mode(PullUp);
pyrostew 0:398432a37ca5 332
pyrostew 0:398432a37ca5 333 RunningTime.start();
pyrostew 0:398432a37ca5 334
pyrostew 0:398432a37ca5 335 pc.baud(115200);
pyrostew 0:398432a37ca5 336
pyrostew 0:398432a37ca5 337 Home();
pyrostew 0:398432a37ca5 338
pyrostew 0:398432a37ca5 339 while(1)
pyrostew 0:398432a37ca5 340 {
pyrostew 0:398432a37ca5 341 SerialTransmit();
pyrostew 0:398432a37ca5 342
pyrostew 0:398432a37ca5 343 switch(counter)
pyrostew 0:398432a37ca5 344 {
pyrostew 0:398432a37ca5 345 case 0:
pyrostew 0:398432a37ca5 346 if (position < 200)
pyrostew 0:398432a37ca5 347 {
pyrostew 0:398432a37ca5 348 SetPower(1.0);
pyrostew 0:398432a37ca5 349 }
pyrostew 0:398432a37ca5 350 else
pyrostew 0:398432a37ca5 351 {
pyrostew 0:398432a37ca5 352 counter++;
pyrostew 0:398432a37ca5 353 }
pyrostew 0:398432a37ca5 354 break;
pyrostew 0:398432a37ca5 355 case 1:
pyrostew 0:398432a37ca5 356 if (position > 10)
pyrostew 0:398432a37ca5 357 {
pyrostew 0:398432a37ca5 358 SetPower(-1.0);
pyrostew 0:398432a37ca5 359 }
pyrostew 0:398432a37ca5 360 else
pyrostew 0:398432a37ca5 361 {
pyrostew 0:398432a37ca5 362 counter = 0;
pyrostew 0:398432a37ca5 363 }
pyrostew 0:398432a37ca5 364 break;
pyrostew 0:398432a37ca5 365 }
pyrostew 0:398432a37ca5 366 }
pyrostew 0:398432a37ca5 367 }