a
Dependencies: Servo ServoArm mbed
Fork of PES_Official-TestF by
Revision 18:a158713a0049, committed 2017-05-19
- Comitter:
- EpicG10
- Date:
- Fri May 19 06:40:33 2017 +0000
- Parent:
- 17:4e1be70bdedb
- Commit message:
- a
Changed in this revision
diff -r 4e1be70bdedb -r a158713a0049 Sources/Robot.cpp --- a/Sources/Robot.cpp Thu May 18 14:27:09 2017 +0000 +++ b/Sources/Robot.cpp Fri May 19 06:40:33 2017 +0000 @@ -57,8 +57,8 @@ { *powerSignal = 1; - *left= 0.45f; - *right= 0.45f; + *left= 0.42f; + *right= 0.42f; } @@ -73,8 +73,8 @@ { *powerSignal = 1; - *left= 0.55f; - *right= 0.55f; + *left= 0.58f; + *right= 0.58f; } @@ -264,7 +264,7 @@ */ - //printf("\n\rcurrent robot state: %d", state); + printf("\n\rcurrent robot state: %d", state); switch( state ){ case neutral: if( counter > MAX ){ @@ -279,7 +279,7 @@ else if( this->see(RIGHT) < NEAR ){ state = wallR; } - else if( this->see(FWD_L) < NEAR_LEGO + 0.015f ){ + else if( this->see(FWD_L) < NEAR_LEGO + 0.03f ){ state = legoF; } else if( this->see(LEFT_L) < NEAR_LEGO ){ @@ -295,7 +295,7 @@ break; case max: { - int time = 0; + int time = 0; if( time < 15 && this->see(FWD) > NEAR ){ rando == -1 ? rando = rand() % 2 : rando = rando; this->turnAround(rando); @@ -346,40 +346,71 @@ state = wallF; //stay = -1; } - else if( this->see(FWD_L) < 0.19f ){ + else if( this->see(FWD_L) < 0.14f ){ this->driveBackSlowly(); } else{ - state = neutral; + //state = neutral; //stay = -1; counter = 0; this->stop(); + state=neutral; return 1; } break; case legoL: counter++; - if( this->see(FWD_L) > NEAR_LEGO + 0.05f ){ - this->turnLeft(); - } + if(counter>MAX) state=neutral; + static int i=0; + if(i<4 && see(LEFT_L)< 0.15){ + this->driveB(); + i++; + } else{ - state = neutral; - this->drive(); + this->stop(); + + if( this->see(FWD_L) > NEAR_LEGO + 0.15f ){ + this->turnLeftS(); + this->USsensor.read()<0.15 ? this->leds[5]=1:leds[5]=0; + } + else if(see(FWD) < NEAR_LEGO + 0.05f ){ + state= wallF; + } + else{ + state = legoF; + i=0; + //this->drive(); //stay = 1; } + } break; case legoR: + if(counter>MAX) state=neutral; counter++; - if( this->see(FWD_L) > NEAR_LEGO + 0.05f ){ - this->turnRight(); + static int a=0; + if(a<4 && this->see(RIGHT_L)< 0.15){ + this->driveB(); + a++; + } + else{ + this->stop(); + + if( this->see(FWD_L) > NEAR_LEGO + 0.15f ){ + this->turnRightS(); + this->USsensor.read()<0.15 ? this->leds[5]=1:leds[5]=0; } + else if(see(FWD) < NEAR_LEGO + 0.05f ){ + state= wallF; + } else{ - this->drive(); - state = neutral; + //this->drive(); + a=0; + state = legoF; //stay = 1; } + } break; }
diff -r 4e1be70bdedb -r a158713a0049 Sources/USsensor.cpp --- a/Sources/USsensor.cpp Thu May 18 14:27:09 2017 +0000 +++ b/Sources/USsensor.cpp Fri May 19 06:40:33 2017 +0000 @@ -21,8 +21,9 @@ float dist = Usensor->get_dist_cm(); static float distFiltered = dist; - distFiltered = 0.05f * distFiltered + 0.95f * dist; - return distFiltered / 100; + //distFiltered = 0.05f * distFiltered + 0.95f * dist; + printf("\n\rmeas= %.3f",distFiltered/100); + return dist / 100; //return dist/100.0f; } else{
diff -r 4e1be70bdedb -r a158713a0049 Sources/main.cpp --- a/Sources/main.cpp Thu May 18 14:27:09 2017 +0000 +++ b/Sources/main.cpp Fri May 19 06:40:33 2017 +0000 @@ -73,18 +73,13 @@ //int done = 0; int color; - enum states { search = 0, LeisteDown, turn, push, backOff, forward, downward, down, upward, colorS, readyDrop, backward, LeisteUp }; + enum states { search = 0, LeisteDown, turn, push, pos, backOff, forward, downward, down, upward, colorS, readyDrop, backward, LeisteUp }; int state = search; - static float messung = 0; + // static float messung = 0; - while( 1 ){ - printf("\n\r%f", sam.see(FWD_L)); - - wait(1.0f); - - } + while( 1 ) { @@ -96,7 +91,7 @@ //printf("\n\rLEFT: %.3f,\tFWD: %.3f,\tRIGHT: %.3f", sam.sensors[LEFT].read(), sam.sensors[FWD].read(), sam.sensors[RIGHT].read()); - //printf("\n\rcurrent main state: %d", state); + printf("\n\rcurrent main state: %d", state); sam.sensors[FWD_L].read() < NEAR ? sam.leds[1] = 1 : sam.leds[1] = 0; switch( state ) { @@ -118,8 +113,8 @@ break; case turn: - static int i = 0; - if( i > 7 ){ + /* static int i = 0; + if( i > 4 ){ sam.stop(); state = push; i = 0; @@ -127,36 +122,54 @@ else{ i++; sam.turnRight(); - } + }*/ + state = push; break; case push:{ static int i = 0; - if( i > 5 ){ + if( i > 6 ){ + sam.stop(); + i = 0; + state = pos; + timer = 0; + } + else{ + sam.drive(); + i++; + } + break; + } + + case pos:{ + static int i = 0; + if( i > 4 ){ sam.stop(); i = 0; state = backOff; timer = 0; } else{ + sam.turnRightS(); + i++; + } + + break; + } + + case backOff:{ + static int i = 0; + if( i > 2 ){ + sam.stop(); + i = 0; + state = forward; + timer = 0; + } + else{ sam.driveSlowly(); i++; } - break; - } - - case backOff:{ - static int i = 0; - if( i > 1 ){ - sam.stop(); - i = 0; - state = forward; - timer = 0; - } - else{ - sam.driveBackSlowly(); - i++; - } + //state = forward; break; }