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@33:2dbbe198adaf, 2019-05-13 (annotated)

Committer:
yuto17320508
Date:
Mon May 13 09:02:19 2019 +0000
Revision:
33:2dbbe198adaf
Parent:
32:aee87dcaf7ca
Child:
34:0a8ae7f92262
p

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