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@26:5fb1aa9cb7f0, 2019-05-06 (annotated)

Committer:
shimizuta
Date:
Mon May 06 06:28:41 2019 +0000
Revision:
26:5fb1aa9cb7f0
Parent:
25:c740e6fd5dab
Child:
27:d392a95f4799
can get log

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 26:5fb1aa9cb7f0 13 printf("file open\r\n");
shimizuta 22:0ed9de464f40 14 SetupMoves();
yuto17320508 25:c740e6fd5dab 15 set_gyro();
yuto17320508 25:c740e6fd5dab 16 int start_state=0;
shimizuta 22:0ed9de464f40 17 printf("reset standby\r\n");
shimizuta 26:5fb1aa9cb7f0 18 reset();//足をリセット位置に移動
shimizuta 22:0ed9de464f40 19 printf("bus standby\r\n");
yuto17320508 5:63462d696256 20 while(1) {
yuto17320508 5:63462d696256 21 if(bus_in.read() == 1) break;
yuto17320508 3:7a608fbd3bcd 22 }
shimizuta 22:0ed9de464f40 23 printf("bus is %d\r\n", bus_in.read());
yuto17320508 12:9a5de6adae9c 24 wait(0.5);
shimizuta 26:5fb1aa9cb7f0 25 //一歩動かして初期位置へ
yuto17320508 25:c740e6fd5dab 26 motor_lo.setDutyLimit(0.1);
yuto17320508 25:c740e6fd5dab 27 motor_li.setDutyLimit(0.1);
yuto17320508 12:9a5de6adae9c 28 straight();
shimizuta 26:5fb1aa9cb7f0 29 // mode選択
yuto17320508 25:c740e6fd5dab 30 while(mode4.read() == 1) {
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 start_state = 0;//debug用
shimizuta 26:5fb1aa9cb7f0 42 printf("mode is %d\r\n", start_state);
yuto17320508 12:9a5de6adae9c 43 wait_gerege();
yuto17320508 12:9a5de6adae9c 44 hand_mode = GEREGE;
yuto17320508 25:c740e6fd5dab 45 wait(1.0);
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 {
yuto17320508 25:c740e6fd5dab 58 switch(i) {
yuto17320508 25:c740e6fd5dab 59 case 0://最初の直線
yuto17320508 25:c740e6fd5dab 60 printf("go straight!!!!!!!!!!\r\n");
shimizuta 26:5fb1aa9cb7f0 61 motor_lo.setDutyLimit(0.3);
shimizuta 26:5fb1aa9cb7f0 62 motor_li.setDutyLimit(0.3);
yuto17320508 25:c740e6fd5dab 63 for(int i = 0; i < 25; i++) {
yuto17320508 25:c740e6fd5dab 64 straightAndInfinity(0, 3);
yuto17320508 25:c740e6fd5dab 65 }
yuto17320508 25:c740e6fd5dab 66 printf("get distance mode");
yuto17320508 25:c740e6fd5dab 67 for(int i=0; i<1; ++i) {
yuto17320508 25:c740e6fd5dab 68 while(get_dist_back() < 270) {//320
yuto17320508 25:c740e6fd5dab 69 straightAndInfinity(0, 3);
yuto17320508 25:c740e6fd5dab 70 }
yuto17320508 25:c740e6fd5dab 71 }
yuto17320508 25:c740e6fd5dab 72 //段差前旋回
yuto17320508 25:c740e6fd5dab 73 motor_lo.setDutyLimit(0.5);//0.5
yuto17320508 25:c740e6fd5dab 74 motor_li.setDutyLimit(0.5);
yuto17320508 25:c740e6fd5dab 75 turn(35.0);
yuto17320508 25:c740e6fd5dab 76 case 1://段差前
yuto17320508 25:c740e6fd5dab 77 printf("climb!!!!!!!!!!\r\n");
yuto17320508 25:c740e6fd5dab 78 //段差乗り越え
yuto17320508 25:c740e6fd5dab 79 motor_lo.setDutyLimit(0.3);//0.5
yuto17320508 25:c740e6fd5dab 80 motor_li.setDutyLimit(0.3);
yuto17320508 25:c740e6fd5dab 81 while(get_dist_forward() < 60) {
yuto17320508 25:c740e6fd5dab 82 straightAndInfinity(45, 5);
yuto17320508 25:c740e6fd5dab 83 }
yuto17320508 25:c740e6fd5dab 84 for(int i= 0; i<14; ++i) straight();
yuto17320508 25:c740e6fd5dab 85 while(get_dist_back() > 80) straight();
yuto17320508 25:c740e6fd5dab 86 //段差後旋回
yuto17320508 25:c740e6fd5dab 87 printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
yuto17320508 25:c740e6fd5dab 88 turn(90.0);
yuto17320508 25:c740e6fd5dab 89 case 2://段差直後
yuto17320508 25:c740e6fd5dab 90 printf("go lope!!!!!!!!!!\r\n");
yuto17320508 25:c740e6fd5dab 91 printf("angle:%.3f back dist:%.2f\r\n", degree0, get_dist_back());
yuto17320508 25:c740e6fd5dab 92 //前進
yuto17320508 25:c740e6fd5dab 93 motor_lo.setDutyLimit(0.6);
yuto17320508 25:c740e6fd5dab 94 motor_li.setDutyLimit(0.6);
yuto17320508 25:c740e6fd5dab 95 for(int i=0; i<3; ++i) {
yuto17320508 25:c740e6fd5dab 96 while(get_dist_forward() > 60) {
yuto17320508 25:c740e6fd5dab 97 straightAndInfinity(90.0, 5.0);
yuto17320508 25:c740e6fd5dab 98 }
yuto17320508 25:c740e6fd5dab 99 }
yuto17320508 25:c740e6fd5dab 100 //ロープ前旋回
yuto17320508 25:c740e6fd5dab 101 printf("before roop deg:%.3f\r\n",degree0);
yuto17320508 25:c740e6fd5dab 102 turn(0);
yuto17320508 25:c740e6fd5dab 103 //ロープ位置ジャストまで前進
yuto17320508 25:c740e6fd5dab 104 for(int i=0; i<3; ++i) {
yuto17320508 25:c740e6fd5dab 105 straightAndInfinity(0.0, 5.0);
yuto17320508 25:c740e6fd5dab 106 }
yuto17320508 25:c740e6fd5dab 107 for(int i=0; i<2; ++i) {
yuto17320508 25:c740e6fd5dab 108 while(get_dist_back() < 100) {
yuto17320508 25:c740e6fd5dab 109 straightAndInfinity(0.0, 5.0);
yuto17320508 25:c740e6fd5dab 110 }
yuto17320508 25:c740e6fd5dab 111 }
yuto17320508 25:c740e6fd5dab 112 //ロープ
yuto17320508 25:c740e6fd5dab 113 while(mode4.read() == 0) {
yuto17320508 25:c740e6fd5dab 114 straightAndInfinity(0, 5);
yuto17320508 25:c740e6fd5dab 115 //straight();
yuto17320508 25:c740e6fd5dab 116 }
yuto17320508 25:c740e6fd5dab 117 case 3://坂
yuto17320508 25:c740e6fd5dab 118 printf("uhai stand by ok!!!!!!!!!!\r\n");
yuto17320508 25:c740e6fd5dab 119 while(get_dist_back() > 40.0) {
yuto17320508 25:c740e6fd5dab 120 wait(0.01);
yuto17320508 25:c740e6fd5dab 121 }
yuto17320508 25:c740e6fd5dab 122 while(get_dist_back() < 40.0) {
yuto17320508 25:c740e6fd5dab 123 wait(0.01);
yuto17320508 25:c740e6fd5dab 124 }
yuto17320508 25:c740e6fd5dab 125 printf("last spart!!!!!!!!");
yuto17320508 25:c740e6fd5dab 126 theta0 = -degree/100.0+90;
yuto17320508 25:c740e6fd5dab 127 motor_lo.setDutyLimit(0.3);//0.3
yuto17320508 25:c740e6fd5dab 128 motor_li.setDutyLimit(0.3);
yuto17320508 25:c740e6fd5dab 129 for(int i=0; i<15; ++i)straight();//climbAndInfinity(-90,5);
yuto17320508 25:c740e6fd5dab 130 printf("wall standby");
yuto17320508 25:c740e6fd5dab 131 while(get_dist_back() < 250) {
yuto17320508 25:c740e6fd5dab 132 straight();
yuto17320508 25:c740e6fd5dab 133 climbAndInfinity(-90,5);
yuto17320508 25:c740e6fd5dab 134 }
yuto17320508 25:c740e6fd5dab 135 for(int i=0; i<1; ++i) {
yuto17320508 25:c740e6fd5dab 136 while(get_dist_forward() > 70) {
yuto17320508 25:c740e6fd5dab 137 //straight();
yuto17320508 25:c740e6fd5dab 138 climbAndInfinity(-90,5);
yuto17320508 25:c740e6fd5dab 139 }
yuto17320508 25:c740e6fd5dab 140 }
yuto17320508 25:c740e6fd5dab 141 hand_mode = GOAL;
yuto17320508 25:c740e6fd5dab 142 straight();
yuto17320508 25:c740e6fd5dab 143 printf("uhai!!!!!!!!!!!!!!!");
shimizuta 22:0ed9de464f40 144 }
shimizuta 22:0ed9de464f40 145 }