Artem Solomatin / Mbed 2 deprecated pendudu

fsdfds

Dependencies:   mbed

main.cpp@10:ca38e4d775c8, 2020-05-22 (annotated)

Committer:
teamat
Date:
Fri May 22 12:01:26 2020 +0000
Revision:
10:ca38e4d775c8
Parent:
9:e8c6a414e226
Child:
11:6bbe8da4b16a
for sharing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
teamat 3:8708e61475fe 1 #include "mbed.h"
teamat 3:8708e61475fe 2
teamat 3:8708e61475fe 3 #define M_PI 3.14159265358979323846f
teamat 3:8708e61475fe 4 #define smoothingNumber 6
teamat 3:8708e61475fe 5
teamat 3:8708e61475fe 6 #define STATE_ERROR 0
teamat 3:8708e61475fe 7 #define STATE_INIT 1
teamat 3:8708e61475fe 8 #define STATE_GOTO_START 2
teamat 3:8708e61475fe 9 #define STATE_GOTO_END_COUNTING 3
teamat 3:8708e61475fe 10 #define STATE_GOTO_MIDDLE 4
teamat 3:8708e61475fe 11 #define STATE_WAITING 5
teamat 3:8708e61475fe 12 #define STATE_GOTO_SWING 6
teamat 3:8708e61475fe 13 #define STATE_START_SWING 7
teamat 3:8708e61475fe 14 #define STATE_SWING_RIGHT 8
teamat 3:8708e61475fe 15 #define STATE_SWING_LEFT 9
teamat 3:8708e61475fe 16 #define STATE_EMPTY_SWING 10
teamat 3:8708e61475fe 17
teamat 3:8708e61475fe 18 #define DIR_LEFT 0
teamat 3:8708e61475fe 19 #define DIR_RIGHT 1
teamat 10:ca38e4d775c8 20 //попробовать поменять дир_лефт на -1
teamat 10:ca38e4d775c8 21
teamat 3:8708e61475fe 22
teamat 3:8708e61475fe 23 DigitalOut RCout(D10);
teamat 3:8708e61475fe 24 DigitalOut dir(D9);
teamat 3:8708e61475fe 25 InterruptIn leftSwitch(D2);
teamat 3:8708e61475fe 26 InterruptIn rightSwitch(D3);
teamat 3:8708e61475fe 27 Ticker tick;
teamat 4:fd49edfabfb2 28 Ticker speedTicker;
teamat 4:fd49edfabfb2 29 Ticker swingTicker;
teamat 6:999e8ae7d969 30 Ticker sendDataTicker;
teamat 3:8708e61475fe 31 DigitalOut myled(LED2);
teamat 3:8708e61475fe 32 RawSerial rpc(D1,D0,9600);
teamat 3:8708e61475fe 33
teamat 3:8708e61475fe 34 int period_us = 26;//300 26
teamat 7:ca62dda005d5 35 int posCounter = 0;
teamat 3:8708e61475fe 36
teamat 3:8708e61475fe 37 long pos=0;
teamat 3:8708e61475fe 38
teamat 3:8708e61475fe 39 long railLength = 0;
teamat 3:8708e61475fe 40
teamat 3:8708e61475fe 41 uint8_t state=STATE_INIT;
teamat 3:8708e61475fe 42
teamat 3:8708e61475fe 43 Timer t;
teamat 3:8708e61475fe 44 SPI spi(D11,D12,D13);// mosi, miso, sclk
teamat 3:8708e61475fe 45 DigitalOut cs(D5);
teamat 3:8708e61475fe 46
teamat 4:fd49edfabfb2 47 float radius = 0.0025;
teamat 3:8708e61475fe 48
teamat 3:8708e61475fe 49 float angularPosNew = 0;
teamat 3:8708e61475fe 50 float angularPosOld = 0;
teamat 3:8708e61475fe 51
teamat 3:8708e61475fe 52 float timeOld = 0.0f;
teamat 3:8708e61475fe 53 float timeStart = 0.0f;
teamat 3:8708e61475fe 54
teamat 3:8708e61475fe 55 float dx = 0;
teamat 3:8708e61475fe 56 float xPosNew = 0;
teamat 3:8708e61475fe 57 float xPosOld = 0;
teamat 8:b3ce040fdebc 58 float speed = -1;
teamat 8:b3ce040fdebc 59 float posOffset = 0.0f;
teamat 7:ca62dda005d5 60 float posMap[4] = {0.0f, 0.0f, 0.0f, 0.0f};
teamat 3:8708e61475fe 61
teamat 3:8708e61475fe 62 double dAngle = 0.0f;
teamat 3:8708e61475fe 63 double anSpd = 0.0f;
teamat 3:8708e61475fe 64
teamat 3:8708e61475fe 65 float timeOldPos = 0.0f;
teamat 3:8708e61475fe 66 float timeStartPos = 0.0f;
teamat 3:8708e61475fe 67
teamat 4:fd49edfabfb2 68 float angle=0.0f;
teamat 3:8708e61475fe 69 float angleOffset = 0;
teamat 3:8708e61475fe 70
teamat 4:fd49edfabfb2 71 float control = 0.0f;
teamat 4:fd49edfabfb2 72
teamat 3:8708e61475fe 73 double PIPI = 6.28;
teamat 3:8708e61475fe 74
teamat 3:8708e61475fe 75 bool canSend = false;
teamat 8:b3ce040fdebc 76 bool periodUpdated = false;
teamat 8:b3ce040fdebc 77 bool calibrated = false;
teamat 9:e8c6a414e226 78 bool dirUpdated = false;
teamat 3:8708e61475fe 79
teamat 3:8708e61475fe 80 typedef union {
teamat 4:fd49edfabfb2 81 float number[6];
teamat 4:fd49edfabfb2 82 uint8_t numberCh[24];
teamat 3:8708e61475fe 83 } my_union;
teamat 3:8708e61475fe 84
teamat 3:8708e61475fe 85 my_union myUnion;
teamat 3:8708e61475fe 86
teamat 3:8708e61475fe 87 bool isPendulumSwinging() {
teamat 3:8708e61475fe 88 return state == STATE_SWING_RIGHT || state == STATE_SWING_LEFT;
teamat 3:8708e61475fe 89 }
teamat 3:8708e61475fe 90
teamat 3:8708e61475fe 91 float getPosMM() {
teamat 3:8708e61475fe 92 //return (pos-railLength/2) * 550.0f/railLength;
teamat 3:8708e61475fe 93 //return (pos - railLength / 2) * (350.0f / railLength);
teamat 3:8708e61475fe 94 return pos / 3500.0f;
teamat 3:8708e61475fe 95 }
teamat 3:8708e61475fe 96
teamat 3:8708e61475fe 97 uint16_t getPendulumPos(){
teamat 3:8708e61475fe 98 cs=0;
teamat 3:8708e61475fe 99 wait_ms(1);
teamat 3:8708e61475fe 100 uint16_t d=spi.write((short)0x00);
teamat 3:8708e61475fe 101 d=d<<1;//fucking shithole fakebit
teamat 3:8708e61475fe 102 d=d>>6;//no need debug info
teamat 3:8708e61475fe 103 cs=1;
teamat 3:8708e61475fe 104 wait_ms(1);
teamat 3:8708e61475fe 105 return (uint16_t)d;
teamat 3:8708e61475fe 106 }
teamat 3:8708e61475fe 107
teamat 3:8708e61475fe 108
teamat 3:8708e61475fe 109 float getPendulumAngle(){
teamat 3:8708e61475fe 110 angle = getPendulumPos();
teamat 3:8708e61475fe 111 angle = angle * 6.28f / 1024.0f;
teamat 4:fd49edfabfb2 112 angle += angleOffset;
teamat 8:b3ce040fdebc 113 if (angle > 3.14) {
teamat 8:b3ce040fdebc 114 angle = angle - PIPI;
teamat 8:b3ce040fdebc 115 }
teamat 8:b3ce040fdebc 116 /*if (angle > PIPI + 0.01) {
teamat 4:fd49edfabfb2 117 angle = fmod((angle + 3.14), PIPI) - 3.14;
teamat 4:fd49edfabfb2 118 if (angle < -3.14) {
teamat 4:fd49edfabfb2 119 angle += PIPI;
teamat 4:fd49edfabfb2 120 }
teamat 9:e8c6a414e226 121 }*/
teamat 8:b3ce040fdebc 122 /*if (calibrated) {
teamat 8:b3ce040fdebc 123 float test = fmodf(angle, M_PI);
teamat 8:b3ce040fdebc 124 if (test >= 1) {
teamat 8:b3ce040fdebc 125 angle = -test;
teamat 8:b3ce040fdebc 126 } else {
teamat 8:b3ce040fdebc 127 angle = test;
teamat 8:b3ce040fdebc 128 }
teamat 8:b3ce040fdebc 129 }*/
teamat 3:8708e61475fe 130 return angle;
teamat 3:8708e61475fe 131 }
teamat 3:8708e61475fe 132
teamat 3:8708e61475fe 133
teamat 3:8708e61475fe 134 /*
teamat 3:8708e61475fe 135 ANGULAR SPEED CALC
teamat 3:8708e61475fe 136 */
teamat 3:8708e61475fe 137
teamat 3:8708e61475fe 138 void getDeltaAng(){
teamat 3:8708e61475fe 139
teamat 3:8708e61475fe 140 angularPosNew = getPendulumAngle();
teamat 3:8708e61475fe 141
teamat 3:8708e61475fe 142 dAngle = fmod((angularPosNew - angularPosOld + 3.14), PIPI) - 3.14;
teamat 3:8708e61475fe 143 if (dAngle < -3.14) {
teamat 3:8708e61475fe 144 dAngle += PIPI;
teamat 3:8708e61475fe 145 }
teamat 3:8708e61475fe 146
teamat 3:8708e61475fe 147 angularPosOld = angularPosNew;
teamat 3:8708e61475fe 148
teamat 3:8708e61475fe 149 }
teamat 3:8708e61475fe 150
teamat 3:8708e61475fe 151 void getAngularSpeed(){
teamat 3:8708e61475fe 152 float deltaTime;
teamat 3:8708e61475fe 153
teamat 3:8708e61475fe 154 timeStart = float(t.read());
teamat 3:8708e61475fe 155 deltaTime = (timeStart - timeOld);
teamat 3:8708e61475fe 156 getDeltaAng();
teamat 3:8708e61475fe 157 anSpd = dAngle / deltaTime;
teamat 3:8708e61475fe 158 timeOld=timeStart;
teamat 3:8708e61475fe 159 //взятие по модулю, спросить
teamat 3:8708e61475fe 160 }
teamat 3:8708e61475fe 161
teamat 3:8708e61475fe 162 /*
teamat 3:8708e61475fe 163 SPEED CALC
teamat 3:8708e61475fe 164 */
teamat 3:8708e61475fe 165
teamat 4:fd49edfabfb2 166 void calcSpeed() {
teamat 7:ca62dda005d5 167 /*posMap[posCounter] = getPosMM() / 1000;
teamat 7:ca62dda005d5 168 if (posCounter == 3) {
teamat 7:ca62dda005d5 169 posCounter = 0;
teamat 7:ca62dda005d5 170 float pathSum = 0;
teamat 7:ca62dda005d5 171 for (int i = 0; i < 3; i++) {
teamat 7:ca62dda005d5 172 float pathDiff = posMap[i + 1] - posMap[i];
teamat 7:ca62dda005d5 173 if ((pathDiff) < 0) {
teamat 7:ca62dda005d5 174 pathSum += -pathDiff;
teamat 7:ca62dda005d5 175 } else {
teamat 7:ca62dda005d5 176 pathSum += pathDiff;
teamat 7:ca62dda005d5 177 }
teamat 7:ca62dda005d5 178 }
teamat 7:ca62dda005d5 179 speed = pathSum;
teamat 7:ca62dda005d5 180 } else {
teamat 7:ca62dda005d5 181 posCounter += 1;
teamat 7:ca62dda005d5 182 }*/
teamat 8:b3ce040fdebc 183 xPosNew = (getPosMM() - posOffset) / 1000;
teamat 4:fd49edfabfb2 184 speed = xPosNew - xPosOld;
teamat 8:b3ce040fdebc 185 if (dir == DIR_LEFT) {
teamat 8:b3ce040fdebc 186 speed = -speed;
teamat 8:b3ce040fdebc 187 }
teamat 9:e8c6a414e226 188 //speed = -speed;
teamat 8:b3ce040fdebc 189
teamat 4:fd49edfabfb2 190 xPosOld = xPosNew;
teamat 4:fd49edfabfb2 191 }
teamat 4:fd49edfabfb2 192
teamat 4:fd49edfabfb2 193 void calcControl() {
teamat 6:999e8ae7d969 194 //float frequency = 1000000 / (period_us / 2);
teamat 6:999e8ae7d969 195 //float rates = frequency / 6400;
teamat 6:999e8ae7d969 196 //control = rates * PIPI * radius;
teamat 8:b3ce040fdebc 197 if (dir = DIR_RIGHT) {
teamat 8:b3ce040fdebc 198 control = -period_us;
teamat 8:b3ce040fdebc 199 } else {
teamat 8:b3ce040fdebc 200 control = period_us;
teamat 8:b3ce040fdebc 201 }
teamat 3:8708e61475fe 202 }
teamat 3:8708e61475fe 203
teamat 3:8708e61475fe 204
teamat 3:8708e61475fe 205
teamat 3:8708e61475fe 206 void stepperFlip() {
teamat 3:8708e61475fe 207 if (state != STATE_WAITING && state != STATE_ERROR && state != STATE_INIT && state){
teamat 3:8708e61475fe 208 RCout = !RCout;
teamat 3:8708e61475fe 209 pos += (dir.read() * 2 - 1);
teamat 3:8708e61475fe 210 }
teamat 3:8708e61475fe 211 if (state == STATE_GOTO_MIDDLE && pos == railLength / 2) {
teamat 8:b3ce040fdebc 212 posOffset = 40;
teamat 3:8708e61475fe 213 state = STATE_WAITING;
teamat 3:8708e61475fe 214 }
teamat 3:8708e61475fe 215 if (state == STATE_SWING_LEFT && state==STATE_SWING_RIGHT) {
teamat 3:8708e61475fe 216 pos += (dir.read() * 2 - 1);
teamat 3:8708e61475fe 217 RCout = !RCout;
teamat 3:8708e61475fe 218 }
teamat 3:8708e61475fe 219 }
teamat 3:8708e61475fe 220
teamat 3:8708e61475fe 221
teamat 3:8708e61475fe 222 void updatePeriod(){
teamat 3:8708e61475fe 223 tick.detach();
teamat 3:8708e61475fe 224 tick.attach_us (&stepperFlip, period_us / 2.0f);
teamat 9:e8c6a414e226 225 wait_ms(20);
teamat 3:8708e61475fe 226 }
teamat 3:8708e61475fe 227
teamat 3:8708e61475fe 228 void leftEnd() {
teamat 3:8708e61475fe 229 dir = DIR_RIGHT;
teamat 3:8708e61475fe 230 if (state == STATE_GOTO_START) {
teamat 3:8708e61475fe 231 state = STATE_GOTO_END_COUNTING;
teamat 3:8708e61475fe 232 pos = 0;
teamat 3:8708e61475fe 233 }
teamat 3:8708e61475fe 234 else if (isPendulumSwinging()) {
teamat 9:e8c6a414e226 235 //state = STATE_GOTO_MIDDLE;
teamat 3:8708e61475fe 236 //angleOffset -= 0.006191;
teamat 9:e8c6a414e226 237 state = STATE_ERROR;
teamat 3:8708e61475fe 238 }
teamat 9:e8c6a414e226 239 if (state == STATE_GOTO_SWING) {
teamat 9:e8c6a414e226 240 state = STATE_ERROR;
teamat 9:e8c6a414e226 241 }
teamat 3:8708e61475fe 242 }
teamat 3:8708e61475fe 243
teamat 3:8708e61475fe 244 void rightEnd() {
teamat 3:8708e61475fe 245 dir=DIR_LEFT;
teamat 3:8708e61475fe 246 if (state == STATE_GOTO_END_COUNTING) {
teamat 3:8708e61475fe 247 railLength=pos;
teamat 3:8708e61475fe 248 state = STATE_GOTO_MIDDLE;
teamat 3:8708e61475fe 249 }
teamat 3:8708e61475fe 250 else if (isPendulumSwinging()) {
teamat 9:e8c6a414e226 251 //state = STATE_GOTO_MIDDLE;
teamat 3:8708e61475fe 252 //angleOffset += 0.006191;
teamat 9:e8c6a414e226 253 state = STATE_ERROR;
teamat 9:e8c6a414e226 254 }
teamat 9:e8c6a414e226 255 if (state == STATE_GOTO_SWING) {
teamat 9:e8c6a414e226 256 state = STATE_ERROR;
teamat 3:8708e61475fe 257 }
teamat 3:8708e61475fe 258 }
teamat 3:8708e61475fe 259
teamat 9:e8c6a414e226 260 int swingCounter = 0;
teamat 3:8708e61475fe 261 void getSwingDirectory() {
teamat 9:e8c6a414e226 262 swingCounter += 1;
teamat 7:ca62dda005d5 263 control = -control;
teamat 4:fd49edfabfb2 264 if (dir == DIR_RIGHT) {
teamat 3:8708e61475fe 265 state = STATE_SWING_RIGHT;
teamat 3:8708e61475fe 266 dir = DIR_LEFT;
teamat 3:8708e61475fe 267 return;
teamat 3:8708e61475fe 268 }
teamat 4:fd49edfabfb2 269 if (dir == DIR_LEFT) {
teamat 3:8708e61475fe 270 state = STATE_SWING_LEFT;
teamat 3:8708e61475fe 271 dir = DIR_RIGHT;
teamat 3:8708e61475fe 272 return;
teamat 3:8708e61475fe 273 }
teamat 3:8708e61475fe 274 }
teamat 3:8708e61475fe 275
teamat 3:8708e61475fe 276 void sendData() {
teamat 3:8708e61475fe 277 myUnion.number[0] = t.read();
teamat 8:b3ce040fdebc 278 myUnion.number[1] = (getPosMM()- posOffset) / 1000 ;
teamat 4:fd49edfabfb2 279 myUnion.number[2] = speed;
teamat 4:fd49edfabfb2 280 //myUnion.number[3] = dAngle;
teamat 4:fd49edfabfb2 281 myUnion.number[3] = angle;
teamat 3:8708e61475fe 282 myUnion.number[4] = anSpd;
teamat 4:fd49edfabfb2 283 myUnion.number[5] = control;
teamat 4:fd49edfabfb2 284 for(int i = 0; i < 24; i++) {
teamat 3:8708e61475fe 285 rpc.putc(myUnion.numberCh[i]);
teamat 3:8708e61475fe 286 }
teamat 3:8708e61475fe 287 }
teamat 3:8708e61475fe 288
teamat 10:ca38e4d775c8 289 int dirCounter = 0;
teamat 10:ca38e4d775c8 290 int perCounter = 0;
teamat 10:ca38e4d775c8 291
teamat 10:ca38e4d775c8 292 void changeDir(int direction)
teamat 10:ca38e4d775c8 293 {
teamat 10:ca38e4d775c8 294 dir = direction;
teamat 10:ca38e4d775c8 295 }
teamat 10:ca38e4d775c8 296
teamat 3:8708e61475fe 297 void Rx_interrupt() {
teamat 4:fd49edfabfb2 298 int command = rpc.getc();
teamat 8:b3ce040fdebc 299 switch (command) {
teamat 8:b3ce040fdebc 300 case 50:
teamat 8:b3ce040fdebc 301 canSend = true;
teamat 8:b3ce040fdebc 302 break;
teamat 8:b3ce040fdebc 303 case 60:
teamat 8:b3ce040fdebc 304 state = STATE_GOTO_SWING;
teamat 8:b3ce040fdebc 305 speedTicker.attach(calcSpeed, 1);
teamat 8:b3ce040fdebc 306 break;
teamat 8:b3ce040fdebc 307 case 65:
teamat 10:ca38e4d775c8 308 if (dirCounter == 10) {
teamat 10:ca38e4d775c8 309 dirCounter = 0;
teamat 10:ca38e4d775c8 310 int direction = rpc.getc();
teamat 10:ca38e4d775c8 311 if (direction <= 0) {
teamat 10:ca38e4d775c8 312 if (dir != DIR_RIGHT) {
teamat 10:ca38e4d775c8 313 changeDir(DIR_RIGHT);
teamat 10:ca38e4d775c8 314 }
teamat 10:ca38e4d775c8 315 } else if (direction > 0) {
teamat 10:ca38e4d775c8 316 if (dir != DIR_LEFT) {
teamat 10:ca38e4d775c8 317 changeDir(DIR_LEFT);
teamat 10:ca38e4d775c8 318 }
teamat 9:e8c6a414e226 319 }
teamat 10:ca38e4d775c8 320 } else {
teamat 10:ca38e4d775c8 321 dirCounter += 1;
teamat 8:b3ce040fdebc 322 }
teamat 8:b3ce040fdebc 323 break;
teamat 8:b3ce040fdebc 324 case 70:
teamat 10:ca38e4d775c8 325 if (perCounter == 40) {
teamat 10:ca38e4d775c8 326 perCounter = 0;
teamat 10:ca38e4d775c8 327 int newPeriod = rpc.getc();
teamat 10:ca38e4d775c8 328 if (newPeriod < 26) {
teamat 10:ca38e4d775c8 329 tick.detach();
teamat 10:ca38e4d775c8 330 } else if (period_us != newPeriod) {
teamat 10:ca38e4d775c8 331 period_us = newPeriod;
teamat 10:ca38e4d775c8 332 periodUpdated = true;
teamat 10:ca38e4d775c8 333 }
teamat 10:ca38e4d775c8 334 } else {
teamat 10:ca38e4d775c8 335 perCounter += 1;
teamat 8:b3ce040fdebc 336 }
teamat 8:b3ce040fdebc 337 break;
teamat 8:b3ce040fdebc 338 default:
teamat 8:b3ce040fdebc 339 break;
teamat 3:8708e61475fe 340 }
teamat 3:8708e61475fe 341 return;
teamat 3:8708e61475fe 342 }
teamat 3:8708e61475fe 343
teamat 3:8708e61475fe 344 int main() {
teamat 3:8708e61475fe 345 RCout = 1;
teamat 3:8708e61475fe 346 wait_ms(500);
teamat 3:8708e61475fe 347 spi.format(16,2);
teamat 3:8708e61475fe 348 spi.frequency(1000000);
teamat 3:8708e61475fe 349 t.start();
teamat 3:8708e61475fe 350 leftSwitch.rise(&leftEnd);
teamat 3:8708e61475fe 351 rightSwitch.rise(&rightEnd);
teamat 4:fd49edfabfb2 352 rpc.attach(&Rx_interrupt, Serial::RxIrq);
teamat 3:8708e61475fe 353 for (int i=5; i>0; i--) {
teamat 3:8708e61475fe 354 getPendulumAngle();
teamat 3:8708e61475fe 355 wait_ms(500);
teamat 3:8708e61475fe 356 }
teamat 4:fd49edfabfb2 357 angleOffset= 3.14 - angle;
teamat 8:b3ce040fdebc 358 calibrated = true;
teamat 8:b3ce040fdebc 359 //wait(12);
teamat 8:b3ce040fdebc 360 wait(7);
teamat 3:8708e61475fe 361 updatePeriod();
teamat 6:999e8ae7d969 362 calcControl();
teamat 3:8708e61475fe 363 state=STATE_GOTO_START;
teamat 3:8708e61475fe 364 dir=DIR_LEFT;
teamat 3:8708e61475fe 365 while(1) {
teamat 4:fd49edfabfb2 366 getAngularSpeed();
teamat 8:b3ce040fdebc 367 if (canSend) {
teamat 3:8708e61475fe 368 sendData();
teamat 3:8708e61475fe 369 }
teamat 9:e8c6a414e226 370 /*if (periodUpdated) {
teamat 8:b3ce040fdebc 371 calcControl();
teamat 8:b3ce040fdebc 372 updatePeriod();
teamat 8:b3ce040fdebc 373 periodUpdated = false;
teamat 9:e8c6a414e226 374 }*/
teamat 3:8708e61475fe 375 switch(state) {
teamat 3:8708e61475fe 376 case STATE_WAITING:
teamat 8:b3ce040fdebc 377 //state = STATE_GOTO_SWING;
teamat 3:8708e61475fe 378 break;
teamat 3:8708e61475fe 379 case STATE_GOTO_START:
teamat 3:8708e61475fe 380 break;
teamat 3:8708e61475fe 381 case STATE_GOTO_END_COUNTING:
teamat 3:8708e61475fe 382 break;
teamat 3:8708e61475fe 383 case STATE_GOTO_SWING:
teamat 9:e8c6a414e226 384 //speedTicker.attach(calcSpeed, 1);
teamat 8:b3ce040fdebc 385 //swingTicker.attach(getSwingDirectory, 1);
teamat 8:b3ce040fdebc 386 //state = STATE_SWING_LEFT;
teamat 3:8708e61475fe 387 break;
teamat 3:8708e61475fe 388 case STATE_SWING_LEFT:
teamat 9:e8c6a414e226 389 /*if (swingCounter == 6) {
teamat 9:e8c6a414e226 390 swingCounter = 0;
teamat 9:e8c6a414e226 391 period_us += 4;
teamat 9:e8c6a414e226 392 periodUpdated = true;
teamat 9:e8c6a414e226 393 }*/
teamat 3:8708e61475fe 394 break;
teamat 3:8708e61475fe 395 case STATE_SWING_RIGHT:
teamat 9:e8c6a414e226 396 /*if (swingCounter == 6) {
teamat 9:e8c6a414e226 397 swingCounter = 0;
teamat 9:e8c6a414e226 398 period_us += 4;
teamat 9:e8c6a414e226 399 periodUpdated = true;
teamat 9:e8c6a414e226 400 }*/
teamat 3:8708e61475fe 401 break;
teamat 3:8708e61475fe 402 default:
teamat 3:8708e61475fe 403 break;
teamat 3:8708e61475fe 404 }
teamat 10:ca38e4d775c8 405 //wait_ms(100);
teamat 3:8708e61475fe 406 }
teamat 3:8708e61475fe 407 }