ロボステ6期
/
NHK2020-arm-sub2
12/18
Revision 1:bc34fdc4e16b, committed 2019-12-18
- Comitter:
- aoikoizumi
- Date:
- Wed Dec 18 02:33:45 2019 +0000
- Parent:
- 0:6e2abd0956f1
- Commit message:
- f
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 6e2abd0956f1 -r bc34fdc4e16b main.cpp --- a/main.cpp Sat Dec 14 12:42:13 2019 +0000 +++ b/main.cpp Wed Dec 18 02:33:45 2019 +0000 @@ -37,32 +37,34 @@ { b=0; } -void move() +void move_motor() { - double old_turn=turn; - a=EC_backdrop.getRad(); - if(a-b>=0.1) { - turn=0.1; - //pc.printf("F"); - - } else if (a-b>=0.05) { - turn=10*(a-b)*(a-b); - //pc.printf("f"); - } else if (b-a>=0.1) { - turn=-0.1; - //pc.printf("B"); - } else if (b-a>=0.05) { - turn=-10*(a-b)*(a-b); - //pc.printf("b"); - } else { - backdrop.stop(); - backdrop.turn(0); - turn=0; - //break; - //pc.printf("s"); + while(1) { + double old_turn=turn; + a=EC_backdrop.getRad(); + if(a-b>=0.1) { + turn=0.1; + pc.printf("F"); + } else if (a-b>=0.05) { + turn=10*(a-b)*(a-b); + pc.printf("f"); + } else if (b-a>=0.1) { + turn=-0.1; + pc.printf("B"); + } else if (b-a>=0.05) { + turn=-10*(a-b)*(a-b); + pc.printf("b"); + } else { + backdrop.stop(); + backdrop.turn(0); + turn=0; + pc.printf("s"); + } + if(turn*old_turn<0)turn=0; + backdrop.turn(turn); + pc.printf("%lf",EC_backdrop.getRad()); + if(b-a<0.05&&a-b<0.05)break; } - if(turn*old_turn<0)turn=0; - backdrop.turn(turn); } int q = 0; @@ -109,9 +111,8 @@ int main() { - move(); - pc.printf("setting please"); + move_motor(); while(1) { @@ -218,16 +219,22 @@ if(T2 == 1) { //ボール掴んで投げる //printf("a-T = 1 T2 = %d t2_r = %d\n\r",T2,t2_r); + snatch=1; tsukami(); - move(); + printf("tsukami"); + move_motor(); wait(3); snatch=0; wait(1); put(); + printf("put"); + move_motor(); wait(3); snatch=1; wait(1); top(); + printf("top"); + move_motor(); wait(5); pass1=0; wait(5);