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@25:c740e6fd5dab, 2019-05-06 (annotated)

Committer:
yuto17320508
Date:
Mon May 06 04:03:00 2019 +0000
Revision:
25:c740e6fd5dab
Parent:
24:9ee1440c6703
Child:
26:5fb1aa9cb7f0
l

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);
kageyuta 1:86c4c38abe40 8 int main()
kageyuta 1:86c4c38abe40 9 {
shimizuta 22:0ed9de464f40 10 can1.frequency(1000000);
shimizuta 22:0ed9de464f40 11 SetupMoves();
yuto17320508 25:c740e6fd5dab 12 set_gyro();
yuto17320508 25:c740e6fd5dab 13 int start_state=0;
yuto17320508 25:c740e6fd5dab 14
shimizuta 22:0ed9de464f40 15 printf("reset standby\r\n");
yuto17320508 25:c740e6fd5dab 16
yuto17320508 25:c740e6fd5dab 17 /*while(1) {
yuto17320508 25:c740e6fd5dab 18 printf("forward:%.3f back:%.3f\r\n", get_dist_forward(), get_dist_back());
yuto17320508 25:c740e6fd5dab 19 wait(0.01);
yuto17320508 25:c740e6fd5dab 20 }*/
yuto17320508 25:c740e6fd5dab 21
yuto17320508 25:c740e6fd5dab 22 reset();
yuto17320508 25:c740e6fd5dab 23
shimizuta 22:0ed9de464f40 24 /*
yuto17320508 25:c740e6fd5dab 25
yuto17320508 25:c740e6fd5dab 26 while(1)
yuto17320508 25:c740e6fd5dab 27 {
yuto17320508 25:c740e6fd5dab 28 motor_lo.setDutyLimit(0.3);
yuto17320508 25:c740e6fd5dab 29 motor_li.setDutyLimit(0.3);
yuto17320508 25:c740e6fd5dab 30 onestraight();
shimizuta 22:0ed9de464f40 31 }
shimizuta 22:0ed9de464f40 32 */
yuto17320508 25:c740e6fd5dab 33
shimizuta 22:0ed9de464f40 34 printf("bus standby\r\n");
yuto17320508 5:63462d696256 35 while(1) {
yuto17320508 5:63462d696256 36 if(bus_in.read() == 1) break;
yuto17320508 3:7a608fbd3bcd 37 }
shimizuta 22:0ed9de464f40 38 printf("bus is %d\r\n", bus_in.read());
yuto17320508 12:9a5de6adae9c 39 wait(0.5);
yuto17320508 25:c740e6fd5dab 40 motor_lo.setDutyLimit(0.1);
yuto17320508 25:c740e6fd5dab 41 motor_li.setDutyLimit(0.1);
yuto17320508 12:9a5de6adae9c 42 straight();
yuto17320508 25:c740e6fd5dab 43
yuto17320508 25:c740e6fd5dab 44 while(mode4.read() == 1) {
yuto17320508 25:c740e6fd5dab 45 start_state = (get_dist_back()-10)/10;
yuto17320508 25:c740e6fd5dab 46 int tmp = start_state;
yuto17320508 25:c740e6fd5dab 47 if(start_state>6 || start_state<0) start_state = 0;
yuto17320508 25:c740e6fd5dab 48 //printf("%d\r\n",start_state);
yuto17320508 25:c740e6fd5dab 49 led2 = start_state/4;
yuto17320508 25:c740e6fd5dab 50 start_state = start_state%4;
yuto17320508 25:c740e6fd5dab 51 led3 = start_state/2;
yuto17320508 25:c740e6fd5dab 52 start_state = start_state%2;
yuto17320508 25:c740e6fd5dab 53 led4 = start_state;
yuto17320508 25:c740e6fd5dab 54 start_state = tmp;
yuto17320508 25:c740e6fd5dab 55
yuto17320508 25:c740e6fd5dab 56 }
yuto17320508 25:c740e6fd5dab 57
yuto17320508 12:9a5de6adae9c 58 wait_gerege();
yuto17320508 12:9a5de6adae9c 59 hand_mode = GEREGE;
yuto17320508 25:c740e6fd5dab 60 wait(1.0);
yuto17320508 25:c740e6fd5dab 61
yuto17320508 25:c740e6fd5dab 62
shimizuta 18:d0a6771fa38d 63 //直進スタート
yuto17320508 25:c740e6fd5dab 64 /*while(1)
yuto17320508 25:c740e6fd5dab 65 {
yuto17320508 25:c740e6fd5dab 66 printf("angle:%.3f\r\n", degree0);
yuto17320508 25:c740e6fd5dab 67 wait(0.1);
yuto17320508 25:c740e6fd5dab 68 }*/
yuto17320508 25:c740e6fd5dab 69
yuto17320508 25:c740e6fd5dab 70 /*
yuto17320508 25:c740e6fd5dab 71 motor_lo.setDutyLimit(0.2);
yuto17320508 25:c740e6fd5dab 72 motor_li.setDutyLimit(0.2);
yuto17320508 25:c740e6fd5dab 73 while(1)
yuto17320508 25:c740e6fd5dab 74 {
yuto17320508 25:c740e6fd5dab 75 backLeft();
yuto17320508 25:c740e6fd5dab 76 }*/
yuto17320508 25:c740e6fd5dab 77
yuto17320508 25:c740e6fd5dab 78
yuto17320508 25:c740e6fd5dab 79
yuto17320508 25:c740e6fd5dab 80 if(start_state == 0) theta0 = -degree/100.0;
yuto17320508 25:c740e6fd5dab 81 else if(start_state == 1) theta0 = -degree/100.0 - 45;
yuto17320508 25:c740e6fd5dab 82 else if(start_state == 2) theta0 = -degree/100.0 - 90;
yuto17320508 25:c740e6fd5dab 83 else if(start_state == 3) theta0 = -degree/100.0 + 90;
yuto17320508 25:c740e6fd5dab 84 else printf("state_error");
yuto17320508 25:c740e6fd5dab 85
yuto17320508 25:c740e6fd5dab 86 StateFlow(start_state);
yuto17320508 25:c740e6fd5dab 87
yuto17320508 25:c740e6fd5dab 88 }
yuto17320508 25:c740e6fd5dab 89
yuto17320508 25:c740e6fd5dab 90 void StateFlow(int i)
yuto17320508 25:c740e6fd5dab 91 {
yuto17320508 25:c740e6fd5dab 92 switch(i) {
yuto17320508 25:c740e6fd5dab 93 case 0://最初の直線
yuto17320508 25:c740e6fd5dab 94 printf("go straight!!!!!!!!!!\r\n");
yuto17320508 25:c740e6fd5dab 95
yuto17320508 25:c740e6fd5dab 96 motor_lo.setDutyLimit(0.6);
yuto17320508 25:c740e6fd5dab 97 motor_li.setDutyLimit(0.6);
yuto17320508 25:c740e6fd5dab 98
yuto17320508 25:c740e6fd5dab 99 for(int i = 0; i < 25; i++) {
yuto17320508 25:c740e6fd5dab 100 straightAndInfinity(0, 3);
yuto17320508 25:c740e6fd5dab 101 //printf("not dist mode angle:%.3f backdist:%.2f ec:%d\r\n", degree0, get_dist_back(), ec_lo.getCount());
yuto17320508 25:c740e6fd5dab 102 }
yuto17320508 25:c740e6fd5dab 103 printf("get distance mode");
yuto17320508 25:c740e6fd5dab 104 for(int i=0; i<1; ++i) {
yuto17320508 25:c740e6fd5dab 105 while(get_dist_back() < 270) {//320
yuto17320508 25:c740e6fd5dab 106 straightAndInfinity(0, 3);
yuto17320508 25:c740e6fd5dab 107 }
yuto17320508 25:c740e6fd5dab 108 }
yuto17320508 25:c740e6fd5dab 109 //段差前旋回
yuto17320508 25:c740e6fd5dab 110
yuto17320508 25:c740e6fd5dab 111 motor_lo.setDutyLimit(0.5);//0.5
yuto17320508 25:c740e6fd5dab 112 motor_li.setDutyLimit(0.5);
yuto17320508 25:c740e6fd5dab 113 turn(35.0);
yuto17320508 25:c740e6fd5dab 114 case 1://段差前
yuto17320508 25:c740e6fd5dab 115 printf("climb!!!!!!!!!!\r\n");
yuto17320508 25:c740e6fd5dab 116 //段差乗り越え
yuto17320508 25:c740e6fd5dab 117
yuto17320508 25:c740e6fd5dab 118 motor_lo.setDutyLimit(0.3);//0.5
yuto17320508 25:c740e6fd5dab 119 motor_li.setDutyLimit(0.3);
yuto17320508 25:c740e6fd5dab 120 while(get_dist_forward() < 60) {
yuto17320508 25:c740e6fd5dab 121 straightAndInfinity(45, 5);
yuto17320508 25:c740e6fd5dab 122 }
yuto17320508 25:c740e6fd5dab 123 for(int i= 0; i<14; ++i) straight();
yuto17320508 25:c740e6fd5dab 124 while(get_dist_back() > 80) straight();
yuto17320508 25:c740e6fd5dab 125 //段差後旋回
yuto17320508 25:c740e6fd5dab 126 printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
yuto17320508 25:c740e6fd5dab 127 turn(90.0);
yuto17320508 25:c740e6fd5dab 128 case 2://段差直後
yuto17320508 25:c740e6fd5dab 129 printf("go lope!!!!!!!!!!\r\n");
yuto17320508 25:c740e6fd5dab 130 printf("angle:%.3f back dist:%.2f\r\n", degree0, get_dist_back());
yuto17320508 25:c740e6fd5dab 131 //前進
yuto17320508 25:c740e6fd5dab 132 motor_lo.setDutyLimit(0.6);
yuto17320508 25:c740e6fd5dab 133 motor_li.setDutyLimit(0.6);
yuto17320508 25:c740e6fd5dab 134 for(int i=0; i<3; ++i) {
yuto17320508 25:c740e6fd5dab 135 while(get_dist_forward() > 60) {
yuto17320508 25:c740e6fd5dab 136 straightAndInfinity(90.0, 5.0);
yuto17320508 25:c740e6fd5dab 137 }
yuto17320508 25:c740e6fd5dab 138 }
yuto17320508 25:c740e6fd5dab 139 //ロープ前旋回
yuto17320508 25:c740e6fd5dab 140 printf("before roop deg:%.3f\r\n",degree0);
yuto17320508 25:c740e6fd5dab 141 turn(0);
yuto17320508 25:c740e6fd5dab 142
yuto17320508 25:c740e6fd5dab 143 //ロープ位置ジャストまで前進
yuto17320508 25:c740e6fd5dab 144
yuto17320508 25:c740e6fd5dab 145 for(int i=0; i<3; ++i) {
yuto17320508 25:c740e6fd5dab 146 straightAndInfinity(0.0, 5.0);
yuto17320508 25:c740e6fd5dab 147 }
yuto17320508 25:c740e6fd5dab 148 for(int i=0; i<2; ++i) {
yuto17320508 25:c740e6fd5dab 149 while(get_dist_back() < 100) {
yuto17320508 25:c740e6fd5dab 150 straightAndInfinity(0.0, 5.0);
yuto17320508 25:c740e6fd5dab 151 }
yuto17320508 25:c740e6fd5dab 152 }
yuto17320508 25:c740e6fd5dab 153
yuto17320508 25:c740e6fd5dab 154 //wait(10);
yuto17320508 25:c740e6fd5dab 155
yuto17320508 25:c740e6fd5dab 156 //ロープ
yuto17320508 25:c740e6fd5dab 157 while(mode4.read() == 0) {
yuto17320508 25:c740e6fd5dab 158 straightAndInfinity(0, 5);
yuto17320508 25:c740e6fd5dab 159 //straight();
yuto17320508 25:c740e6fd5dab 160 }
yuto17320508 25:c740e6fd5dab 161 case 3://坂
yuto17320508 25:c740e6fd5dab 162 printf("uhai stand by ok!!!!!!!!!!\r\n");
yuto17320508 25:c740e6fd5dab 163 while(get_dist_back() > 40.0) {
yuto17320508 25:c740e6fd5dab 164 wait(0.01);
yuto17320508 25:c740e6fd5dab 165 }
yuto17320508 25:c740e6fd5dab 166 while(get_dist_back() < 40.0) {
yuto17320508 25:c740e6fd5dab 167 wait(0.01);
yuto17320508 25:c740e6fd5dab 168 }
yuto17320508 25:c740e6fd5dab 169
yuto17320508 25:c740e6fd5dab 170 printf("last spart!!!!!!!!");
yuto17320508 25:c740e6fd5dab 171 theta0 = -degree/100.0+90;
yuto17320508 25:c740e6fd5dab 172 motor_lo.setDutyLimit(0.3);//0.3
yuto17320508 25:c740e6fd5dab 173 motor_li.setDutyLimit(0.3);
yuto17320508 25:c740e6fd5dab 174
yuto17320508 25:c740e6fd5dab 175 for(int i=0; i<15; ++i)straight();//climbAndInfinity(-90,5);
yuto17320508 25:c740e6fd5dab 176 printf("wall standby");
yuto17320508 25:c740e6fd5dab 177 while(get_dist_back() < 250) {
yuto17320508 25:c740e6fd5dab 178 straight();
yuto17320508 25:c740e6fd5dab 179 climbAndInfinity(-90,5);
yuto17320508 25:c740e6fd5dab 180 // DEBUG("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
yuto17320508 25:c740e6fd5dab 181 }
yuto17320508 25:c740e6fd5dab 182 for(int i=0; i<1; ++i) {
yuto17320508 25:c740e6fd5dab 183 while(get_dist_forward() > 70) {
yuto17320508 25:c740e6fd5dab 184 //straight();
yuto17320508 25:c740e6fd5dab 185 climbAndInfinity(-90,5);
yuto17320508 25:c740e6fd5dab 186 // DEBUG("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
yuto17320508 25:c740e6fd5dab 187 }
yuto17320508 25:c740e6fd5dab 188 }
yuto17320508 25:c740e6fd5dab 189 hand_mode = GOAL;
yuto17320508 25:c740e6fd5dab 190 straight();
yuto17320508 25:c740e6fd5dab 191 printf("uhai!!!!!!!!!!!!!!!");
shimizuta 22:0ed9de464f40 192 }
shimizuta 22:0ed9de464f40 193 }