春ロボ1班(元F3RC4班+) / Mbed 2 deprecated harurobo_ashi_3_20

3/20 2:04

Dependencies:   mbed move4wheel2 EC CruizCore_R1370P

main.cpp@12:762061580168, 2019-03-19 (annotated)

Committer:
yuki0701
Date:
Tue Mar 19 14:31:24 2019 +0000
Revision:
12:762061580168
Parent:
11:9db93bce4eef
Child:
13:f9867c3b97b0
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
la00noix 0:c61c6e4775ca 1 #include "EC.h"
la00noix 0:c61c6e4775ca 2 #include "R1370P.h"
la00noix 0:c61c6e4775ca 3 #include "move4wheel.h"
la00noix 0:c61c6e4775ca 4 #include "mbed.h"
la00noix 0:c61c6e4775ca 5 #include "math.h"
la00noix 0:c61c6e4775ca 6 #include "PathFollowing.h"
la00noix 0:c61c6e4775ca 7 #include "movement.h"
la00noix 0:c61c6e4775ca 8 #include "maxonsetting.h"
la00noix 0:c61c6e4775ca 9 #include "manual.h"
la00noix 0:c61c6e4775ca 10 #include "can.h"
la00noix 0:c61c6e4775ca 11
la00noix 0:c61c6e4775ca 12 #define PI 3.141592
la00noix 0:c61c6e4775ca 13
la00noix 0:c61c6e4775ca 14 //#define PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示する際に定義
la00noix 0:c61c6e4775ca 15
la00noix 0:c61c6e4775ca 16 int go_waitmode = 0;
la00noix 0:c61c6e4775ca 17
yuki0701 11:9db93bce4eef 18 /*------------運動時の各速度の設定----------*/
yuki0701 11:9db93bce4eef 19 double st_speed = 500; // 移動開始時の速度
yuki0701 11:9db93bce4eef 20 double max_speed = 2500; // 直線移動時の最大速度
yuki0701 11:9db93bce4eef 21 double mid_speed = 2000; // 直線移動時の中間速度
yuki0701 11:9db93bce4eef 22 double turn_speed = 1000; // 旋回時の速度
yuki0701 11:9db93bce4eef 23 double end_speed = 200; // 停止直前の速度
yuki0701 11:9db93bce4eef 24 double fruit_speed = 200; // 果物を取る時の速度
yuki0701 11:9db93bce4eef 25 /*--------------------------------------*/
yuki0701 11:9db93bce4eef 26
la00noix 0:c61c6e4775ca 27 //-----mbed led------------------//点灯条件-----------------------//参照場所------------------------------//
la00noix 9:7667dcfc7ce5 28 //DigitalOut cansend_led(LED1); //cansend -> on //can.cpp
la00noix 9:7667dcfc7ce5 29 //DigitalOut canread_led(LED2); //canread -> on //can.cpp
la00noix 0:c61c6e4775ca 30 //DigitalOut debug_led(LED3); //maxon debug programme -> on //maxonsetting.cpp
la00noix 0:c61c6e4775ca 31
la00noix 0:c61c6e4775ca 32 //////////////////////////////////////////////////////////////以下main文/////////////////////////////////////////////////////////////////
la00noix 0:c61c6e4775ca 33
la00noix 6:26724c287387 34 int main()
la00noix 6:26724c287387 35 {
la00noix 0:c61c6e4775ca 36 UserLoopSetting_maxon();
la00noix 0:c61c6e4775ca 37 UserLoopSetting_sensor();
la00noix 0:c61c6e4775ca 38 UserLoopSetting_can();
la00noix 6:26724c287387 39
la00noix 0:c61c6e4775ca 40 #ifdef PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示
la00noix 0:c61c6e4775ca 41
la00noix 6:26724c287387 42 printf("ソースファイル名 : %s¥n", __FILE__);
la00noix 6:26724c287387 43 printf("作成日付 : %s¥n", __DATE__);
la00noix 6:26724c287387 44 printf("作成時刻 : %s¥n", __TIME__);
la00noix 6:26724c287387 45
la00noix 0:c61c6e4775ca 46 #endif
la00noix 0:c61c6e4775ca 47
la00noix 6:26724c287387 48 while(1) {
yuki0701 11:9db93bce4eef 49 //id1_value[0] = 1;
la00noix 6:26724c287387 50 switch(id1_value[0]) {
la00noix 0:c61c6e4775ca 51 //-----auto mode----------------------------------------------------------------------------------------------------------------------//
la00noix 6:26724c287387 52 case 1:
yuki0701 11:9db93bce4eef 53 //id1_value[6] = 0;
la00noix 6:26724c287387 54 switch(id1_value[6]) {
la00noix 6:26724c287387 55 case 0:
la00noix 6:26724c287387 56 //-----right mode-------------------------------------------------------------------------------------------------------------//
yuki0701 11:9db93bce4eef 57 //未修正
yuki0701 11:9db93bce4eef 58
la00noix 9:7667dcfc7ce5 59 flag = 0;
yuki0701 10:c741191360de 60 UserLoopSetting_enc_right();
yuki0701 10:c741191360de 61 if(T1 == 0) { //スタート位置からみかんの木まで移動
yuki0701 12:762061580168 62
yuki0701 12:762061580168 63 gogo_straight(1,1,3112,3500,2962,3100,st_speed,mid_speed,5,0.1,10,0.1,600,0);
yuki0701 12:762061580168 64 gogo_straight(1,1,2962,3100,2962,2900,mid_speed,turn_speed,5,0.1,10,0.1,600,0);
yuki0701 12:762061580168 65 purecurve(6,1,1,2962,2900,2317,2500,9,turn_speed,5,0.1,10,0.1,600,0);
yuki0701 12:762061580168 66 purecurve(5,1,1,2317,2500,1672,2000,9,turn_speed,5,0.1,10,0.1,600,0);
yuki0701 10:c741191360de 67 set_cond(2,1,1050,1,1076);
yuki0701 12:762061580168 68 gogo_straight(0,0,1672,2000,1672,1600,turn_speed,end_speed,5,0.1,10,0.1,600,0);
yuki0701 10:c741191360de 69 MaxonControl(0,0,0,0);
yuki0701 11:9db93bce4eef 70 pos_correction(1672,1600,0,0,0,25);
yuki0701 10:c741191360de 71 enc_correction(1,1);
yuki0701 12:762061580168 72 gogo_straight(0,0,1672,1600,1672,1375,fruit_speed,fruit_speed,5,0.1,10,0.1,600,0);
yuki0701 12:762061580168 73 MaxonControl(0,0,0,0);
yuki0701 12:762061580168 74
yuki0701 12:762061580168 75 //printf("t1 = 0\n\r");
yuki0701 12:762061580168 76 //wait(1);
yuki0701 10:c741191360de 77 T1++;
yuki0701 10:c741191360de 78 }
yuki0701 10:c741191360de 79 if(T1 == 1) {
yuki0701 10:c741191360de 80 while(1) {
yuki0701 10:c741191360de 81 wait(0.1);
yuki0701 11:9db93bce4eef 82 //calc_xy(0,0,0);
yuki0701 10:c741191360de 83 if(T1 == 2) {
yuki0701 11:9db93bce4eef 84 // enc_correction(1,1);
yuki0701 10:c741191360de 85 break;
yuki0701 10:c741191360de 86 }
yuki0701 10:c741191360de 87 }
yuki0701 10:c741191360de 88 }
yuki0701 10:c741191360de 89 if(T1 == 2) { //みかんの木から三宝置き場まで移動
yuki0701 12:762061580168 90
yuki0701 12:762061580168 91 gogo_straight(1,1,1672,1375,1672,2000,st_speed,turn_speed,5,0.1,10,0.1,600,0);
yuki0701 12:762061580168 92 purecurve(2,1,1,1672,2000,2317,2500,9,turn_speed,5,0.1,10,0.1,600,0);
yuki0701 12:762061580168 93 purecurve(1,1,1,2317,2500,2962,3000,9,turn_speed,5,0.1,10,0.1,600,90);
yuki0701 12:762061580168 94 gogo_straight(1,1,2962,3000,2962,4000,turn_speed,max_speed,5,0.1,10,0.1,600,90);
yuki0701 12:762061580168 95 gogo_straight(1,1,2962,4000,2962,4810,max_speed,end_speed,5,0.1,10,0.1,600,90);
yuki0701 10:c741191360de 96 MaxonControl(0,0,0,0);
yuki0701 10:c741191360de 97 set_cond(2,1,2462,0,6000);
yuki0701 12:762061580168 98 pos_correction(2962,4837.5,90,0,0,10);
yuki0701 12:762061580168 99 gogo_straight(0,0,2962,4837.5,2782,4837.5,300,200,5,0.1,10,0.1,800,90);
yuki0701 10:c741191360de 100 MaxonControl(0,0,0,0);
yuki0701 10:c741191360de 101 enc_correction(1,1);
yuki0701 12:762061580168 102
yuki0701 12:762061580168 103 //printf("t1 = 2\n\r");
yuki0701 12:762061580168 104 //wait(1);
yuki0701 10:c741191360de 105 T1++;
yuki0701 10:c741191360de 106 }
yuki0701 10:c741191360de 107 if(T1 == 3) {
yuki0701 10:c741191360de 108 while(1) {
yuki0701 10:c741191360de 109 wait(0.1);
yuki0701 11:9db93bce4eef 110 //calc_xy(90,0,0);
yuki0701 10:c741191360de 111 if(T1 == 4) {
yuki0701 11:9db93bce4eef 112 // enc_correction(1,1);
yuki0701 10:c741191360de 113 break;
yuki0701 10:c741191360de 114 }
yuki0701 10:c741191360de 115 }
yuki0701 10:c741191360de 116 }
yuki0701 10:c741191360de 117 if(T1 == 4) { //三宝置き場からりんごの木まで移動
yuki0701 12:762061580168 118
yuki0701 12:762061580168 119 gogo_straight(1,1,2782,4837.5,2782,5150,st_speed,turn_speed,5,0.1,10,0.1,800,90);
yuki0701 12:762061580168 120 purecurve(3,1,1,2850,5150,2257,5500,9,turn_speed,5,0.1,10,0.1,800,90);
yuki0701 12:762061580168 121 purecurve(4,1,1,2257,5500,1700,6000,9,turn_speed,5,0.1,10,0.1,800,90);
yuki0701 12:762061580168 122 purecurve(2,1,1,1700,6000,2257,6550,9,turn_speed,5,0.1,10,0.1,800,90);
yuki0701 12:762061580168 123 gogo_straight(1,1,2257,6550,2500,6605,turn_speed,end_speed,5,0.1,10,0.1,800,90);
yuki0701 10:c741191360de 124 MaxonControl(0,0,0,0);
yuki0701 12:762061580168 125 set_cond(1,0,0,1,6038);
yuki0701 12:762061580168 126 pos_correction(2500,6610,90,1,0,8);
yuki0701 12:762061580168 127 set_cond(2,0,3162,1,6038);
yuki0701 12:762061580168 128 gogo_straight(0,0,2500,6610,2700,6610,fruit_speed,fruit_speed,5,0.1,10,0.1,800,90);
yuki0701 12:762061580168 129 MaxonControl(0,0,0,0);
yuki0701 10:c741191360de 130 enc_correction(1,1);
yuki0701 12:762061580168 131
yuki0701 12:762061580168 132 // printf("t1 = 4\n\r");
yuki0701 12:762061580168 133 // wait(1);
yuki0701 10:c741191360de 134 T1++;
yuki0701 10:c741191360de 135 }
yuki0701 10:c741191360de 136 if(T1 == 5) {
yuki0701 10:c741191360de 137 while(1) {
yuki0701 10:c741191360de 138 wait(0.1);
yuki0701 11:9db93bce4eef 139 //calc_xy(90,0,0);
yuki0701 10:c741191360de 140 if(T1 == 6) {
yuki0701 11:9db93bce4eef 141 // enc_correction(1,1);
yuki0701 10:c741191360de 142 break;
yuki0701 10:c741191360de 143 }
yuki0701 10:c741191360de 144 }
yuki0701 10:c741191360de 145 }
yuki0701 10:c741191360de 146 if(T1 == 6) { //りんごの木からお供え台まで移動
yuki0701 12:762061580168 147
yuki0701 12:762061580168 148 set_cond(1,0,0,0,6962);
yuki0701 12:762061580168 149 gogo_straight(1,0,2700,6610,2500,6550,st_speed,max_speed,5,0.1,10,0.1,800,90);
yuki0701 12:762061580168 150 gogo_straight(1,0,2500,6550,1300,6550,max_speed,max_speed,5,0.1,10,0.1,800,90);
yuki0701 12:762061580168 151 gogo_straight(1,0,1300,6550,850,6550,max_speed,turn_speed,5,0.1,10,0.1,800,90);
yuki0701 12:762061580168 152 enc_correction(0,1);
yuki0701 12:762061580168 153 set_cond(2,1,19,0,6962); //要修正
yuki0701 12:762061580168 154 purecurve(5,0,0,850,6550,620,6000,9,turn_speed,10,0.1,10,0.1,600,180); //要修正
yuki0701 12:762061580168 155 enc_correction(1,0);
yuki0701 12:762061580168 156 set_cond(0,1,19,0,0);
yuki0701 12:762061580168 157 gogo_straight(0,1,620,6000,620,5500,turn_speed,max_speed,5,0.1,10,0.1,600,180);
yuki0701 12:762061580168 158 gogo_straight(0,1,620,5500,620,5000,max_speed,max_speed,5,0.1,10,0.1,600,180);
yuki0701 12:762061580168 159 set_cond(2,0,1000,1,4000);
yuki0701 12:762061580168 160 gogo_straight(0,0,620,5000,620,4600,max_speed,end_speed,5,0.1,10,0.1,800,180);
yuki0701 10:c741191360de 161 MaxonControl(0,0,0,0);
yuki0701 12:762061580168 162 pos_correction(620,4370,180,0,0,30);
yuki0701 10:c741191360de 163 enc_correction(1,1);
yuki0701 10:c741191360de 164 MaxonControl(0,0,0,0);
yuki0701 12:762061580168 165
yuki0701 12:762061580168 166 // printf("t1 = 6\n\r");
yuki0701 12:762061580168 167 // wait(1);
yuki0701 10:c741191360de 168 T1++;
yuki0701 10:c741191360de 169 }
yuki0701 10:c741191360de 170 if(T1 == 7) {
yuki0701 10:c741191360de 171 while(1) {
yuki0701 10:c741191360de 172 printf("ashi finished\n\r");
yuki0701 10:c741191360de 173 MaxonControl(0,0,0,0);
yuki0701 10:c741191360de 174 if(id1_value[0] != 1)break; //これらは他のwhileにも入れる必要あり
yuki0701 10:c741191360de 175 if(id1_value[6] != flag)break;
yuki0701 10:c741191360de 176 }
yuki0701 10:c741191360de 177 }
yuki0701 10:c741191360de 178 break;
yuki0701 11:9db93bce4eef 179
yuki0701 10:c741191360de 180 case 1:
yuki0701 10:c741191360de 181 //-----left mode--------------------------------------------------------------------------------------------------------------//
yuki0701 10:c741191360de 182 flag = 1;
yuki0701 11:9db93bce4eef 183 printf("start\n\r");
yuki0701 10:c741191360de 184 UserLoopSetting_enc_left();
la00noix 9:7667dcfc7ce5 185 if(T1 == 0) { //スタート位置からみかんの木まで移動
yuki0701 11:9db93bce4eef 186
yuki0701 11:9db93bce4eef 187 gogo_straight(1,1,-3112,3500,-2962,3100,st_speed,mid_speed,5,0.1,10,0.1,600,0);
yuki0701 11:9db93bce4eef 188 gogo_straight(1,1,-2962,3100,-2962,2900,mid_speed,turn_speed,5,0.1,10,0.1,600,0);
yuki0701 11:9db93bce4eef 189 purecurve(7,1,1,-2962,2900,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0);
yuki0701 11:9db93bce4eef 190 purecurve(8,1,1,-2317,2500,-1672,2000,9,turn_speed,5,0.1,10,0.1,600,0);
la00noix 9:7667dcfc7ce5 191 set_cond(2,0,-1050,1,1076);
yuki0701 11:9db93bce4eef 192 gogo_straight(0,0,-1672,2000,-1672,1600,turn_speed,end_speed,5,0.1,10,0.1,600,0);
la00noix 9:7667dcfc7ce5 193 MaxonControl(0,0,0,0);
yuki0701 11:9db93bce4eef 194 pos_correction(-1672,1600,0,0,0,25);
la00noix 9:7667dcfc7ce5 195 enc_correction(1,1);
yuki0701 11:9db93bce4eef 196 gogo_straight(0,0,-1672,1600,-1672,1375,fruit_speed,fruit_speed,5,0.1,10,0.1,600,0);
yuki0701 11:9db93bce4eef 197 MaxonControl(0,0,0,0);
yuki0701 11:9db93bce4eef 198
la00noix 8:2ba338b4590e 199 T1++;
la00noix 8:2ba338b4590e 200 }
la00noix 8:2ba338b4590e 201 if(T1 == 1) {
la00noix 8:2ba338b4590e 202 while(1) {
la00noix 9:7667dcfc7ce5 203 wait(0.1);
yuki0701 11:9db93bce4eef 204 calc_xy(0,0,0);
la00noix 8:2ba338b4590e 205 if(T1 == 2) {
yuki0701 11:9db93bce4eef 206 enc_correction(1,1);
la00noix 8:2ba338b4590e 207 break;
la00noix 8:2ba338b4590e 208 }
la00noix 8:2ba338b4590e 209 }
la00noix 8:2ba338b4590e 210 }
la00noix 9:7667dcfc7ce5 211 if(T1 == 2) { //みかんの木から三宝置き場まで移動
yuki0701 11:9db93bce4eef 212 gogo_straight(1,1,-1672,1375,-1672,2000,st_speed,turn_speed,5,0.1,10,0.1,600,0);
yuki0701 11:9db93bce4eef 213 purecurve(3,1,1,-1672,2000,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0);
yuki0701 11:9db93bce4eef 214 purecurve(4,1,1,-2317,2500,-2962,3000,9,turn_speed,5,0.1,10,0.1,600,-90); //purecurve(4,1,1,-2317,2500,-2962,3000,9,1000,5,0.1,10,0.1,600,-90);
yuki0701 11:9db93bce4eef 215 gogo_straight(1,1,-2962,3000,-2962,4000,turn_speed,max_speed,5,0.1,10,0.1,600,-90);
yuki0701 12:762061580168 216 gogo_straight(1,1,-2962,4000,-2962,4810,max_speed,end_speed,5,0.1,10,0.1,600,-90);
la00noix 9:7667dcfc7ce5 217 MaxonControl(0,0,0,0);
la00noix 9:7667dcfc7ce5 218 set_cond(2,0,-2462,0,6000);
yuki0701 12:762061580168 219 pos_correction(-2962,4837.5,-90,0,0,10);
yuki0701 12:762061580168 220 gogo_straight(0,0,-2962,4837.5,-2782,4837.5,300,200,5,0.1,10,0.1,800,-90);
la00noix 9:7667dcfc7ce5 221 MaxonControl(0,0,0,0);
la00noix 9:7667dcfc7ce5 222 enc_correction(1,1);
yuki0701 11:9db93bce4eef 223
la00noix 8:2ba338b4590e 224 T1++;
la00noix 8:2ba338b4590e 225 }
la00noix 8:2ba338b4590e 226 if(T1 == 3) {
la00noix 8:2ba338b4590e 227 while(1) {
la00noix 9:7667dcfc7ce5 228 wait(0.1);
yuki0701 11:9db93bce4eef 229 calc_xy(-90,0,0);
la00noix 8:2ba338b4590e 230 if(T1 == 4) {
yuki0701 11:9db93bce4eef 231 enc_correction(1,1);
la00noix 8:2ba338b4590e 232 break;
la00noix 8:2ba338b4590e 233 }
la00noix 8:2ba338b4590e 234 }
la00noix 8:2ba338b4590e 235 }
la00noix 9:7667dcfc7ce5 236 if(T1 == 4) { //三宝置き場からりんごの木まで移動
yuki0701 11:9db93bce4eef 237 gogo_straight(1,1,-2782,4837.5,-2782,5150,st_speed,turn_speed,5,0.1,10,0.1,800,-90);
yuki0701 11:9db93bce4eef 238 purecurve(2,1,1,-2850,5150,-2257,5500,9,turn_speed,5,0.1,10,0.1,800,-90);
yuki0701 11:9db93bce4eef 239 purecurve(1,1,1,-2257,5500,-1700,6000,9,turn_speed,5,0.1,10,0.1,800,-90);
yuki0701 11:9db93bce4eef 240 purecurve(3,1,1,-1700,6000,-2257,6550,9,turn_speed,5,0.1,10,0.1,800,-90);
yuki0701 12:762061580168 241 gogo_straight(1,1,-2257,6550,-2500,6605,turn_speed,end_speed,5,0.1,10,0.1,800,-90);
la00noix 9:7667dcfc7ce5 242 MaxonControl(0,0,0,0);
yuki0701 12:762061580168 243 set_cond(1,0,0,1,6038);
yuki0701 12:762061580168 244 pos_correction(-2500,6610,-90,1,0,8);
yuki0701 11:9db93bce4eef 245 set_cond(2,1,-3162,1,6038);
yuki0701 12:762061580168 246 gogo_straight(0,0,-2500,6610,-2700,6610,fruit_speed,fruit_speed,5,0.1,10,0.1,800,-90);
yuki0701 11:9db93bce4eef 247 MaxonControl(0,0,0,0);
la00noix 9:7667dcfc7ce5 248 enc_correction(1,1);
yuki0701 11:9db93bce4eef 249
la00noix 8:2ba338b4590e 250 T1++;
la00noix 8:2ba338b4590e 251 }
la00noix 8:2ba338b4590e 252 if(T1 == 5) {
la00noix 8:2ba338b4590e 253 while(1) {
la00noix 9:7667dcfc7ce5 254 wait(0.1);
yuki0701 11:9db93bce4eef 255 calc_xy(-90,0,0);
la00noix 8:2ba338b4590e 256 if(T1 == 6) {
yuki0701 11:9db93bce4eef 257 enc_correction(1,1);
la00noix 8:2ba338b4590e 258 break;
la00noix 8:2ba338b4590e 259 }
la00noix 8:2ba338b4590e 260 }
la00noix 8:2ba338b4590e 261 }
la00noix 9:7667dcfc7ce5 262 if(T1 == 6) { //りんごの木からお供え台まで移動
yuki0701 11:9db93bce4eef 263 set_cond(1,0,0,0,6962);
yuki0701 12:762061580168 264 gogo_straight(1,0,-2700,6610,-2500,6550,st_speed,max_speed,5,0.1,10,0.1,800,-90);
yuki0701 11:9db93bce4eef 265 gogo_straight(1,0,-2500,6550,-1300,6550,max_speed,max_speed,5,0.1,10,0.1,800,-90);
yuki0701 12:762061580168 266
yuki0701 11:9db93bce4eef 267 gogo_straight(1,0,-1300,6550,-850,6550,max_speed,turn_speed,5,0.1,10,0.1,800,-90);
yuki0701 11:9db93bce4eef 268 enc_correction(0,1);
yuki0701 11:9db93bce4eef 269 set_cond(2,0,-19,0,6962);
yuki0701 12:762061580168 270 purecurve(8,0,0,-850,6550,-620,6000,9,turn_speed,10,0.1,10,0.1,600,-180); //要修正
yuki0701 11:9db93bce4eef 271 enc_correction(1,0);
yuki0701 11:9db93bce4eef 272 set_cond(0,0,-19,0,0);
yuki0701 12:762061580168 273 gogo_straight(0,1,-620,6000,-620,5500,turn_speed,max_speed,5,0.1,10,0.1,600,-180);
yuki0701 12:762061580168 274 gogo_straight(0,1,-620,5500,-620,5000,max_speed,max_speed,5,0.1,10,0.1,600,-180);
la00noix 9:7667dcfc7ce5 275 set_cond(2,1,-1000,1,4000);
yuki0701 12:762061580168 276 gogo_straight(0,0,-620,5000,-620,4600,max_speed,end_speed,5,0.1,10,0.1,800,-180);
la00noix 9:7667dcfc7ce5 277 MaxonControl(0,0,0,0);
yuki0701 12:762061580168 278 pos_correction(-620,4370,-180,0,0,30);
la00noix 9:7667dcfc7ce5 279 enc_correction(1,1);
la00noix 9:7667dcfc7ce5 280 MaxonControl(0,0,0,0);
yuki0701 11:9db93bce4eef 281
la00noix 8:2ba338b4590e 282 T1++;
la00noix 8:2ba338b4590e 283 }
la00noix 9:7667dcfc7ce5 284 if(T1 == 7) {
la00noix 9:7667dcfc7ce5 285 while(1) {
la00noix 9:7667dcfc7ce5 286 printf("ashi finished\n\r");
la00noix 9:7667dcfc7ce5 287 MaxonControl(0,0,0,0);
yuki0701 11:9db93bce4eef 288
la00noix 9:7667dcfc7ce5 289 if(id1_value[0] != 1)break; //これらは他のwhileにも入れる必要あり
la00noix 9:7667dcfc7ce5 290 if(id1_value[6] != flag)break;
la00noix 9:7667dcfc7ce5 291 }
la00noix 9:7667dcfc7ce5 292 }
la00noix 6:26724c287387 293
yuki0701 11:9db93bce4eef 294
yuki0701 11:9db93bce4eef 295
la00noix 6:26724c287387 296 break;
la00noix 6:26724c287387 297 }
la00noix 6:26724c287387 298 //-----wait mode----------------------------------------------------------------------------------------------------------------------//
la00noix 6:26724c287387 299 case 0:
la00noix 6:26724c287387 300
yuki0701 11:9db93bce4eef 301 // printf("wait\n\r");
la00noix 6:26724c287387 302 calc_xy(0,1,1);
la00noix 6:26724c287387 303 ashi_led();
la00noix 6:26724c287387 304 MaxonControl(0,0,0,0);
la00noix 7:44ce34007499 305 go_waitmode = 0;
la00noix 6:26724c287387 306
la00noix 6:26724c287387 307 break;
la00noix 6:26724c287387 308 //-----manual mode--------------------------------------------------------------------------------------------------------------------//
la00noix 6:26724c287387 309 case 2:
la00noix 6:26724c287387 310
la00noix 6:26724c287387 311 ManualOut(250,100,500,200);
la00noix 7:44ce34007499 312 go_waitmode = 0;
la00noix 6:26724c287387 313
la00noix 6:26724c287387 314 break;
la00noix 0:c61c6e4775ca 315 }
la00noix 0:c61c6e4775ca 316 //------------------------------------------------------------------------------------------------------------------------------------//
la00noix 0:c61c6e4775ca 317 }
la00noix 0:c61c6e4775ca 318 }