Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Diff: main.cpp
- Revision:
- 32:aee87dcaf7ca
- Parent:
- 31:ed4249a288d7
- Child:
- 33:2dbbe198adaf
--- a/main.cpp Wed May 08 11:00:09 2019 +0000 +++ b/main.cpp Mon May 13 03:44:50 2019 +0000 @@ -5,6 +5,11 @@ #include "moves.h" void StateFlow(int i); +void SetMode(); +void Start(); +void NoHandSignal(); +int RightOrLeft = 0; +int start_state=0; int main() { @@ -12,7 +17,14 @@ can1.frequency(1000000); SetupMoves(); set_gyro(); - int start_state=0; + + /*while(1) + { + printf("switch 1:%d 2:%d 3:%d LR:%d \r\n", switch_modes[0].read(),switch_modes[1].read(),switch_modes[2].read(),switch_LR.read()); + wait(0.1); + }*/ + printf("hand read:%d\r\n",hand.read()); + printf("reset standby\r\n"); reset();//足をリセット位置に移動 printf("bus standby\r\n"); @@ -25,31 +37,32 @@ motor_lo.setDutyLimit(0.1); motor_li.setDutyLimit(0.1); straight(); + + // mode選択 - while(mode4.read() == 1) { - start_state = (get_dist_back()-10)/10; - if(start_state>6 || start_state<0) start_state = 0; - int tmp = start_state; - led2 = start_state/4; - start_state = start_state%4; - led3 = start_state/2; - start_state = start_state%2; - led4 = start_state; - start_state = tmp; + Start(); + //SetMode(); + + if(RightOrLeft == 0) + { + if(start_state == 0) theta0 = -degree/100.0; + else if(start_state == 1) theta0 = -degree/100.0 - 45; + else if(start_state == 2) theta0 = -degree/100.0 - 90; + else if(start_state == 3) theta0 = -degree/100.0 + 90; + else printf("state_error"); } - printf("mode is %d\r\n", start_state); - wait_gerege(); - hand_mode = GEREGE; - HandMove(); - wait(0.5); - if(start_state == 0) theta0 = -degree/100.0; - else if(start_state == 1) theta0 = -degree/100.0 - 45; - else if(start_state == 2) theta0 = -degree/100.0 - 90; - else if(start_state == 3) theta0 = -degree/100.0 + 90; - else printf("state_error"); - FileOpen(); + else + { + if(start_state == 0) theta0 = -degree/100.0; + else if(start_state == 1) theta0 = -degree/100.0 + 45; + else if(start_state == 2) theta0 = -degree/100.0 + 90; + else if(start_state == 3) theta0 = -degree/100.0 - 90; + else printf("state_error"); + } + //FileOpen(); + printf("LR:%d StateFlow:%d\r\n", RightOrLeft,start_state); StateFlow(start_state); - FileClose(); + //FileClose(); } void StateFlow(int i) @@ -57,46 +70,58 @@ switch(i) { case 0://最初の直線 printf("go straight!!!!!!!!!!\r\n"); - motor_lo.setDutyLimit(0.5); - motor_li.setDutyLimit(0.5); - for(int i = 0; i < 25; i++) { - straightAndInfinity(0, 3); + motor_lo.setDutyLimit(0.7); + motor_li.setDutyLimit(0.7); + for(int i = 0; i < 30; i++) { + straightAndInfinity(0, 7); } printf("get distance mode"); for(int i=0; i<1; ++i) { - while(get_dist_back() < 290) {//320 - straightAndInfinity(0, 3); + while(get_dist_back() < 320) {//300 + straightAndInfinity(0, 7); } } //段差前旋回 motor_lo.setDutyLimit(0.5);//0.5 motor_li.setDutyLimit(0.5); - turn(35.0); + + if(RightOrLeft == 0) turn(35.0); + else turn(-35.0); case 1://段差前 printf("climb!!!!!!!!!!\r\n"); //段差乗り越え - motor_lo.setDutyLimit(0.2);//0.5 - motor_li.setDutyLimit(0.2); + motor_lo.setDutyLimit(0.3);//0.5 + motor_li.setDutyLimit(0.3); + while(get_dist_forward() < 60) { - straightAndInfinity(45, 5); + if(RightOrLeft == 0) straightAndInfinity(45, 5); + else straightAndInfinity(-45, 5); } - phasing(45.0); + if(RightOrLeft == 0) phasing(45.0); + else phasing(-45.0); + + + for(int i= 0; i<14; ++i) straight(); while(get_dist_back() > 80) straight(); //段差後旋回 printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back()); - motor_lo.setDutyLimit(0.4);//0.5 - motor_li.setDutyLimit(0.4); - turn(90.0); + motor_lo.setDutyLimit(0.5);//0.5 + motor_li.setDutyLimit(0.5); + + if(RightOrLeft == 0) turn(85.0); + else turn(-85.0); + case 2://段差直後 printf("go lope!!!!!!!!!!\r\n"); printf("angle:%.3f back dist:%.2f\r\n", degree0, get_dist_back()); //前進 - motor_lo.setDutyLimit(0.5); - motor_li.setDutyLimit(0.5); + motor_lo.setDutyLimit(0.7); + motor_li.setDutyLimit(0.7); for(int i=0; i<3; ++i) { while(get_dist_forward() > 60) { - straightAndInfinity(90.0, 5.0); + if(RightOrLeft == 0) straightAndInfinity(90.0, 7.0); + else straightAndInfinity(-90.0, 7.0); } } //ロープ前旋回 @@ -104,45 +129,98 @@ turn(0); //ロープ位置ジャストまで前進 for(int i=0; i<3; ++i) { - straightAndInfinity(0.0, 5.0); + straightAndInfinity(0.0, 7.0); } for(int i=0; i<2; ++i) { while(get_dist_back() < 100) { - straightAndInfinity(0.0, 5.0); + straightAndInfinity(0.0, 7.0); } } phasing(0.0); //ロープ - while(mode4.read() == 0) { - straightAndInfinity(0, 5); + while(switch_modes[2].read() == 0) { + straightAndInfinity(0, 7); //straight(); } + printf("uhai stand by ok!!!!!!!!!!\r\n"); + NoHandSignal(); case 3://坂 - printf("uhai stand by ok!!!!!!!!!!\r\n"); - while(get_dist_back() > 40.0) { - wait(0.01); + printf("last spart!!!!!!!!"); + + if(RightOrLeft == 0) theta0 = -degree/100.0+90; + else theta0 = -degree/100.0-90; + motor_lo.setDutyLimit(0.2);//0.3 + motor_li.setDutyLimit(0.2); + for(int i=0; i<20; ++i) + { + if(RightOrLeft == 0) straightAndInfinity(-90, 15);//straight();//climbAndInfinity(-90,5); + else straightAndInfinity(90, 15); } - while(get_dist_back() < 40.0) { - wait(0.01); - } - printf("last spart!!!!!!!!"); - theta0 = -degree/100.0+90; - motor_lo.setDutyLimit(0.3);//0.3 - motor_li.setDutyLimit(0.3); - for(int i=0; i<20; ++i)straightAndInfinity(-90, 5);//straight();//climbAndInfinity(-90,5); printf("wall standby"); while(get_dist_back() < 250) { // straight(); - climbAndInfinity(-90,5); + if(RightOrLeft == 0) climbAndInfinity(-90,15); + else climbAndInfinity(90,15); } for(int i=0; i<1; ++i) { while(get_dist_forward() > 60) { //straight(); - climbAndInfinity(-90,5); + if(RightOrLeft == 0) climbAndInfinity(-90,15); + else climbAndInfinity(90,15); } } hand_mode = GOAL; stop(); - printf("uhai!!!!!!!!!!!!!!!"); + printf("uhai!!!!!!!!!!!!!!!/r/n"); + } +} +void Start() +{ + if(hand.read()==0)//開始時すでにスイッチが押されていた場合 + { + + SetMode(); + NoHandSignal(); + } + else + { + SetMode(); + wait_gerege(); + hand_mode = GEREGE; + HandMove(); + wait(0.5); + } +} +void SetMode() +{ + while(get_dist_back() > 50.0) + { + led1 = led2 = led3 = led4 = 0; + if(switch_modes[2].read()) start_state=3; + else if(switch_modes[1].read()) start_state=2; + else if(switch_modes[0].read()) start_state=1; + else start_state=0; + + if(switch_LR.read()) RightOrLeft = 1; + else RightOrLeft = 0; + + if(start_state==0)led4 = 1; + else if(start_state==1)led3 = 1; + else if(start_state==2)led2 = 1; + else if(start_state==3)led1 = 1; + + } + printf("mode is %d RightOrLeft is %d \r\n", start_state, RightOrLeft); + led1 = led2 = led3 = led4 = 1; +} +void NoHandSignal() +{ + while(get_dist_back() > 40.0) + { + wait(0.01); + } + while(get_dist_back() < 40.0) + { + wait(0.01); } } \ No newline at end of file