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@27:d392a95f4799, 2019-05-08 (annotated)

Committer:
shimizuta
Date:
Wed May 08 02:55:40 2019 +0000
Revision:
27:d392a95f4799
Parent:
26:5fb1aa9cb7f0
Child:
28:c92912036b03
debuging

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) {
shimizuta 27:d392a95f4799 30 int
yuto17320508 25:c740e6fd5dab 31 start_state = (get_dist_back()-10)/10;
yuto17320508 25:c740e6fd5dab 32 int tmp = start_state;
yuto17320508 25:c740e6fd5dab 33 if(start_state>6 || start_state<0) start_state = 0;
yuto17320508 25:c740e6fd5dab 34 led2 = start_state/4;
yuto17320508 25:c740e6fd5dab 35 start_state = start_state%4;
yuto17320508 25:c740e6fd5dab 36 led3 = start_state/2;
yuto17320508 25:c740e6fd5dab 37 start_state = start_state%2;
yuto17320508 25:c740e6fd5dab 38 led4 = start_state;
yuto17320508 25:c740e6fd5dab 39 start_state = tmp;
yuto17320508 25:c740e6fd5dab 40 }
shimizuta 26:5fb1aa9cb7f0 41 printf("mode is %d\r\n", start_state);
yuto17320508 12:9a5de6adae9c 42 wait_gerege();
yuto17320508 12:9a5de6adae9c 43 hand_mode = GEREGE;
shimizuta 27:d392a95f4799 44 HandMove();
shimizuta 27:d392a95f4799 45 wait(0.5);
yuto17320508 25:c740e6fd5dab 46 if(start_state == 0) theta0 = -degree/100.0;
yuto17320508 25:c740e6fd5dab 47 else if(start_state == 1) theta0 = -degree/100.0 - 45;
yuto17320508 25:c740e6fd5dab 48 else if(start_state == 2) theta0 = -degree/100.0 - 90;
yuto17320508 25:c740e6fd5dab 49 else if(start_state == 3) theta0 = -degree/100.0 + 90;
yuto17320508 25:c740e6fd5dab 50 else printf("state_error");
shimizuta 26:5fb1aa9cb7f0 51 FileOpen();
yuto17320508 25:c740e6fd5dab 52 StateFlow(start_state);
shimizuta 26:5fb1aa9cb7f0 53 FileClose();
yuto17320508 25:c740e6fd5dab 54 }
yuto17320508 25:c740e6fd5dab 55
yuto17320508 25:c740e6fd5dab 56 void StateFlow(int i)
yuto17320508 25:c740e6fd5dab 57 {
shimizuta 27:d392a95f4799 58 motor_lo.setDutyLimit(0.3);
shimizuta 27:d392a95f4799 59 motor_li.setDutyLimit(0.3);
shimizuta 27:d392a95f4799 60 while(1)
shimizuta 27:d392a95f4799 61
shimizuta 27:d392a95f4799 62 // straight();
yuto17320508 25:c740e6fd5dab 63 switch(i) {
yuto17320508 25:c740e6fd5dab 64 case 0://最初の直線
yuto17320508 25:c740e6fd5dab 65 printf("go straight!!!!!!!!!!\r\n");
shimizuta 27:d392a95f4799 66 motor_lo.setDutyLimit(0.5);
shimizuta 27:d392a95f4799 67 motor_li.setDutyLimit(0.5);
yuto17320508 25:c740e6fd5dab 68 for(int i = 0; i < 25; i++) {
yuto17320508 25:c740e6fd5dab 69 straightAndInfinity(0, 3);
yuto17320508 25:c740e6fd5dab 70 }
yuto17320508 25:c740e6fd5dab 71 printf("get distance mode");
yuto17320508 25:c740e6fd5dab 72 for(int i=0; i<1; ++i) {
shimizuta 27:d392a95f4799 73 while(get_dist_back() < 290) {//320
yuto17320508 25:c740e6fd5dab 74 straightAndInfinity(0, 3);
yuto17320508 25:c740e6fd5dab 75 }
yuto17320508 25:c740e6fd5dab 76 }
yuto17320508 25:c740e6fd5dab 77 //段差前旋回
yuto17320508 25:c740e6fd5dab 78 motor_lo.setDutyLimit(0.5);//0.5
yuto17320508 25:c740e6fd5dab 79 motor_li.setDutyLimit(0.5);
yuto17320508 25:c740e6fd5dab 80 turn(35.0);
yuto17320508 25:c740e6fd5dab 81 case 1://段差前
yuto17320508 25:c740e6fd5dab 82 printf("climb!!!!!!!!!!\r\n");
yuto17320508 25:c740e6fd5dab 83 //段差乗り越え
shimizuta 27:d392a95f4799 84 motor_lo.setDutyLimit(0.2);//0.5
shimizuta 27:d392a95f4799 85 motor_li.setDutyLimit(0.2);
yuto17320508 25:c740e6fd5dab 86 while(get_dist_forward() < 60) {
yuto17320508 25:c740e6fd5dab 87 straightAndInfinity(45, 5);
yuto17320508 25:c740e6fd5dab 88 }
yuto17320508 25:c740e6fd5dab 89 for(int i= 0; i<14; ++i) straight();
yuto17320508 25:c740e6fd5dab 90 while(get_dist_back() > 80) straight();
yuto17320508 25:c740e6fd5dab 91 //段差後旋回
yuto17320508 25:c740e6fd5dab 92 printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
shimizuta 27:d392a95f4799 93 motor_lo.setDutyLimit(0.4);//0.5
shimizuta 27:d392a95f4799 94 motor_li.setDutyLimit(0.4);
yuto17320508 25:c740e6fd5dab 95 turn(90.0);
yuto17320508 25:c740e6fd5dab 96 case 2://段差直後
yuto17320508 25:c740e6fd5dab 97 printf("go lope!!!!!!!!!!\r\n");
yuto17320508 25:c740e6fd5dab 98 printf("angle:%.3f back dist:%.2f\r\n", degree0, get_dist_back());
yuto17320508 25:c740e6fd5dab 99 //前進
shimizuta 27:d392a95f4799 100 motor_lo.setDutyLimit(0.5);
shimizuta 27:d392a95f4799 101 motor_li.setDutyLimit(0.5);
yuto17320508 25:c740e6fd5dab 102 for(int i=0; i<3; ++i) {
yuto17320508 25:c740e6fd5dab 103 while(get_dist_forward() > 60) {
yuto17320508 25:c740e6fd5dab 104 straightAndInfinity(90.0, 5.0);
yuto17320508 25:c740e6fd5dab 105 }
yuto17320508 25:c740e6fd5dab 106 }
yuto17320508 25:c740e6fd5dab 107 //ロープ前旋回
yuto17320508 25:c740e6fd5dab 108 printf("before roop deg:%.3f\r\n",degree0);
yuto17320508 25:c740e6fd5dab 109 turn(0);
yuto17320508 25:c740e6fd5dab 110 //ロープ位置ジャストまで前進
yuto17320508 25:c740e6fd5dab 111 for(int i=0; i<3; ++i) {
yuto17320508 25:c740e6fd5dab 112 straightAndInfinity(0.0, 5.0);
yuto17320508 25:c740e6fd5dab 113 }
yuto17320508 25:c740e6fd5dab 114 for(int i=0; i<2; ++i) {
yuto17320508 25:c740e6fd5dab 115 while(get_dist_back() < 100) {
yuto17320508 25:c740e6fd5dab 116 straightAndInfinity(0.0, 5.0);
yuto17320508 25:c740e6fd5dab 117 }
yuto17320508 25:c740e6fd5dab 118 }
yuto17320508 25:c740e6fd5dab 119 //ロープ
yuto17320508 25:c740e6fd5dab 120 while(mode4.read() == 0) {
yuto17320508 25:c740e6fd5dab 121 straightAndInfinity(0, 5);
yuto17320508 25:c740e6fd5dab 122 //straight();
yuto17320508 25:c740e6fd5dab 123 }
yuto17320508 25:c740e6fd5dab 124 case 3://坂
yuto17320508 25:c740e6fd5dab 125 printf("uhai stand by ok!!!!!!!!!!\r\n");
yuto17320508 25:c740e6fd5dab 126 while(get_dist_back() > 40.0) {
yuto17320508 25:c740e6fd5dab 127 wait(0.01);
yuto17320508 25:c740e6fd5dab 128 }
yuto17320508 25:c740e6fd5dab 129 while(get_dist_back() < 40.0) {
yuto17320508 25:c740e6fd5dab 130 wait(0.01);
yuto17320508 25:c740e6fd5dab 131 }
yuto17320508 25:c740e6fd5dab 132 printf("last spart!!!!!!!!");
yuto17320508 25:c740e6fd5dab 133 theta0 = -degree/100.0+90;
yuto17320508 25:c740e6fd5dab 134 motor_lo.setDutyLimit(0.3);//0.3
yuto17320508 25:c740e6fd5dab 135 motor_li.setDutyLimit(0.3);
shimizuta 27:d392a95f4799 136 for(int i=0; i<15; ++i)straightAndInfinity(-90, 5);//straight();//climbAndInfinity(-90,5);
yuto17320508 25:c740e6fd5dab 137 printf("wall standby");
yuto17320508 25:c740e6fd5dab 138 while(get_dist_back() < 250) {
shimizuta 27:d392a95f4799 139 // straight();
yuto17320508 25:c740e6fd5dab 140 climbAndInfinity(-90,5);
yuto17320508 25:c740e6fd5dab 141 }
yuto17320508 25:c740e6fd5dab 142 for(int i=0; i<1; ++i) {
yuto17320508 25:c740e6fd5dab 143 while(get_dist_forward() > 70) {
yuto17320508 25:c740e6fd5dab 144 //straight();
yuto17320508 25:c740e6fd5dab 145 climbAndInfinity(-90,5);
yuto17320508 25:c740e6fd5dab 146 }
yuto17320508 25:c740e6fd5dab 147 }
yuto17320508 25:c740e6fd5dab 148 hand_mode = GOAL;
yuto17320508 25:c740e6fd5dab 149 straight();
yuto17320508 25:c740e6fd5dab 150 printf("uhai!!!!!!!!!!!!!!!");
shimizuta 22:0ed9de464f40 151 }
shimizuta 22:0ed9de464f40 152 }