Alpesh Bhudia / Mbed 2 deprecated EquatorStrutDigitalMonitor

Merged to branch

Dependencies:   USBDevice mbed EquatorStrutController LightWeightSerialTransmit

Fork of EquatorStrutDigitalMonitor by Stewart Coulden-Smith

main.cpp@6:bfe745b152fa, 2014-08-07 (annotated)

Committer:
alpesh
Date:
Thu Aug 07 09:19:16 2014 +0000
Revision:
6:bfe745b152fa
Parent:
5:4468f8ff5ec6
Child:
7:4cd7be306626
ErrorOutput

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 {
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 3:e693c65b04de 449 double TargetSpeed=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 6:bfe745b152fa 455 double KpGain = 0.000009;
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 4:2ec05810bc47 471 if (GetSpeed() <= TargetSpeed)
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 4:2ec05810bc47 476 Error = TargetSpeed - GetSpeed();
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 4:2ec05810bc47 484 PwmChange = (KpTerm + KiGain * ErrorInt + KdGain * ErrorDer);
alpesh 4:2ec05810bc47 485
alpesh 4:2ec05810bc47 486 NewPwm = pwm + PwmChange;
alpesh 4:2ec05810bc47 487
alpesh 4:2ec05810bc47 488 if (NewPwm > 1.0)
alpesh 4:2ec05810bc47 489 {
alpesh 4:2ec05810bc47 490 pwm = 1.0;
alpesh 4:2ec05810bc47 491 }
alpesh 4:2ec05810bc47 492
alpesh 5:4468f8ff5ec6 493 else if (NewPwm < -1.0)
alpesh 4:2ec05810bc47 494 {
alpesh 5:4468f8ff5ec6 495 pwm = -1.0;
alpesh 4:2ec05810bc47 496 }
alpesh 4:2ec05810bc47 497
alpesh 4:2ec05810bc47 498 else
alpesh 4:2ec05810bc47 499 {
alpesh 4:2ec05810bc47 500 pwm = NewPwm;
alpesh 4:2ec05810bc47 501 }
alpesh 4:2ec05810bc47 502
alpesh 4:2ec05810bc47 503 SetPower(pwm);
alpesh 4:2ec05810bc47 504
alpesh 4:2ec05810bc47 505 errorcounter ++;
alpesh 4:2ec05810bc47 506
alpesh 6:bfe745b152fa 507 if (errorcounter > 9)
alpesh 4:2ec05810bc47 508 {
alpesh 6:bfe745b152fa 509 errorcounter = 0;
alpesh 4:2ec05810bc47 510 }
alpesh 4:2ec05810bc47 511
alpesh 4:2ec05810bc47 512 PreviousError[errorcounter] = Error;
alpesh 4:2ec05810bc47 513 }
alpesh 4:2ec05810bc47 514 }
alpesh 4:2ec05810bc47 515
alpesh 4:2ec05810bc47 516
pyrostew 0:398432a37ca5 517 int main()
alpesh 2:d1805e7d46fb 518
pyrostew 0:398432a37ca5 519 {
pyrostew 0:398432a37ca5 520 RGHSinInterrupt.rise(&RGHSinHandler);
pyrostew 0:398432a37ca5 521 RGHCosInterrupt.rise(&RGHCosHandler);
pyrostew 0:398432a37ca5 522 HallSensor.fall(&HallEffectFall);
pyrostew 0:398432a37ca5 523 HallSensor.mode(PullUp);
pyrostew 0:398432a37ca5 524
pyrostew 0:398432a37ca5 525 RunningTime.start();
pyrostew 0:398432a37ca5 526
pyrostew 0:398432a37ca5 527 pc.baud(115200);
pyrostew 0:398432a37ca5 528
alpesh 4:2ec05810bc47 529 Home();
alpesh 4:2ec05810bc47 530 //Enable();
pyrostew 0:398432a37ca5 531
alpesh 3:e693c65b04de 532 errorcounter = 0;
alpesh 3:e693c65b04de 533 PreviousError[errorcounter]=0;
alpesh 2:d1805e7d46fb 534
alpesh 3:e693c65b04de 535 previousTime = RunningTime.read_us();
alpesh 3:e693c65b04de 536
alpesh 6:bfe745b152fa 537 while(Enabled)
alpesh 2:d1805e7d46fb 538 {
alpesh 4:2ec05810bc47 539 while(position < 200)
alpesh 3:e693c65b04de 540 {
alpesh 5:4468f8ff5ec6 541 SerialTransmit();
alpesh 5:4468f8ff5ec6 542
alpesh 4:2ec05810bc47 543 Controller();
alpesh 4:2ec05810bc47 544 }
alpesh 2:d1805e7d46fb 545
alpesh 6:bfe745b152fa 546 Disable();
pyrostew 0:398432a37ca5 547 }
pyrostew 0:398432a37ca5 548 }