Alpesh Bhudia / Mbed 2 deprecated EquatorStrutDigitalMonitor

Merged to branch

Dependencies:   USBDevice mbed EquatorStrutController LightWeightSerialTransmit

Fork of EquatorStrutDigitalMonitor by Stewart Coulden-Smith

main.cpp@4:2ec05810bc47, 2014-08-07 (annotated)

Committer:
alpesh
Date:
Thu Aug 07 08:38:18 2014 +0000
Revision:
4:2ec05810bc47
Parent:
3:e693c65b04de
Child:
5:4468f8ff5ec6
dsd

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
alpesh 2:d1805e7d46fb 214 pc.putc('\t');
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
alpesh 2:d1805e7d46fb 272 pc.putc('\t');
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
alpesh 2:d1805e7d46fb 318 pc.putc('\t');
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
alpesh 3:e693c65b04de 394 double TargetSpeed=50.0; //Millimeter per second
alpesh 2:d1805e7d46fb 395 double Error =0;
alpesh 2:d1805e7d46fb 396 double KpTerm;
alpesh 3:e693c65b04de 397 double ErrorInt;
alpesh 2:d1805e7d46fb 398 double ErrorDer;
alpesh 2:d1805e7d46fb 399
alpesh 4:2ec05810bc47 400 double KpGain = 0.002;
alpesh 2:d1805e7d46fb 401 double KiGain = 0.0;
alpesh 2:d1805e7d46fb 402 double KdGain = 0.0;
alpesh 2:d1805e7d46fb 403
alpesh 3:e693c65b04de 404 int errorcounter;
alpesh 2:d1805e7d46fb 405 double PreviousError [10];
alpesh 2:d1805e7d46fb 406
alpesh 3:e693c65b04de 407 double PwmChange=0;
alpesh 3:e693c65b04de 408
alpesh 3:e693c65b04de 409 double pwm;
alpesh 3:e693c65b04de 410 double NewPwm;
alpesh 3:e693c65b04de 411
alpesh 3:e693c65b04de 412 int previousTime = 0;
alpesh 3:e693c65b04de 413
alpesh 4:2ec05810bc47 414 void Controller ()
alpesh 4:2ec05810bc47 415 {
alpesh 4:2ec05810bc47 416 if (GetSpeed() <= TargetSpeed)
alpesh 4:2ec05810bc47 417 {
alpesh 4:2ec05810bc47 418 int timeStep = RunningTime.read_us() - previousTime;
alpesh 4:2ec05810bc47 419 previousTime = RunningTime.read_us();
alpesh 4:2ec05810bc47 420
alpesh 4:2ec05810bc47 421 Error = TargetSpeed - GetSpeed();
alpesh 4:2ec05810bc47 422
alpesh 4:2ec05810bc47 423 KpTerm = Error * KpGain;
alpesh 4:2ec05810bc47 424
alpesh 4:2ec05810bc47 425 //ErrorDer = (Error - PreviousError[errorcounter]) / timeStep;
alpesh 4:2ec05810bc47 426
alpesh 4:2ec05810bc47 427 ErrorInt = ErrorInt + Error * timeStep;
alpesh 4:2ec05810bc47 428
alpesh 4:2ec05810bc47 429 PwmChange = (KpTerm + KiGain * ErrorInt + KdGain * ErrorDer);
alpesh 4:2ec05810bc47 430
alpesh 4:2ec05810bc47 431 NewPwm = pwm + PwmChange;
alpesh 4:2ec05810bc47 432
alpesh 4:2ec05810bc47 433 if (NewPwm > 1.0)
alpesh 4:2ec05810bc47 434 {
alpesh 4:2ec05810bc47 435 pwm = 1.0;
alpesh 4:2ec05810bc47 436 }
alpesh 4:2ec05810bc47 437
alpesh 4:2ec05810bc47 438 else if (NewPwm < 0.0)
alpesh 4:2ec05810bc47 439 {
alpesh 4:2ec05810bc47 440 pwm = 0.0;
alpesh 4:2ec05810bc47 441 }
alpesh 4:2ec05810bc47 442
alpesh 4:2ec05810bc47 443 else
alpesh 4:2ec05810bc47 444 {
alpesh 4:2ec05810bc47 445 pwm = NewPwm;
alpesh 4:2ec05810bc47 446 }
alpesh 4:2ec05810bc47 447
alpesh 4:2ec05810bc47 448 SetPower(pwm);
alpesh 4:2ec05810bc47 449
alpesh 4:2ec05810bc47 450 errorcounter ++;
alpesh 4:2ec05810bc47 451
alpesh 4:2ec05810bc47 452 if (counter > 9)
alpesh 4:2ec05810bc47 453 {
alpesh 4:2ec05810bc47 454 counter = 0;
alpesh 4:2ec05810bc47 455 }
alpesh 4:2ec05810bc47 456
alpesh 4:2ec05810bc47 457 PreviousError[errorcounter] = Error;
alpesh 4:2ec05810bc47 458 }
alpesh 4:2ec05810bc47 459 }
alpesh 4:2ec05810bc47 460
alpesh 4:2ec05810bc47 461
pyrostew 0:398432a37ca5 462 int main()
alpesh 2:d1805e7d46fb 463
pyrostew 0:398432a37ca5 464 {
pyrostew 0:398432a37ca5 465 RGHSinInterrupt.rise(&RGHSinHandler);
pyrostew 0:398432a37ca5 466 RGHCosInterrupt.rise(&RGHCosHandler);
pyrostew 0:398432a37ca5 467 HallSensor.fall(&HallEffectFall);
pyrostew 0:398432a37ca5 468 HallSensor.mode(PullUp);
pyrostew 0:398432a37ca5 469
pyrostew 0:398432a37ca5 470 RunningTime.start();
pyrostew 0:398432a37ca5 471
pyrostew 0:398432a37ca5 472 pc.baud(115200);
pyrostew 0:398432a37ca5 473
alpesh 4:2ec05810bc47 474 Home();
alpesh 4:2ec05810bc47 475 //Enable();
pyrostew 0:398432a37ca5 476
alpesh 3:e693c65b04de 477 errorcounter = 0;
alpesh 3:e693c65b04de 478 PreviousError[errorcounter]=0;
alpesh 2:d1805e7d46fb 479
alpesh 3:e693c65b04de 480 previousTime = RunningTime.read_us();
alpesh 3:e693c65b04de 481
alpesh 3:e693c65b04de 482 while(1)
alpesh 2:d1805e7d46fb 483 {
alpesh 4:2ec05810bc47 484 while(position < 200)
alpesh 3:e693c65b04de 485 {
alpesh 4:2ec05810bc47 486 Controller();
alpesh 4:2ec05810bc47 487 }
alpesh 2:d1805e7d46fb 488
alpesh 4:2ec05810bc47 489 TargetSpeed = -50.0
alpesh 3:e693c65b04de 490
alpesh 4:2ec05810bc47 491 while(position > 10)
alpesh 4:2ec05810bc47 492 {
alpesh 4:2ec05810bc47 493 Controller();
pyrostew 0:398432a37ca5 494 }
alpesh 3:e693c65b04de 495
alpesh 3:e693c65b04de 496 SerialTransmit();
pyrostew 0:398432a37ca5 497 }
pyrostew 0:398432a37ca5 498 }