Artem Solomatin / Mbed 2 deprecated pendudu

fsdfds

Dependencies:   mbed

main.cpp@14:353e92037bc8, 2020-05-24 (annotated)

Committer:
teamat
Date:
Sun May 24 13:53:26 2020 +0000
Revision:
14:353e92037bc8
Parent:
13:205002e3c176 
For typical experiments

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