Artem Solomatin / Mbed 2 deprecated pendulum

fsdfds

Dependencies:   mbed

main.cpp@1:36f06541698a, 2020-05-16 (annotated)

Committer:
teamat
Date:
Sat May 16 12:03:46 2020 +0000
Revision:
1:36f06541698a
Parent:
0:bdfad365ab0b
Child:
2:32c9df18a589
test

Who changed what in which revision?

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