fsdfds

Dependencies:   mbed

Committer:
teamat
Date:
Fri May 22 13:27:01 2020 +0000
Revision:
11:6bbe8da4b16a
Parent:
10:ca38e4d775c8
Child:
12:ee175985ef09
angle not working

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 3:8708e61475fe 20
teamat 3:8708e61475fe 21 DigitalOut RCout(D10);
teamat 3:8708e61475fe 22 DigitalOut dir(D9);
teamat 3:8708e61475fe 23 InterruptIn leftSwitch(D2);
teamat 3:8708e61475fe 24 InterruptIn rightSwitch(D3);
teamat 3:8708e61475fe 25 Ticker tick;
teamat 4:fd49edfabfb2 26 Ticker speedTicker;
teamat 4:fd49edfabfb2 27 Ticker swingTicker;
teamat 6:999e8ae7d969 28 Ticker sendDataTicker;
teamat 3:8708e61475fe 29 DigitalOut myled(LED2);
teamat 3:8708e61475fe 30 RawSerial rpc(D1,D0,9600);
teamat 3:8708e61475fe 31
teamat 11:6bbe8da4b16a 32
teamat 11:6bbe8da4b16a 33 int period_us = 22;//300 26
teamat 7:ca62dda005d5 34 int posCounter = 0;
teamat 3:8708e61475fe 35
teamat 3:8708e61475fe 36 long pos=0;
teamat 3:8708e61475fe 37
teamat 3:8708e61475fe 38 long railLength = 0;
teamat 3:8708e61475fe 39
teamat 3:8708e61475fe 40 uint8_t state=STATE_INIT;
teamat 3:8708e61475fe 41
teamat 3:8708e61475fe 42 Timer t;
teamat 3:8708e61475fe 43 SPI spi(D11,D12,D13);// mosi, miso, sclk
teamat 3:8708e61475fe 44 DigitalOut cs(D5);
teamat 3:8708e61475fe 45
teamat 4:fd49edfabfb2 46 float radius = 0.0025;
teamat 3:8708e61475fe 47
teamat 3:8708e61475fe 48 float angularPosNew = 0;
teamat 3:8708e61475fe 49 float angularPosOld = 0;
teamat 3:8708e61475fe 50
teamat 3:8708e61475fe 51 float timeOld = 0.0f;
teamat 3:8708e61475fe 52 float timeStart = 0.0f;
teamat 3:8708e61475fe 53
teamat 3:8708e61475fe 54 float dx = 0;
teamat 3:8708e61475fe 55 float xPosNew = 0;
teamat 3:8708e61475fe 56 float xPosOld = 0;
teamat 8:b3ce040fdebc 57 float speed = -1;
teamat 8:b3ce040fdebc 58 float posOffset = 0.0f;
teamat 7:ca62dda005d5 59 float posMap[4] = {0.0f, 0.0f, 0.0f, 0.0f};
teamat 3:8708e61475fe 60
teamat 3:8708e61475fe 61 double dAngle = 0.0f;
teamat 3:8708e61475fe 62 double anSpd = 0.0f;
teamat 3:8708e61475fe 63
teamat 3:8708e61475fe 64 float timeOldPos = 0.0f;
teamat 3:8708e61475fe 65 float timeStartPos = 0.0f;
teamat 3:8708e61475fe 66
teamat 4:fd49edfabfb2 67 float angle=0.0f;
teamat 3:8708e61475fe 68 float angleOffset = 0;
teamat 3:8708e61475fe 69
teamat 4:fd49edfabfb2 70 float control = 0.0f;
teamat 4:fd49edfabfb2 71
teamat 3:8708e61475fe 72 double PIPI = 6.28;
teamat 3:8708e61475fe 73
teamat 3:8708e61475fe 74 bool canSend = false;
teamat 8:b3ce040fdebc 75 bool periodUpdated = false;
teamat 8:b3ce040fdebc 76 bool calibrated = false;
teamat 9:e8c6a414e226 77 bool dirUpdated = false;
teamat 3:8708e61475fe 78
teamat 11:6bbe8da4b16a 79
teamat 11:6bbe8da4b16a 80 DigitalOut led(LED1);
teamat 11:6bbe8da4b16a 81 Timeout flipper;
teamat 11:6bbe8da4b16a 82
teamat 11:6bbe8da4b16a 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 3:8708e61475fe 91 bool isPendulumSwinging() {
teamat 3:8708e61475fe 92 return state == STATE_SWING_RIGHT || state == STATE_SWING_LEFT;
teamat 3:8708e61475fe 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 8:b3ce040fdebc 117 if (angle > 3.14) {
teamat 8:b3ce040fdebc 118 angle = angle - PIPI;
teamat 8:b3ce040fdebc 119 }
teamat 8:b3ce040fdebc 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 9:e8c6a414e226 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 4:fd49edfabfb2 170 void calcSpeed() {
teamat 7:ca62dda005d5 171 /*posMap[posCounter] = getPosMM() / 1000;
teamat 7:ca62dda005d5 172 if (posCounter == 3) {
teamat 7:ca62dda005d5 173 posCounter = 0;
teamat 7:ca62dda005d5 174 float pathSum = 0;
teamat 7:ca62dda005d5 175 for (int i = 0; i < 3; i++) {
teamat 7:ca62dda005d5 176 float pathDiff = posMap[i + 1] - posMap[i];
teamat 7:ca62dda005d5 177 if ((pathDiff) < 0) {
teamat 7:ca62dda005d5 178 pathSum += -pathDiff;
teamat 7:ca62dda005d5 179 } else {
teamat 7:ca62dda005d5 180 pathSum += pathDiff;
teamat 7:ca62dda005d5 181 }
teamat 7:ca62dda005d5 182 }
teamat 7:ca62dda005d5 183 speed = pathSum;
teamat 7:ca62dda005d5 184 } else {
teamat 7:ca62dda005d5 185 posCounter += 1;
teamat 7:ca62dda005d5 186 }*/
teamat 8:b3ce040fdebc 187 xPosNew = (getPosMM() - posOffset) / 1000;
teamat 4:fd49edfabfb2 188 speed = xPosNew - xPosOld;
teamat 8:b3ce040fdebc 189 if (dir == DIR_LEFT) {
teamat 8:b3ce040fdebc 190 speed = -speed;
teamat 8:b3ce040fdebc 191 }
teamat 9:e8c6a414e226 192 //speed = -speed;
teamat 8:b3ce040fdebc 193
teamat 4:fd49edfabfb2 194 xPosOld = xPosNew;
teamat 4:fd49edfabfb2 195 }
teamat 4:fd49edfabfb2 196
teamat 4:fd49edfabfb2 197 void calcControl() {
teamat 6:999e8ae7d969 198 //float frequency = 1000000 / (period_us / 2);
teamat 6:999e8ae7d969 199 //float rates = frequency / 6400;
teamat 6:999e8ae7d969 200 //control = rates * PIPI * radius;
teamat 8:b3ce040fdebc 201 if (dir = DIR_RIGHT) {
teamat 8:b3ce040fdebc 202 control = -period_us;
teamat 8:b3ce040fdebc 203 } else {
teamat 8:b3ce040fdebc 204 control = period_us;
teamat 8:b3ce040fdebc 205 }
teamat 3:8708e61475fe 206 }
teamat 3:8708e61475fe 207
teamat 3:8708e61475fe 208
teamat 3:8708e61475fe 209
teamat 3:8708e61475fe 210 void stepperFlip() {
teamat 3:8708e61475fe 211 if (state != STATE_WAITING && state != STATE_ERROR && state != STATE_INIT && state){
teamat 3:8708e61475fe 212 RCout = !RCout;
teamat 3:8708e61475fe 213 pos += (dir.read() * 2 - 1);
teamat 3:8708e61475fe 214 }
teamat 3:8708e61475fe 215 if (state == STATE_GOTO_MIDDLE && pos == railLength / 2) {
teamat 8:b3ce040fdebc 216 posOffset = 40;
teamat 3:8708e61475fe 217 state = STATE_WAITING;
teamat 3:8708e61475fe 218 }
teamat 3:8708e61475fe 219 if (state == STATE_SWING_LEFT && state==STATE_SWING_RIGHT) {
teamat 3:8708e61475fe 220 pos += (dir.read() * 2 - 1);
teamat 3:8708e61475fe 221 RCout = !RCout;
teamat 3:8708e61475fe 222 }
teamat 11:6bbe8da4b16a 223 flipper.attach_us(stepperFlip, period_us/2);
teamat 3:8708e61475fe 224 }
teamat 3:8708e61475fe 225
teamat 3:8708e61475fe 226
teamat 3:8708e61475fe 227 void updatePeriod(){
teamat 11:6bbe8da4b16a 228 //tick.detach();
teamat 3:8708e61475fe 229 tick.attach_us (&stepperFlip, period_us / 2.0f);
teamat 3:8708e61475fe 230 }
teamat 3:8708e61475fe 231
teamat 3:8708e61475fe 232 void leftEnd() {
teamat 3:8708e61475fe 233 dir = DIR_RIGHT;
teamat 3:8708e61475fe 234 if (state == STATE_GOTO_START) {
teamat 3:8708e61475fe 235 state = STATE_GOTO_END_COUNTING;
teamat 3:8708e61475fe 236 pos = 0;
teamat 3:8708e61475fe 237 }
teamat 3:8708e61475fe 238 else if (isPendulumSwinging()) {
teamat 9:e8c6a414e226 239 //state = STATE_GOTO_MIDDLE;
teamat 3:8708e61475fe 240 //angleOffset -= 0.006191;
teamat 9:e8c6a414e226 241 state = STATE_ERROR;
teamat 3:8708e61475fe 242 }
teamat 9:e8c6a414e226 243 if (state == STATE_GOTO_SWING) {
teamat 9:e8c6a414e226 244 state = STATE_ERROR;
teamat 9:e8c6a414e226 245 }
teamat 3:8708e61475fe 246 }
teamat 3:8708e61475fe 247
teamat 3:8708e61475fe 248 void rightEnd() {
teamat 3:8708e61475fe 249 dir=DIR_LEFT;
teamat 3:8708e61475fe 250 if (state == STATE_GOTO_END_COUNTING) {
teamat 3:8708e61475fe 251 railLength=pos;
teamat 3:8708e61475fe 252 state = STATE_GOTO_MIDDLE;
teamat 3:8708e61475fe 253 }
teamat 3:8708e61475fe 254 else if (isPendulumSwinging()) {
teamat 9:e8c6a414e226 255 //state = STATE_GOTO_MIDDLE;
teamat 3:8708e61475fe 256 //angleOffset += 0.006191;
teamat 9:e8c6a414e226 257 state = STATE_ERROR;
teamat 9:e8c6a414e226 258 }
teamat 9:e8c6a414e226 259 if (state == STATE_GOTO_SWING) {
teamat 9:e8c6a414e226 260 state = STATE_ERROR;
teamat 3:8708e61475fe 261 }
teamat 3:8708e61475fe 262 }
teamat 3:8708e61475fe 263
teamat 9:e8c6a414e226 264 int swingCounter = 0;
teamat 3:8708e61475fe 265 void getSwingDirectory() {
teamat 9:e8c6a414e226 266 swingCounter += 1;
teamat 7:ca62dda005d5 267 control = -control;
teamat 4:fd49edfabfb2 268 if (dir == DIR_RIGHT) {
teamat 3:8708e61475fe 269 state = STATE_SWING_RIGHT;
teamat 3:8708e61475fe 270 dir = DIR_LEFT;
teamat 3:8708e61475fe 271 return;
teamat 3:8708e61475fe 272 }
teamat 4:fd49edfabfb2 273 if (dir == DIR_LEFT) {
teamat 3:8708e61475fe 274 state = STATE_SWING_LEFT;
teamat 3:8708e61475fe 275 dir = DIR_RIGHT;
teamat 3:8708e61475fe 276 return;
teamat 3:8708e61475fe 277 }
teamat 3:8708e61475fe 278 }
teamat 3:8708e61475fe 279
teamat 3:8708e61475fe 280 void sendData() {
teamat 3:8708e61475fe 281 myUnion.number[0] = t.read();
teamat 8:b3ce040fdebc 282 myUnion.number[1] = (getPosMM()- posOffset) / 1000 ;
teamat 4:fd49edfabfb2 283 myUnion.number[2] = speed;
teamat 4:fd49edfabfb2 284 //myUnion.number[3] = dAngle;
teamat 4:fd49edfabfb2 285 myUnion.number[3] = angle;
teamat 3:8708e61475fe 286 myUnion.number[4] = anSpd;
teamat 4:fd49edfabfb2 287 myUnion.number[5] = control;
teamat 4:fd49edfabfb2 288 for(int i = 0; i < 24; i++) {
teamat 3:8708e61475fe 289 rpc.putc(myUnion.numberCh[i]);
teamat 3:8708e61475fe 290 }
teamat 3:8708e61475fe 291 }
teamat 3:8708e61475fe 292
teamat 10:ca38e4d775c8 293 int dirCounter = 0;
teamat 10:ca38e4d775c8 294 int perCounter = 0;
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 11:6bbe8da4b16a 308 int direction = rpc.getc();
teamat 11:6bbe8da4b16a 309 if (direction <= 0) {
teamat 11:6bbe8da4b16a 310 if (dir != DIR_RIGHT) {
teamat 11:6bbe8da4b16a 311 dir = DIR_RIGHT;
teamat 9:e8c6a414e226 312 }
teamat 11:6bbe8da4b16a 313 } else if (direction > 0) {
teamat 11:6bbe8da4b16a 314 if (dir != DIR_LEFT) {
teamat 11:6bbe8da4b16a 315 dir = DIR_LEFT;
teamat 11:6bbe8da4b16a 316 }
teamat 8:b3ce040fdebc 317 }
teamat 8:b3ce040fdebc 318 break;
teamat 8:b3ce040fdebc 319 case 70:
teamat 11:6bbe8da4b16a 320 /*int newPeriod = rpc.getc();
teamat 11:6bbe8da4b16a 321 if (newPeriod < 22) {
teamat 11:6bbe8da4b16a 322 flipper.detach();
teamat 11:6bbe8da4b16a 323 } else if (period_us != newPeriod) {
teamat 11:6bbe8da4b16a 324 period_us = newPeriod;
teamat 11:6bbe8da4b16a 325 periodUpdated = true;
teamat 11:6bbe8da4b16a 326 }*/
teamat 8:b3ce040fdebc 327 break;
teamat 8:b3ce040fdebc 328 default:
teamat 8:b3ce040fdebc 329 break;
teamat 3:8708e61475fe 330 }
teamat 3:8708e61475fe 331 return;
teamat 3:8708e61475fe 332 }
teamat 3:8708e61475fe 333
teamat 11:6bbe8da4b16a 334 /*void flipLed()
teamat 11:6bbe8da4b16a 335 {
teamat 11:6bbe8da4b16a 336 flipper.attach_us(stepperFlip, period_us);
teamat 11:6bbe8da4b16a 337 }*/
teamat 11:6bbe8da4b16a 338
teamat 3:8708e61475fe 339 int main() {
teamat 3:8708e61475fe 340 RCout = 1;
teamat 3:8708e61475fe 341 wait_ms(500);
teamat 3:8708e61475fe 342 spi.format(16,2);
teamat 3:8708e61475fe 343 spi.frequency(1000000);
teamat 3:8708e61475fe 344 t.start();
teamat 3:8708e61475fe 345 leftSwitch.rise(&leftEnd);
teamat 3:8708e61475fe 346 rightSwitch.rise(&rightEnd);
teamat 4:fd49edfabfb2 347 rpc.attach(&Rx_interrupt, Serial::RxIrq);
teamat 3:8708e61475fe 348 for (int i=5; i>0; i--) {
teamat 3:8708e61475fe 349 getPendulumAngle();
teamat 3:8708e61475fe 350 wait_ms(500);
teamat 3:8708e61475fe 351 }
teamat 4:fd49edfabfb2 352 angleOffset= 3.14 - angle;
teamat 8:b3ce040fdebc 353 calibrated = true;
teamat 8:b3ce040fdebc 354 //wait(12);
teamat 8:b3ce040fdebc 355 wait(7);
teamat 11:6bbe8da4b16a 356 //updatePeriod();
teamat 6:999e8ae7d969 357 calcControl();
teamat 3:8708e61475fe 358 state=STATE_GOTO_START;
teamat 3:8708e61475fe 359 dir=DIR_LEFT;
teamat 11:6bbe8da4b16a 360 flipper.attach_us(stepperFlip, period_us/2);
teamat 3:8708e61475fe 361 while(1) {
teamat 4:fd49edfabfb2 362 getAngularSpeed();
teamat 8:b3ce040fdebc 363 if (canSend) {
teamat 3:8708e61475fe 364 sendData();
teamat 3:8708e61475fe 365 }
teamat 11:6bbe8da4b16a 366 if (periodUpdated) {
teamat 8:b3ce040fdebc 367 calcControl();
teamat 11:6bbe8da4b16a 368 // updatePeriod();
teamat 11:6bbe8da4b16a 369 //flipper.attach_us(stepperFlip, period_us/2);
teamat 8:b3ce040fdebc 370 periodUpdated = false;
teamat 11:6bbe8da4b16a 371 }
teamat 3:8708e61475fe 372 switch(state) {
teamat 3:8708e61475fe 373 case STATE_WAITING:
teamat 8:b3ce040fdebc 374 //state = STATE_GOTO_SWING;
teamat 3:8708e61475fe 375 break;
teamat 3:8708e61475fe 376 case STATE_GOTO_START:
teamat 3:8708e61475fe 377 break;
teamat 3:8708e61475fe 378 case STATE_GOTO_END_COUNTING:
teamat 3:8708e61475fe 379 break;
teamat 3:8708e61475fe 380 case STATE_GOTO_SWING:
teamat 9:e8c6a414e226 381 //speedTicker.attach(calcSpeed, 1);
teamat 8:b3ce040fdebc 382 //swingTicker.attach(getSwingDirectory, 1);
teamat 8:b3ce040fdebc 383 //state = STATE_SWING_LEFT;
teamat 3:8708e61475fe 384 break;
teamat 3:8708e61475fe 385 case STATE_SWING_LEFT:
teamat 9:e8c6a414e226 386 /*if (swingCounter == 6) {
teamat 9:e8c6a414e226 387 swingCounter = 0;
teamat 9:e8c6a414e226 388 period_us += 4;
teamat 9:e8c6a414e226 389 periodUpdated = true;
teamat 9:e8c6a414e226 390 }*/
teamat 3:8708e61475fe 391 break;
teamat 3:8708e61475fe 392 case STATE_SWING_RIGHT:
teamat 9:e8c6a414e226 393 /*if (swingCounter == 6) {
teamat 9:e8c6a414e226 394 swingCounter = 0;
teamat 9:e8c6a414e226 395 period_us += 4;
teamat 9:e8c6a414e226 396 periodUpdated = true;
teamat 9:e8c6a414e226 397 }*/
teamat 3:8708e61475fe 398 break;
teamat 3:8708e61475fe 399 default:
teamat 3:8708e61475fe 400 break;
teamat 3:8708e61475fe 401 }
teamat 10:ca38e4d775c8 402 //wait_ms(100);
teamat 3:8708e61475fe 403 }
teamat 3:8708e61475fe 404 }