ROBOSTEP_5期 / Mbed 2 deprecated George_Master_BOTHMOVE

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.

Dependencies:   mbed robot

main.cpp@30:231e6584afe9, 2019-05-08 (annotated)

Committer:
shimizuta
Date:
Wed May 08 09:16:30 2019 +0000
Revision:
30:231e6584afe9
Parent:
28:c92912036b03
Child:
31:ed4249a288d7
debug

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eri 0:c1476d342c13 1 #include "mbed.h"
shimizuta 17:2b3fa9b1a05b 2 #include "debug.h"
eri 0:c1476d342c13 3 #include "microinfinity.h"
shimizuta 21:14133581387b 4 #include "sensors.h"
shimizuta 22:0ed9de464f40 5 #include "moves.h"
shimizuta 21:14133581387b 6
yuto17320508 25:c740e6fd5dab 7 void StateFlow(int i);
shimizuta 26:5fb1aa9cb7f0 8
kageyuta 1:86c4c38abe40 9 int main()
kageyuta 1:86c4c38abe40 10 {
shimizuta 26:5fb1aa9cb7f0 11 //setup関連
shimizuta 22:0ed9de464f40 12 can1.frequency(1000000);
shimizuta 22:0ed9de464f40 13 SetupMoves();
yuto17320508 25:c740e6fd5dab 14 set_gyro();
yuto17320508 25:c740e6fd5dab 15 int start_state=0;
shimizuta 22:0ed9de464f40 16 printf("reset standby\r\n");
shimizuta 26:5fb1aa9cb7f0 17 reset();//足をリセット位置に移動
shimizuta 22:0ed9de464f40 18 printf("bus standby\r\n");
yuto17320508 5:63462d696256 19 while(1) {
yuto17320508 5:63462d696256 20 if(bus_in.read() == 1) break;
yuto17320508 3:7a608fbd3bcd 21 }
shimizuta 22:0ed9de464f40 22 printf("bus is %d\r\n", bus_in.read());
yuto17320508 12:9a5de6adae9c 23 wait(0.5);
shimizuta 26:5fb1aa9cb7f0 24 //一歩動かして初期位置へ
yuto17320508 25:c740e6fd5dab 25 motor_lo.setDutyLimit(0.1);
yuto17320508 25:c740e6fd5dab 26 motor_li.setDutyLimit(0.1);
yuto17320508 12:9a5de6adae9c 27 straight();
shimizuta 26:5fb1aa9cb7f0 28 // mode選択
yuto17320508 25:c740e6fd5dab 29 while(mode4.read() == 1) {
yuto17320508 25:c740e6fd5dab 30 start_state = (get_dist_back()-10)/10;
yuto17320508 28:c92912036b03 31 if(start_state>6 || start_state<0) start_state = 0;
yuto17320508 25:c740e6fd5dab 32 int tmp = start_state;
yuto17320508 25:c740e6fd5dab 33 led2 = start_state/4;
yuto17320508 25:c740e6fd5dab 34 start_state = start_state%4;
yuto17320508 25:c740e6fd5dab 35 led3 = start_state/2;
yuto17320508 25:c740e6fd5dab 36 start_state = start_state%2;
yuto17320508 25:c740e6fd5dab 37 led4 = start_state;
yuto17320508 25:c740e6fd5dab 38 start_state = tmp;
yuto17320508 25:c740e6fd5dab 39 }
shimizuta 26:5fb1aa9cb7f0 40 printf("mode is %d\r\n", start_state);
yuto17320508 12:9a5de6adae9c 41 wait_gerege();
yuto17320508 12:9a5de6adae9c 42 hand_mode = GEREGE;
shimizuta 27:d392a95f4799 43 HandMove();
shimizuta 27:d392a95f4799 44 wait(0.5);
yuto17320508 25:c740e6fd5dab 45 if(start_state == 0) theta0 = -degree/100.0;
yuto17320508 25:c740e6fd5dab 46 else if(start_state == 1) theta0 = -degree/100.0 - 45;
yuto17320508 25:c740e6fd5dab 47 else if(start_state == 2) theta0 = -degree/100.0 - 90;
yuto17320508 25:c740e6fd5dab 48 else if(start_state == 3) theta0 = -degree/100.0 + 90;
yuto17320508 25:c740e6fd5dab 49 else printf("state_error");
shimizuta 26:5fb1aa9cb7f0 50 FileOpen();
yuto17320508 25:c740e6fd5dab 51 StateFlow(start_state);
shimizuta 26:5fb1aa9cb7f0 52 FileClose();
yuto17320508 25:c740e6fd5dab 53 }
yuto17320508 25:c740e6fd5dab 54
yuto17320508 25:c740e6fd5dab 55 void StateFlow(int i)
yuto17320508 25:c740e6fd5dab 56 {
yuto17320508 25:c740e6fd5dab 57 switch(i) {
yuto17320508 25:c740e6fd5dab 58 case 0://最初の直線
yuto17320508 25:c740e6fd5dab 59 printf("go straight!!!!!!!!!!\r\n");
shimizuta 27:d392a95f4799 60 motor_lo.setDutyLimit(0.5);
shimizuta 27:d392a95f4799 61 motor_li.setDutyLimit(0.5);
yuto17320508 25:c740e6fd5dab 62 for(int i = 0; i < 25; i++) {
yuto17320508 25:c740e6fd5dab 63 straightAndInfinity(0, 3);
yuto17320508 25:c740e6fd5dab 64 }
yuto17320508 25:c740e6fd5dab 65 printf("get distance mode");
yuto17320508 25:c740e6fd5dab 66 for(int i=0; i<1; ++i) {
shimizuta 27:d392a95f4799 67 while(get_dist_back() < 290) {//320
yuto17320508 25:c740e6fd5dab 68 straightAndInfinity(0, 3);
yuto17320508 25:c740e6fd5dab 69 }
yuto17320508 25:c740e6fd5dab 70 }
yuto17320508 25:c740e6fd5dab 71 //段差前旋回
yuto17320508 25:c740e6fd5dab 72 motor_lo.setDutyLimit(0.5);//0.5
yuto17320508 25:c740e6fd5dab 73 motor_li.setDutyLimit(0.5);
yuto17320508 25:c740e6fd5dab 74 turn(35.0);
yuto17320508 25:c740e6fd5dab 75 case 1://段差前
yuto17320508 25:c740e6fd5dab 76 printf("climb!!!!!!!!!!\r\n");
yuto17320508 25:c740e6fd5dab 77 //段差乗り越え
shimizuta 27:d392a95f4799 78 motor_lo.setDutyLimit(0.2);//0.5
shimizuta 27:d392a95f4799 79 motor_li.setDutyLimit(0.2);
yuto17320508 25:c740e6fd5dab 80 while(get_dist_forward() < 60) {
yuto17320508 25:c740e6fd5dab 81 straightAndInfinity(45, 5);
yuto17320508 25:c740e6fd5dab 82 }
yuto17320508 25:c740e6fd5dab 83 for(int i= 0; i<14; ++i) straight();
yuto17320508 25:c740e6fd5dab 84 while(get_dist_back() > 80) straight();
yuto17320508 25:c740e6fd5dab 85 //段差後旋回
yuto17320508 25:c740e6fd5dab 86 printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
shimizuta 27:d392a95f4799 87 motor_lo.setDutyLimit(0.4);//0.5
shimizuta 27:d392a95f4799 88 motor_li.setDutyLimit(0.4);
yuto17320508 25:c740e6fd5dab 89 turn(90.0);
yuto17320508 25:c740e6fd5dab 90 case 2://段差直後
yuto17320508 25:c740e6fd5dab 91 printf("go lope!!!!!!!!!!\r\n");
yuto17320508 25:c740e6fd5dab 92 printf("angle:%.3f back dist:%.2f\r\n", degree0, get_dist_back());
yuto17320508 25:c740e6fd5dab 93 //前進
shimizuta 27:d392a95f4799 94 motor_lo.setDutyLimit(0.5);
shimizuta 27:d392a95f4799 95 motor_li.setDutyLimit(0.5);
yuto17320508 25:c740e6fd5dab 96 for(int i=0; i<3; ++i) {
yuto17320508 25:c740e6fd5dab 97 while(get_dist_forward() > 60) {
yuto17320508 25:c740e6fd5dab 98 straightAndInfinity(90.0, 5.0);
yuto17320508 25:c740e6fd5dab 99 }
yuto17320508 25:c740e6fd5dab 100 }
yuto17320508 25:c740e6fd5dab 101 //ロープ前旋回
yuto17320508 25:c740e6fd5dab 102 printf("before roop deg:%.3f\r\n",degree0);
yuto17320508 25:c740e6fd5dab 103 turn(0);
yuto17320508 25:c740e6fd5dab 104 //ロープ位置ジャストまで前進
yuto17320508 25:c740e6fd5dab 105 for(int i=0; i<3; ++i) {
yuto17320508 25:c740e6fd5dab 106 straightAndInfinity(0.0, 5.0);
yuto17320508 25:c740e6fd5dab 107 }
yuto17320508 25:c740e6fd5dab 108 for(int i=0; i<2; ++i) {
yuto17320508 25:c740e6fd5dab 109 while(get_dist_back() < 100) {
yuto17320508 25:c740e6fd5dab 110 straightAndInfinity(0.0, 5.0);
yuto17320508 25:c740e6fd5dab 111 }
yuto17320508 25:c740e6fd5dab 112 }
yuto17320508 25:c740e6fd5dab 113 //ロープ
yuto17320508 25:c740e6fd5dab 114 while(mode4.read() == 0) {
yuto17320508 25:c740e6fd5dab 115 straightAndInfinity(0, 5);
yuto17320508 25:c740e6fd5dab 116 //straight();
yuto17320508 25:c740e6fd5dab 117 }
yuto17320508 25:c740e6fd5dab 118 case 3://坂
yuto17320508 25:c740e6fd5dab 119 printf("uhai stand by ok!!!!!!!!!!\r\n");
yuto17320508 25:c740e6fd5dab 120 while(get_dist_back() > 40.0) {
yuto17320508 25:c740e6fd5dab 121 wait(0.01);
yuto17320508 25:c740e6fd5dab 122 }
yuto17320508 25:c740e6fd5dab 123 while(get_dist_back() < 40.0) {
yuto17320508 25:c740e6fd5dab 124 wait(0.01);
yuto17320508 25:c740e6fd5dab 125 }
yuto17320508 25:c740e6fd5dab 126 printf("last spart!!!!!!!!");
yuto17320508 25:c740e6fd5dab 127 theta0 = -degree/100.0+90;
yuto17320508 25:c740e6fd5dab 128 motor_lo.setDutyLimit(0.3);//0.3
yuto17320508 25:c740e6fd5dab 129 motor_li.setDutyLimit(0.3);
shimizuta 27:d392a95f4799 130 for(int i=0; i<15; ++i)straightAndInfinity(-90, 5);//straight();//climbAndInfinity(-90,5);
yuto17320508 25:c740e6fd5dab 131 printf("wall standby");
yuto17320508 25:c740e6fd5dab 132 while(get_dist_back() < 250) {
shimizuta 27:d392a95f4799 133 // straight();
yuto17320508 25:c740e6fd5dab 134 climbAndInfinity(-90,5);
yuto17320508 25:c740e6fd5dab 135 }
yuto17320508 25:c740e6fd5dab 136 for(int i=0; i<1; ++i) {
yuto17320508 25:c740e6fd5dab 137 while(get_dist_forward() > 70) {
yuto17320508 25:c740e6fd5dab 138 //straight();
yuto17320508 25:c740e6fd5dab 139 climbAndInfinity(-90,5);
yuto17320508 25:c740e6fd5dab 140 }
yuto17320508 25:c740e6fd5dab 141 }
yuto17320508 25:c740e6fd5dab 142 hand_mode = GOAL;
shimizuta 30:231e6584afe9 143 stop();
yuto17320508 25:c740e6fd5dab 144 printf("uhai!!!!!!!!!!!!!!!");
shimizuta 22:0ed9de464f40 145 }
shimizuta 22:0ed9de464f40 146 }