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@24:9ee1440c6703, 2019-05-03 (annotated)

Committer:
shimizuta
Date:
Fri May 03 10:00:20 2019 +0000
Revision:
24:9ee1440c6703
Parent:
22:0ed9de464f40
Child:
25:c740e6fd5dab
a

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 12:9a5de6adae9c 7
kageyuta 1:86c4c38abe40 8 int main()
kageyuta 1:86c4c38abe40 9 {
shimizuta 22:0ed9de464f40 10 can1.frequency(1000000);
shimizuta 22:0ed9de464f40 11 SetupMoves();
shimizuta 22:0ed9de464f40 12 printf("reset standby\r\n");
shimizuta 22:0ed9de464f40 13 /*
shimizuta 22:0ed9de464f40 14 while(1) {
shimizuta 22:0ed9de464f40 15 get_dist_forward();
shimizuta 22:0ed9de464f40 16 get_dist_back();
shimizuta 22:0ed9de464f40 17 wait(0.1);
shimizuta 22:0ed9de464f40 18 }
shimizuta 22:0ed9de464f40 19 */
kageyuta 2:55c616d2e0fe 20 reset();
shimizuta 22:0ed9de464f40 21 printf("bus standby\r\n");
yuto17320508 5:63462d696256 22 while(1) {
yuto17320508 5:63462d696256 23 if(bus_in.read() == 1) break;
yuto17320508 3:7a608fbd3bcd 24 }
shimizuta 22:0ed9de464f40 25 printf("bus is %d\r\n", bus_in.read());
yuto17320508 12:9a5de6adae9c 26 wait(0.5);
shimizuta 22:0ed9de464f40 27 motor_lo.setDutyLimit(0.3);
shimizuta 22:0ed9de464f40 28 motor_li.setDutyLimit(0.3);
yuto17320508 12:9a5de6adae9c 29 straight();
yuto17320508 12:9a5de6adae9c 30 wait_gerege();
yuto17320508 12:9a5de6adae9c 31 hand_mode = GEREGE;
yuto17320508 10:a335588b9ef0 32 set_gyro();
shimizuta 18:d0a6771fa38d 33 //直進スタート
shimizuta 22:0ed9de464f40 34 motor_lo.setDutyLimit(0.6);
yuto17320508 19:b162e7f4cc06 35 motor_li.setDutyLimit(0.6);
shimizuta 22:0ed9de464f40 36 for(int i = 0; i < 25; i++)
shimizuta 22:0ed9de464f40 37 straightAndInfinity(0, 5);
shimizuta 22:0ed9de464f40 38
shimizuta 22:0ed9de464f40 39 for(int i = 0; i < sizeof(lowpassfilter)/sizeof(lowpassfilter[0]); i++)
shimizuta 22:0ed9de464f40 40 lowpassfilter[i].SetOldWeight(0);
shimizuta 22:0ed9de464f40 41 for(int i=0; i<2; ++i) {
shimizuta 22:0ed9de464f40 42 while(get_dist_back() < 320) {
yuto17320508 13:29e71a2fd11e 43 straightAndInfinity(0, 5);
yuto17320508 13:29e71a2fd11e 44 }
yuto17320508 3:7a608fbd3bcd 45 }
yuto17320508 10:a335588b9ef0 46 //段差前旋回
yuto17320508 19:b162e7f4cc06 47 motor_lo.setDutyLimit(0.3);//0.5
yuto17320508 19:b162e7f4cc06 48 motor_li.setDutyLimit(0.3);
shimizuta 22:0ed9de464f40 49 turn(35.0);
yuto17320508 10:a335588b9ef0 50 //段差乗り越え
yuto17320508 19:b162e7f4cc06 51 motor_lo.setDutyLimit(0.3);//0.5
yuto17320508 19:b162e7f4cc06 52 motor_li.setDutyLimit(0.3);
shimizuta 22:0ed9de464f40 53 while(get_dist_back() < 80) {
shimizuta 22:0ed9de464f40 54 straightAndInfinity(0, 5);
shimizuta 22:0ed9de464f40 55 }
shimizuta 22:0ed9de464f40 56 for(int i= 0; i<10; ++i) straight();
shimizuta 21:14133581387b 57 //filter切る
shimizuta 21:14133581387b 58 for(int i = 0; i < sizeof(lowpassfilter)/sizeof(lowpassfilter[0]); i++)
shimizuta 21:14133581387b 59 lowpassfilter[i].SetOldWeight(0);
shimizuta 24:9ee1440c6703 60 while(get_dist_back() > 80) straight();
shimizuta 21:14133581387b 61 //filter軽く
shimizuta 21:14133581387b 62 for(int i = 0; i < sizeof(lowpassfilter)/sizeof(lowpassfilter[0]); i++)
shimizuta 21:14133581387b 63 lowpassfilter[i].SetOldWeight(kOldWeightLight);
yuto17320508 10:a335588b9ef0 64 //段差後旋回
yuto17320508 12:9a5de6adae9c 65 printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
yuto17320508 13:29e71a2fd11e 66 turn(90.0);
yuto17320508 12:9a5de6adae9c 67 printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
yuto17320508 10:a335588b9ef0 68 //前進
shimizuta 20:e30e6e175991 69 motor_lo.setDutyLimit(0.5);
yuto17320508 12:9a5de6adae9c 70 motor_li.setDutyLimit(0.5);
shimizuta 20:e30e6e175991 71 for(int i=0; i<3; ++i) {
shimizuta 20:e30e6e175991 72 while(get_dist_forward() > 65) {
yuto17320508 13:29e71a2fd11e 73 straightAndInfinity(90.0, 5.0);
yuto17320508 13:29e71a2fd11e 74 }
yuto17320508 12:9a5de6adae9c 75 }
yuto17320508 10:a335588b9ef0 76 //ロープ前旋回
shimizuta 22:0ed9de464f40 77 printf("before roop deg:%.3f\r\n",degree0);
yuto17320508 13:29e71a2fd11e 78 turn(0);
yuto17320508 14:1a3a673d85e3 79
yuto17320508 10:a335588b9ef0 80 //ロープ
shimizuta 20:e30e6e175991 81 while(mode4.read() == 0) {
yuto17320508 13:29e71a2fd11e 82 straightAndInfinity(0, 5);
yuto17320508 13:29e71a2fd11e 83 }
shimizuta 22:0ed9de464f40 84 printf("go to uhai deg:%.3f forward dist:%.3f \r\n",degree0,get_dist_forward());
shimizuta 20:e30e6e175991 85 for(int i=0; i<3; ++i) {
shimizuta 20:e30e6e175991 86 while(get_dist_forward() > 65) { //65
yuto17320508 13:29e71a2fd11e 87 straightAndInfinity(0, 5);
yuto17320508 13:29e71a2fd11e 88 }
kageyuta 11:a6fadff7cc78 89 }
yuto17320508 14:1a3a673d85e3 90 printf("turn");
yuto17320508 14:1a3a673d85e3 91 turn(-90);
yuto17320508 14:1a3a673d85e3 92
shimizuta 20:e30e6e175991 93 while(get_dist_back() > 10.0) {}
shimizuta 20:e30e6e175991 94 while(get_dist_back() < 30.0) {}
shimizuta 20:e30e6e175991 95
yuto17320508 15:1098bf926b5b 96 printf("last spart!!!!!!!!");
yuto17320508 14:1a3a673d85e3 97 motor_lo.setDutyLimit(0.2);//0.5
yuto17320508 14:1a3a673d85e3 98 motor_li.setDutyLimit(0.2);
yuto17320508 14:1a3a673d85e3 99
yuto17320508 19:b162e7f4cc06 100 for(int i=0; i<15; ++i)straightAndInfinity(-90, 5);
yuto17320508 14:1a3a673d85e3 101 printf("wall standby");
yuto17320508 14:1a3a673d85e3 102 while(get_dist_back() < 250) {
yuto17320508 19:b162e7f4cc06 103 straightAndInfinity(-90, 5);
shimizuta 22:0ed9de464f40 104 // DEBUG("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
yuto17320508 14:1a3a673d85e3 105 }
yuto17320508 14:1a3a673d85e3 106 for(int i=0; i<2; ++i) {
yuto17320508 14:1a3a673d85e3 107 while(get_dist_forward() > 65) {
yuto17320508 19:b162e7f4cc06 108 straightAndInfinity(-90, 5);
shimizuta 22:0ed9de464f40 109 // DEBUG("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
yuto17320508 14:1a3a673d85e3 110 }
yuto17320508 14:1a3a673d85e3 111 }
yuto17320508 12:9a5de6adae9c 112 hand_mode = GOAL;
yuto17320508 12:9a5de6adae9c 113 straight();
yuto17320508 13:29e71a2fd11e 114 printf("uhai!!!!!!!!!!!!!!!");
shimizuta 22:0ed9de464f40 115 }