ROBOSTEP_5期 / Mbed 2 deprecated George_Master_Param

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@47:b77796718a9b, 2019-05-17 (annotated)

Committer:
shimizuta
Date:
Fri May 17 10:46:05 2019 +0000
Revision:
47:b77796718a9b
Parent:
46:c58335028928 
unsync

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);
yuto17320508 32:aee87dcaf7ca 8 void SetMode();
yuto17320508 32:aee87dcaf7ca 9 void Start();
yuto17320508 32:aee87dcaf7ca 10 void NoHandSignal();
yuto17320508 33:2dbbe198adaf 11
yuto17320508 32:aee87dcaf7ca 12 int start_state=0;
kageyuta 1:86c4c38abe40 13 int main()
kageyuta 1:86c4c38abe40 14 {
shimizuta 26:5fb1aa9cb7f0 15 //setup関連
shimizuta 22:0ed9de464f40 16 can1.frequency(1000000);
shimizuta 22:0ed9de464f40 17 SetupMoves();
yuto17320508 25:c740e6fd5dab 18 set_gyro();
yuto17320508 32:aee87dcaf7ca 19 printf("hand read:%d\r\n",hand.read());
shimizuta 22:0ed9de464f40 20 printf("reset standby\r\n");
shimizuta 26:5fb1aa9cb7f0 21 reset();//足をリセット位置に移動
shimizuta 22:0ed9de464f40 22 printf("bus standby\r\n");
yuto17320508 5:63462d696256 23 while(1) {
yuto17320508 5:63462d696256 24 if(bus_in.read() == 1) break;
yuto17320508 3:7a608fbd3bcd 25 }
shimizuta 22:0ed9de464f40 26 printf("bus is %d\r\n", bus_in.read());
yuto17320508 12:9a5de6adae9c 27 wait(0.5);
shimizuta 26:5fb1aa9cb7f0 28 //一歩動かして初期位置へ
yuto17320508 25:c740e6fd5dab 29 motor_lo.setDutyLimit(0.1);
yuto17320508 25:c740e6fd5dab 30 motor_li.setDutyLimit(0.1);
yuto17320508 12:9a5de6adae9c 31 straight();
yuto17320508 38:89d2a9e6c96f 32
yuto17320508 38:89d2a9e6c96f 33
shimizuta 26:5fb1aa9cb7f0 34 // mode選択
yuto17320508 32:aee87dcaf7ca 35 Start();
yuto17320508 32:aee87dcaf7ca 36 //SetMode();
yuto17320508 32:aee87dcaf7ca 37
yuto17320508 38:89d2a9e6c96f 38 if(RightOrLeft == 0) {
yuto17320508 32:aee87dcaf7ca 39 if(start_state == 0) theta0 = -degree/100.0;
yuto17320508 32:aee87dcaf7ca 40 else if(start_state == 1) theta0 = -degree/100.0 - 45;
yuto17320508 32:aee87dcaf7ca 41 else if(start_state == 2) theta0 = -degree/100.0 - 90;
yuto17320508 32:aee87dcaf7ca 42 else if(start_state == 3) theta0 = -degree/100.0 + 90;
yuto17320508 32:aee87dcaf7ca 43 else printf("state_error");
yuto17320508 38:89d2a9e6c96f 44 } else {
yuto17320508 32:aee87dcaf7ca 45 if(start_state == 0) theta0 = -degree/100.0;
yuto17320508 32:aee87dcaf7ca 46 else if(start_state == 1) theta0 = -degree/100.0 + 45;
yuto17320508 32:aee87dcaf7ca 47 else if(start_state == 2) theta0 = -degree/100.0 + 90;
yuto17320508 32:aee87dcaf7ca 48 else if(start_state == 3) theta0 = -degree/100.0 - 90;
yuto17320508 32:aee87dcaf7ca 49 else printf("state_error");
yuto17320508 32:aee87dcaf7ca 50 }
yuto17320508 33:2dbbe198adaf 51 FileOpen();
yuto17320508 32:aee87dcaf7ca 52 printf("LR:%d StateFlow:%d\r\n", RightOrLeft,start_state);
yuto17320508 25:c740e6fd5dab 53 StateFlow(start_state);
yuto17320508 33:2dbbe198adaf 54 FileClose();
yuto17320508 25:c740e6fd5dab 55 }
yuto17320508 25:c740e6fd5dab 56
yuto17320508 25:c740e6fd5dab 57 void StateFlow(int i)
yuto17320508 25:c740e6fd5dab 58 {
yuto17320508 25:c740e6fd5dab 59 switch(i) {
yuto17320508 25:c740e6fd5dab 60 case 0://最初の直線
yuto17320508 25:c740e6fd5dab 61 printf("go straight!!!!!!!!!!\r\n");
yuto17320508 38:89d2a9e6c96f 62 motor_lo.setDutyLimit(0.6);
yuto17320508 38:89d2a9e6c96f 63 motor_li.setDutyLimit(0.6);
yuto17320508 32:aee87dcaf7ca 64 for(int i = 0; i < 30; i++) {
shimizuta 47:b77796718a9b 65 straight(UNSYNC);
shimizuta 47:b77796718a9b 66 checkUW(0, degree0, ec_lo.getCount());
yuto17320508 25:c740e6fd5dab 67 }
yuto17320508 25:c740e6fd5dab 68 printf("get distance mode");
yuto17320508 25:c740e6fd5dab 69 for(int i=0; i<1; ++i) {
shimizuta 45:6df20a8f4b21 70 while(Hcsr04BackWithEc() < 340) {//300
yuto17320508 32:aee87dcaf7ca 71 straightAndInfinity(0, 7);
yuto17320508 25:c740e6fd5dab 72 }
yuto17320508 25:c740e6fd5dab 73 }
yuto17320508 25:c740e6fd5dab 74 //段差前旋回
yuto17320508 25:c740e6fd5dab 75 motor_lo.setDutyLimit(0.5);//0.5
yuto17320508 25:c740e6fd5dab 76 motor_li.setDutyLimit(0.5);
yuto17320508 38:89d2a9e6c96f 77
yuto17320508 32:aee87dcaf7ca 78 if(RightOrLeft == 0) turn(35.0);
yuto17320508 32:aee87dcaf7ca 79 else turn(-35.0);
yuto17320508 25:c740e6fd5dab 80 case 1://段差前
yuto17320508 25:c740e6fd5dab 81 printf("climb!!!!!!!!!!\r\n");
yuto17320508 38:89d2a9e6c96f 82 //段差乗り越え
yuto17320508 25:c740e6fd5dab 83 while(get_dist_forward() < 60) {
yuto17320508 32:aee87dcaf7ca 84 if(RightOrLeft == 0) straightAndInfinity(45, 5);
yuto17320508 32:aee87dcaf7ca 85 else straightAndInfinity(-45, 5);
yuto17320508 25:c740e6fd5dab 86 }
yuto17320508 32:aee87dcaf7ca 87 if(RightOrLeft == 0) phasing(45.0);
yuto17320508 32:aee87dcaf7ca 88 else phasing(-45.0);
yuto17320508 38:89d2a9e6c96f 89
yuto17320508 38:89d2a9e6c96f 90 motor_lo.setDutyLimit(0.2);//0.5
yuto17320508 38:89d2a9e6c96f 91 motor_li.setDutyLimit(0.2);
yuto17320508 38:89d2a9e6c96f 92
yuto17320508 25:c740e6fd5dab 93 for(int i= 0; i<14; ++i) straight();
yuto17320508 25:c740e6fd5dab 94 while(get_dist_back() > 80) straight();
yuto17320508 25:c740e6fd5dab 95 //段差後旋回
yuto17320508 32:aee87dcaf7ca 96 motor_lo.setDutyLimit(0.5);//0.5
yuto17320508 32:aee87dcaf7ca 97 motor_li.setDutyLimit(0.5);
yuto17320508 38:89d2a9e6c96f 98
yuto17320508 32:aee87dcaf7ca 99 if(RightOrLeft == 0) turn(85.0);
yuto17320508 32:aee87dcaf7ca 100 else turn(-85.0);
yuto17320508 38:89d2a9e6c96f 101
yuto17320508 25:c740e6fd5dab 102 case 2://段差直後
yuto17320508 25:c740e6fd5dab 103 printf("go lope!!!!!!!!!!\r\n");
yuto17320508 25:c740e6fd5dab 104 //前進
yuto17320508 38:89d2a9e6c96f 105 motor_lo.setDutyLimit(0.6);
yuto17320508 38:89d2a9e6c96f 106 motor_li.setDutyLimit(0.6);
yuto17320508 38:89d2a9e6c96f 107 for(int i=0; i<13; ++i) {
yuto17320508 38:89d2a9e6c96f 108 if(RightOrLeft == 0) straightAndInfinity(90.0, 7.0);
yuto17320508 38:89d2a9e6c96f 109 else straightAndInfinity(-90.0, 7.0);
yuto17320508 38:89d2a9e6c96f 110 }
yuto17320508 25:c740e6fd5dab 111 for(int i=0; i<3; ++i) {
yuto17320508 25:c740e6fd5dab 112 while(get_dist_forward() > 60) {
yuto17320508 32:aee87dcaf7ca 113 if(RightOrLeft == 0) straightAndInfinity(90.0, 7.0);
yuto17320508 32:aee87dcaf7ca 114 else straightAndInfinity(-90.0, 7.0);
yuto17320508 25:c740e6fd5dab 115 }
yuto17320508 25:c740e6fd5dab 116 }
yuto17320508 25:c740e6fd5dab 117 //ロープ前旋回
yuto17320508 25:c740e6fd5dab 118 printf("before roop deg:%.3f\r\n",degree0);
yuto17320508 25:c740e6fd5dab 119 turn(0);
yuto17320508 25:c740e6fd5dab 120 //ロープ位置ジャストまで前進
yuto17320508 25:c740e6fd5dab 121 for(int i=0; i<3; ++i) {
yuto17320508 32:aee87dcaf7ca 122 straightAndInfinity(0.0, 7.0);
yuto17320508 25:c740e6fd5dab 123 }
yuto17320508 25:c740e6fd5dab 124 for(int i=0; i<2; ++i) {
yuto17320508 25:c740e6fd5dab 125 while(get_dist_back() < 100) {
yuto17320508 32:aee87dcaf7ca 126 straightAndInfinity(0.0, 7.0);
yuto17320508 25:c740e6fd5dab 127 }
yuto17320508 25:c740e6fd5dab 128 }
yuto17320508 31:ed4249a288d7 129 phasing(0.0);
yuto17320508 25:c740e6fd5dab 130 //ロープ
yuto17320508 32:aee87dcaf7ca 131 while(switch_modes[2].read() == 0) {
yuto17320508 32:aee87dcaf7ca 132 straightAndInfinity(0, 7);
yuto17320508 25:c740e6fd5dab 133 //straight();
yuto17320508 25:c740e6fd5dab 134 }
yuto17320508 32:aee87dcaf7ca 135 printf("uhai stand by ok!!!!!!!!!!\r\n");
yuto17320508 32:aee87dcaf7ca 136 NoHandSignal();
yuto17320508 25:c740e6fd5dab 137 case 3://坂
yuto17320508 32:aee87dcaf7ca 138 printf("last spart!!!!!!!!");
yuto17320508 38:89d2a9e6c96f 139
yuto17320508 32:aee87dcaf7ca 140 if(RightOrLeft == 0) theta0 = -degree/100.0+90;
yuto17320508 32:aee87dcaf7ca 141 else theta0 = -degree/100.0-90;
yuto17320508 32:aee87dcaf7ca 142 motor_lo.setDutyLimit(0.2);//0.3
yuto17320508 32:aee87dcaf7ca 143 motor_li.setDutyLimit(0.2);
yuto17320508 38:89d2a9e6c96f 144 for(int i=0; i<20; ++i) {
yuto17320508 32:aee87dcaf7ca 145 if(RightOrLeft == 0) straightAndInfinity(-90, 15);//straight();//climbAndInfinity(-90,5);
yuto17320508 32:aee87dcaf7ca 146 else straightAndInfinity(90, 15);
yuto17320508 25:c740e6fd5dab 147 }
yuto17320508 25:c740e6fd5dab 148 printf("wall standby");
yuto17320508 33:2dbbe198adaf 149 motor_lo.setDutyLimit(0.3);//0.3
yuto17320508 33:2dbbe198adaf 150 motor_li.setDutyLimit(0.3);
yuto17320508 25:c740e6fd5dab 151 while(get_dist_back() < 250) {
shimizuta 27:d392a95f4799 152 // straight();
yuto17320508 32:aee87dcaf7ca 153 if(RightOrLeft == 0) climbAndInfinity(-90,15);
yuto17320508 32:aee87dcaf7ca 154 else climbAndInfinity(90,15);
yuto17320508 25:c740e6fd5dab 155 }
yuto17320508 25:c740e6fd5dab 156 for(int i=0; i<1; ++i) {
yuto17320508 31:ed4249a288d7 157 while(get_dist_forward() > 60) {
yuto17320508 25:c740e6fd5dab 158 //straight();
yuto17320508 32:aee87dcaf7ca 159 if(RightOrLeft == 0) climbAndInfinity(-90,15);
yuto17320508 32:aee87dcaf7ca 160 else climbAndInfinity(90,15);
yuto17320508 25:c740e6fd5dab 161 }
yuto17320508 25:c740e6fd5dab 162 }
yuto17320508 25:c740e6fd5dab 163 hand_mode = GOAL;
shimizuta 30:231e6584afe9 164 stop();
yuto17320508 32:aee87dcaf7ca 165 printf("uhai!!!!!!!!!!!!!!!/r/n");
yuto17320508 32:aee87dcaf7ca 166 }
yuto17320508 32:aee87dcaf7ca 167 }
yuto17320508 32:aee87dcaf7ca 168 void Start()
yuto17320508 32:aee87dcaf7ca 169 {
yuto17320508 38:89d2a9e6c96f 170 if(hand.read()==0) { //開始時すでにスイッチが押されていた場合
yuto17320508 38:89d2a9e6c96f 171
yuto17320508 32:aee87dcaf7ca 172 SetMode();
yuto17320508 35:04699b49c463 173 hand_mode = G_CLOSE;
yuto17320508 32:aee87dcaf7ca 174 NoHandSignal();
yuto17320508 38:89d2a9e6c96f 175 } else {
yuto17320508 32:aee87dcaf7ca 176 SetMode();
yuto17320508 35:04699b49c463 177 hand_mode = G_OPEN;
yuto17320508 35:04699b49c463 178 stop();//ここで開くことをcanでslaveに送る
shimizuta 45:6df20a8f4b21 179 wait(1);
yuto17320508 32:aee87dcaf7ca 180 wait_gerege();
yuto17320508 35:04699b49c463 181 hand_mode = G_CLOSE;
yuto17320508 32:aee87dcaf7ca 182 HandMove();
yuto17320508 32:aee87dcaf7ca 183 wait(0.5);
yuto17320508 38:89d2a9e6c96f 184 }
yuto17320508 32:aee87dcaf7ca 185 }
yuto17320508 32:aee87dcaf7ca 186 void SetMode()
yuto17320508 32:aee87dcaf7ca 187 {
yuto17320508 38:89d2a9e6c96f 188 while(get_dist_back() > 50.0) {
yuto17320508 32:aee87dcaf7ca 189 led1 = led2 = led3 = led4 = 0;
yuto17320508 32:aee87dcaf7ca 190 if(switch_modes[2].read()) start_state=3;
yuto17320508 32:aee87dcaf7ca 191 else if(switch_modes[1].read()) start_state=2;
yuto17320508 32:aee87dcaf7ca 192 else if(switch_modes[0].read()) start_state=1;
yuto17320508 32:aee87dcaf7ca 193 else start_state=0;
yuto17320508 38:89d2a9e6c96f 194
yuto17320508 38:89d2a9e6c96f 195 if(switch_LR.read()) RightOrLeft = 1;
yuto17320508 38:89d2a9e6c96f 196 else RightOrLeft = 0;
yuto17320508 38:89d2a9e6c96f 197
yuto17320508 32:aee87dcaf7ca 198 if(start_state==0)led4 = 1;
yuto17320508 32:aee87dcaf7ca 199 else if(start_state==1)led3 = 1;
yuto17320508 32:aee87dcaf7ca 200 else if(start_state==2)led2 = 1;
yuto17320508 32:aee87dcaf7ca 201 else if(start_state==3)led1 = 1;
yuto17320508 38:89d2a9e6c96f 202
yuto17320508 32:aee87dcaf7ca 203 }
yuto17320508 32:aee87dcaf7ca 204 printf("mode is %d RightOrLeft is %d \r\n", start_state, RightOrLeft);
yuto17320508 32:aee87dcaf7ca 205 led1 = led2 = led3 = led4 = 1;
yuto17320508 32:aee87dcaf7ca 206 }
yuto17320508 32:aee87dcaf7ca 207 void NoHandSignal()
yuto17320508 32:aee87dcaf7ca 208 {
yuto17320508 38:89d2a9e6c96f 209 while(get_dist_back() > 40.0) {
yuto17320508 32:aee87dcaf7ca 210 wait(0.01);
yuto17320508 32:aee87dcaf7ca 211 }
yuto17320508 38:89d2a9e6c96f 212 while(get_dist_back() < 40.0) {
yuto17320508 32:aee87dcaf7ca 213 wait(0.01);
shimizuta 22:0ed9de464f40 214 }
shimizuta 22:0ed9de464f40 215 }