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

3/19

Dependencies:   mbed move4wheel2 EC CruizCore_R1370P

main.cpp@11:9db93bce4eef, 2019-03-19 (annotated)

Committer:
yuki0701
Date:
Tue Mar 19 02:30:34 2019 +0000
Revision:
11:9db93bce4eef
Parent:
10:c741191360de 
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 11:9db93bce4eef 62 /*
yuki0701 10:c741191360de 63 gogo_straight(1,1,2962,3500,2962,2900,200,1000,5,0.1,10,0.1,600,0);
yuki0701 10:c741191360de 64 purecurve(6,1,1,2962,2900,2317,2500,9,1000,5,0.1,10,0.1,600,0);
yuki0701 10:c741191360de 65 purecurve(5,1,1,2317,2500,1672,2000,9,1000,5,0.1,10,0.1,600,0);
yuki0701 10:c741191360de 66 set_cond(2,1,1050,1,1076);
yuki0701 11:9db93bce4eef 67 gogo_straight(0,0,1672,2000,1672,1600,1000,200,5,0.1,10,0.1,600,0);
yuki0701 10:c741191360de 68 MaxonControl(0,0,0,0);
yuki0701 11:9db93bce4eef 69 pos_correction(1672,1600,0,0,0,25);
yuki0701 10:c741191360de 70 enc_correction(1,1);
yuki0701 11:9db93bce4eef 71 gogo_straight(0,0,1672,1600,1672,1400,200,200,5,0.1,10,0.1,600,0);
yuki0701 11:9db93bce4eef 72 */
yuki0701 11:9db93bce4eef 73 printf("t1 = 0\n\r");
yuki0701 11:9db93bce4eef 74 wait(1);
yuki0701 10:c741191360de 75 T1++;
yuki0701 10:c741191360de 76 }
yuki0701 10:c741191360de 77 if(T1 == 1) {
yuki0701 10:c741191360de 78 while(1) {
yuki0701 10:c741191360de 79 wait(0.1);
yuki0701 11:9db93bce4eef 80 //calc_xy(0,0,0);
yuki0701 10:c741191360de 81 if(T1 == 2) {
yuki0701 11:9db93bce4eef 82 // enc_correction(1,1);
yuki0701 10:c741191360de 83 break;
yuki0701 10:c741191360de 84 }
yuki0701 10:c741191360de 85 }
yuki0701 10:c741191360de 86 }
yuki0701 10:c741191360de 87 if(T1 == 2) { //みかんの木から三宝置き場まで移動
yuki0701 11:9db93bce4eef 88 /*gogo_straight(1,1,1672,1400,1672,2000,200,1000,5,0.1,10,0.1,600,0);
yuki0701 10:c741191360de 89 purecurve(2,1,1,1672,2000,2317,2500,9,1000,5,0.1,10,0.1,600,0);
yuki0701 10:c741191360de 90 purecurve(1,1,1,2317,2500,2962,3000,9,1000,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 10:c741191360de 91 gogo_straight(1,1,2962,3000,2962,4000,1000,1000,5,0.1,10,0.1,600,90);
yuki0701 10:c741191360de 92 gogo_straight(1,1,2962,4000,2962,4500,1000,200,5,0.1,10,0.1,600,90);
yuki0701 10:c741191360de 93 MaxonControl(0,0,0,0);
yuki0701 11:9db93bce4eef 94 // pos_correction(2962,4500,90,1,1);
yuki0701 10:c741191360de 95 set_cond(2,1,2462,0,6000);
yuki0701 10:c741191360de 96 gogo_straight(0,0,2962,4500,2850,4500,200,200,5,0.1,10,0.1,800,90);
yuki0701 10:c741191360de 97 MaxonControl(0,0,0,0);
yuki0701 11:9db93bce4eef 98 pos_correction(2850,4500,90,0,0,10);
yuki0701 10:c741191360de 99 enc_correction(1,1);
yuki0701 11:9db93bce4eef 100 wait(0.5);*/
yuki0701 11:9db93bce4eef 101 printf("t1 = 2\n\r");
yuki0701 11:9db93bce4eef 102 wait(1);
yuki0701 10:c741191360de 103 T1++;
yuki0701 10:c741191360de 104 }
yuki0701 10:c741191360de 105 if(T1 == 3) {
yuki0701 10:c741191360de 106 while(1) {
yuki0701 10:c741191360de 107 wait(0.1);
yuki0701 11:9db93bce4eef 108 //calc_xy(90,0,0);
yuki0701 10:c741191360de 109 if(T1 == 4) {
yuki0701 11:9db93bce4eef 110 // enc_correction(1,1);
yuki0701 10:c741191360de 111 break;
yuki0701 10:c741191360de 112 }
yuki0701 10:c741191360de 113 }
yuki0701 10:c741191360de 114 }
yuki0701 10:c741191360de 115 if(T1 == 4) { //三宝置き場からりんごの木まで移動
yuki0701 11:9db93bce4eef 116 /*gogo_straight(1,1,2850,4500,2850,5150,200,1000,5,0.1,10,0.1,800,90);
yuki0701 10:c741191360de 117 purecurve(3,1,1,2850,5150,2257,5500,9,1000,5,0.1,10,0.1,800,90);
yuki0701 10:c741191360de 118 purecurve(4,1,1,2257,5500,1700,6000,9,1000,5,0.1,10,0.1,800,90);
yuki0701 10:c741191360de 119 purecurve(2,1,1,1700,6000,2257,6550,9,1000,5,0.1,10,0.1,800,90);
yuki0701 10:c741191360de 120 gogo_straight(1,1,2257,6550,2500,6550,1000,1000,5,0.1,10,0.1,800,90);
yuki0701 10:c741191360de 121 set_cond(2,0,3500,1,6050);
yuki0701 10:c741191360de 122 gogo_straight(0,0,2500,6550,2700,6600,1000,200,5,0.1,10,0.1,800,90);
yuki0701 10:c741191360de 123 MaxonControl(0,0,0,0);
yuki0701 11:9db93bce4eef 124 pos_correction(2700,6600,90,0,0,15);
yuki0701 10:c741191360de 125 enc_correction(1,1);
yuki0701 11:9db93bce4eef 126 wait(0.5);*/
yuki0701 11:9db93bce4eef 127 printf("t1 = 4\n\r");
yuki0701 11:9db93bce4eef 128 wait(1);
yuki0701 10:c741191360de 129 T1++;
yuki0701 10:c741191360de 130 }
yuki0701 10:c741191360de 131 if(T1 == 5) {
yuki0701 10:c741191360de 132 while(1) {
yuki0701 10:c741191360de 133 wait(0.1);
yuki0701 11:9db93bce4eef 134 //calc_xy(90,0,0);
yuki0701 10:c741191360de 135 if(T1 == 6) {
yuki0701 11:9db93bce4eef 136 // enc_correction(1,1);
yuki0701 10:c741191360de 137 break;
yuki0701 10:c741191360de 138 }
yuki0701 10:c741191360de 139 }
yuki0701 10:c741191360de 140 }
yuki0701 10:c741191360de 141 if(T1 == 6) { //りんごの木からお供え台まで移動
yuki0701 11:9db93bce4eef 142 /*gogo_straight(1,1,-3112,3500,-2962,3100,st_speed,mid_speed,5,0.1,10,0.1,600,0);
yuki0701 11:9db93bce4eef 143 gogo_straight(1,1,-2962,3100,-2962,2900,mid_speed,turn_speed,5,0.1,10,0.1,600,0);
yuki0701 11:9db93bce4eef 144 purecurve(7,1,1,-2962,2900,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0);
yuki0701 11:9db93bce4eef 145 purecurve(8,1,1,-2317,2500,-1672,2000,9,turn_speed,5,0.1,10,0.1,600,0);
yuki0701 11:9db93bce4eef 146 set_cond(2,0,-1050,1,1076);
yuki0701 11:9db93bce4eef 147 gogo_straight(0,0,-1672,2000,-1672,1600,turn_speed,end_speed,5,0.1,10,0.1,600,0);
yuki0701 10:c741191360de 148 MaxonControl(0,0,0,0);
yuki0701 11:9db93bce4eef 149 pos_correction(-1672,1600,0,0,0,25);
yuki0701 10:c741191360de 150 enc_correction(1,1);
yuki0701 11:9db93bce4eef 151 gogo_straight(0,0,-1672,1600,-1672,1375,fruit_speed,fruit_speed,5,0.1,10,0.1,600,0);
yuki0701 10:c741191360de 152 MaxonControl(0,0,0,0);
yuki0701 11:9db93bce4eef 153 */
yuki0701 11:9db93bce4eef 154 printf("t1 = 6\n\r");
yuki0701 11:9db93bce4eef 155 wait(1);
yuki0701 10:c741191360de 156 T1++;
yuki0701 10:c741191360de 157 }
yuki0701 10:c741191360de 158 if(T1 == 7) {
yuki0701 10:c741191360de 159 while(1) {
yuki0701 10:c741191360de 160 printf("ashi finished\n\r");
yuki0701 10:c741191360de 161 MaxonControl(0,0,0,0);
yuki0701 10:c741191360de 162 if(id1_value[0] != 1)break; //これらは他のwhileにも入れる必要あり
yuki0701 10:c741191360de 163 if(id1_value[6] != flag)break;
yuki0701 10:c741191360de 164 }
yuki0701 10:c741191360de 165 }
yuki0701 10:c741191360de 166 break;
yuki0701 11:9db93bce4eef 167
yuki0701 10:c741191360de 168 case 1:
yuki0701 10:c741191360de 169 //-----left mode--------------------------------------------------------------------------------------------------------------//
yuki0701 10:c741191360de 170 flag = 1;
yuki0701 11:9db93bce4eef 171 printf("start\n\r");
yuki0701 10:c741191360de 172 UserLoopSetting_enc_left();
la00noix 9:7667dcfc7ce5 173 if(T1 == 0) { //スタート位置からみかんの木まで移動
yuki0701 11:9db93bce4eef 174
yuki0701 11:9db93bce4eef 175 gogo_straight(1,1,-3112,3500,-2962,3100,st_speed,mid_speed,5,0.1,10,0.1,600,0);
yuki0701 11:9db93bce4eef 176 gogo_straight(1,1,-2962,3100,-2962,2900,mid_speed,turn_speed,5,0.1,10,0.1,600,0);
yuki0701 11:9db93bce4eef 177 purecurve(7,1,1,-2962,2900,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0);
yuki0701 11:9db93bce4eef 178 purecurve(8,1,1,-2317,2500,-1672,2000,9,turn_speed,5,0.1,10,0.1,600,0);
la00noix 9:7667dcfc7ce5 179 set_cond(2,0,-1050,1,1076);
yuki0701 11:9db93bce4eef 180 gogo_straight(0,0,-1672,2000,-1672,1600,turn_speed,end_speed,5,0.1,10,0.1,600,0);
la00noix 9:7667dcfc7ce5 181 MaxonControl(0,0,0,0);
yuki0701 11:9db93bce4eef 182 pos_correction(-1672,1600,0,0,0,25);
la00noix 9:7667dcfc7ce5 183 enc_correction(1,1);
yuki0701 11:9db93bce4eef 184 gogo_straight(0,0,-1672,1600,-1672,1375,fruit_speed,fruit_speed,5,0.1,10,0.1,600,0);
yuki0701 11:9db93bce4eef 185 MaxonControl(0,0,0,0);
yuki0701 11:9db93bce4eef 186
la00noix 8:2ba338b4590e 187 T1++;
la00noix 8:2ba338b4590e 188 }
la00noix 8:2ba338b4590e 189 if(T1 == 1) {
la00noix 8:2ba338b4590e 190 while(1) {
la00noix 9:7667dcfc7ce5 191 wait(0.1);
yuki0701 11:9db93bce4eef 192 calc_xy(0,0,0);
la00noix 8:2ba338b4590e 193 if(T1 == 2) {
yuki0701 11:9db93bce4eef 194 enc_correction(1,1);
la00noix 8:2ba338b4590e 195 break;
la00noix 8:2ba338b4590e 196 }
la00noix 8:2ba338b4590e 197 }
la00noix 8:2ba338b4590e 198 }
la00noix 9:7667dcfc7ce5 199 if(T1 == 2) { //みかんの木から三宝置き場まで移動
yuki0701 11:9db93bce4eef 200 gogo_straight(1,1,-1672,1375,-1672,2000,st_speed,turn_speed,5,0.1,10,0.1,600,0);
yuki0701 11:9db93bce4eef 201 purecurve(3,1,1,-1672,2000,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0);
yuki0701 11:9db93bce4eef 202 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 203 gogo_straight(1,1,-2962,3000,-2962,4000,turn_speed,max_speed,5,0.1,10,0.1,600,-90);
yuki0701 11:9db93bce4eef 204 gogo_straight(1,1,-2962,4000,-2962,4820,max_speed,end_speed,5,0.1,10,0.1,600,-90);
la00noix 9:7667dcfc7ce5 205 MaxonControl(0,0,0,0);
la00noix 9:7667dcfc7ce5 206 set_cond(2,0,-2462,0,6000);
yuki0701 11:9db93bce4eef 207 pos_correction(-2962,4837.5,-90,0,0,7);
yuki0701 11:9db93bce4eef 208 gogo_straight(0,0,-2962,4837.5,-2800,4837.5,300,200,5,0.1,10,0.1,800,-90);
la00noix 9:7667dcfc7ce5 209 MaxonControl(0,0,0,0);
la00noix 9:7667dcfc7ce5 210 enc_correction(1,1);
yuki0701 11:9db93bce4eef 211
la00noix 8:2ba338b4590e 212 T1++;
la00noix 8:2ba338b4590e 213 }
la00noix 8:2ba338b4590e 214 if(T1 == 3) {
la00noix 8:2ba338b4590e 215 while(1) {
la00noix 9:7667dcfc7ce5 216 wait(0.1);
yuki0701 11:9db93bce4eef 217 calc_xy(-90,0,0);
la00noix 8:2ba338b4590e 218 if(T1 == 4) {
yuki0701 11:9db93bce4eef 219 enc_correction(1,1);
la00noix 8:2ba338b4590e 220 break;
la00noix 8:2ba338b4590e 221 }
la00noix 8:2ba338b4590e 222 }
la00noix 8:2ba338b4590e 223 }
la00noix 9:7667dcfc7ce5 224 if(T1 == 4) { //三宝置き場からりんごの木まで移動
yuki0701 11:9db93bce4eef 225 gogo_straight(1,1,-2782,4837.5,-2782,5150,st_speed,turn_speed,5,0.1,10,0.1,800,-90);
yuki0701 11:9db93bce4eef 226 purecurve(2,1,1,-2850,5150,-2257,5500,9,turn_speed,5,0.1,10,0.1,800,-90);
yuki0701 11:9db93bce4eef 227 purecurve(1,1,1,-2257,5500,-1700,6000,9,turn_speed,5,0.1,10,0.1,800,-90);
yuki0701 11:9db93bce4eef 228 purecurve(3,1,1,-1700,6000,-2257,6550,9,turn_speed,5,0.1,10,0.1,800,-90);
yuki0701 11:9db93bce4eef 229 gogo_straight(1,1,-2257,6550,-2500,6593,turn_speed,end_speed,5,0.1,10,0.1,800,-90);
la00noix 9:7667dcfc7ce5 230 MaxonControl(0,0,0,0);
yuki0701 11:9db93bce4eef 231 set_cond(1,0,0,1,6050);
yuki0701 11:9db93bce4eef 232 pos_correction(-2500,6600,-90,1,0,8);
yuki0701 11:9db93bce4eef 233 set_cond(2,1,-3162,1,6038);
yuki0701 11:9db93bce4eef 234 gogo_straight(0,0,-2500,6600,-2700,6600,fruit_speed,fruit_speed,5,0.1,10,0.1,800,-90);
yuki0701 11:9db93bce4eef 235 MaxonControl(0,0,0,0);
la00noix 9:7667dcfc7ce5 236 enc_correction(1,1);
yuki0701 11:9db93bce4eef 237
la00noix 8:2ba338b4590e 238 T1++;
la00noix 8:2ba338b4590e 239 }
la00noix 8:2ba338b4590e 240 if(T1 == 5) {
la00noix 8:2ba338b4590e 241 while(1) {
la00noix 9:7667dcfc7ce5 242 wait(0.1);
yuki0701 11:9db93bce4eef 243 calc_xy(-90,0,0);
la00noix 8:2ba338b4590e 244 if(T1 == 6) {
yuki0701 11:9db93bce4eef 245 enc_correction(1,1);
la00noix 8:2ba338b4590e 246 break;
la00noix 8:2ba338b4590e 247 }
la00noix 8:2ba338b4590e 248 }
la00noix 8:2ba338b4590e 249 }
la00noix 9:7667dcfc7ce5 250 if(T1 == 6) { //りんごの木からお供え台まで移動
yuki0701 11:9db93bce4eef 251 set_cond(1,0,0,0,6962);
yuki0701 11:9db93bce4eef 252 gogo_straight(1,0,-2700,6600,-2500,6550,st_speed,max_speed,5,0.1,10,0.1,800,-90);
yuki0701 11:9db93bce4eef 253 gogo_straight(1,0,-2500,6550,-1300,6550,max_speed,max_speed,5,0.1,10,0.1,800,-90);
yuki0701 11:9db93bce4eef 254 gogo_straight(1,0,-1300,6550,-850,6550,max_speed,turn_speed,5,0.1,10,0.1,800,-90);
yuki0701 11:9db93bce4eef 255 enc_correction(0,1);
yuki0701 11:9db93bce4eef 256 set_cond(2,0,-19,0,6962);
yuki0701 11:9db93bce4eef 257 purecurve(8,0,0,-850,6550,-519,6000,9,turn_speed,10,0.1,10,0.1,600,-180);
yuki0701 11:9db93bce4eef 258 enc_correction(1,0);
yuki0701 11:9db93bce4eef 259 set_cond(0,0,-19,0,0);
yuki0701 11:9db93bce4eef 260 gogo_straight(0,1,-519,6000,-519,5500,turn_speed,max_speed,5,0.1,10,0.1,600,-180);
yuki0701 11:9db93bce4eef 261 gogo_straight(0,1,-519,5500,-519,5000,max_speed,max_speed,5,0.1,10,0.1,600,-180);
la00noix 9:7667dcfc7ce5 262 set_cond(2,1,-1000,1,4000);
yuki0701 11:9db93bce4eef 263 gogo_straight(0,0,-519,5000,-519,4600,max_speed,end_speed,5,0.1,10,0.1,800,-180);
la00noix 9:7667dcfc7ce5 264 MaxonControl(0,0,0,0);
yuki0701 11:9db93bce4eef 265 pos_correction(-519,4370,-180,0,0,30);
la00noix 9:7667dcfc7ce5 266 enc_correction(1,1);
la00noix 9:7667dcfc7ce5 267 MaxonControl(0,0,0,0);
yuki0701 11:9db93bce4eef 268
la00noix 8:2ba338b4590e 269 T1++;
la00noix 8:2ba338b4590e 270 }
la00noix 9:7667dcfc7ce5 271 if(T1 == 7) {
la00noix 9:7667dcfc7ce5 272 while(1) {
la00noix 9:7667dcfc7ce5 273 printf("ashi finished\n\r");
la00noix 9:7667dcfc7ce5 274 MaxonControl(0,0,0,0);
yuki0701 11:9db93bce4eef 275
yuki0701 11:9db93bce4eef 276 /*gogo_straight(1,1,-519,4500,-519,5000,200,1000,5,0.1,10,0.1,600,-180);
yuki0701 11:9db93bce4eef 277 gogo_straight(1,1,-519,5000,-519,6000,1000,1000,5,0.1,10,0.1,600,-180);
yuki0701 11:9db93bce4eef 278 gogo_straight(1,1,-519,6000,-519,6600,1000,200,5,0.1,10,0.1,600,-180);
yuki0701 11:9db93bce4eef 279 MaxonControl(0,0,0,0);
yuki0701 11:9db93bce4eef 280 set_cond(2,0,0,0,7000);
yuki0701 11:9db93bce4eef 281 //pos_correction(-519,6600,-180,0,0,30);
yuki0701 11:9db93bce4eef 282 calc_xy(-180,0,0);
yuki0701 11:9db93bce4eef 283 enc_correction(1,1);
yuki0701 11:9db93bce4eef 284 gogo_straight(1,1,-519,6600,-1000,6600,200,1000,5,0.1,10,0.1,600,-180);
yuki0701 11:9db93bce4eef 285 purecurve(5,1,1,-1000,6600,-1672,6000,9,1000,5,0.1,10,0.1,600,-180);
yuki0701 11:9db93bce4eef 286 purecurve(6,1,1,-1672,6000,-2462,5500,9,1000,5,0.1,10,0.1,600,-180);
yuki0701 11:9db93bce4eef 287 gogo_straight(1,1,-2462,5500,-2962,5500,1000,200,5,0.1,10,0.1,600,-180);
yuki0701 11:9db93bce4eef 288 MaxonControl(0,0,0,0);
yuki0701 11:9db93bce4eef 289 set_cond(2,1,-3500,0,6000);
yuki0701 11:9db93bce4eef 290 //pos_correction(-2962,5500,-180,0,0,30);
yuki0701 11:9db93bce4eef 291 enc_correction(1,1);
yuki0701 11:9db93bce4eef 292 calc_xy(-180,0,0);
yuki0701 11:9db93bce4eef 293 gogo_straight(1,1,-2962,5500,-2962,4700,200,1000,5,0.1,10,0.1,600,-90);
yuki0701 11:9db93bce4eef 294 gogo_straight(1,1,-2962,4700,-2962,4000,1000,1000,5,0.1,10,0.1,600,0);
yuki0701 11:9db93bce4eef 295 gogo_straight(1,1,-2962,4000,-2962,3500,1000,200,5,0.1,10,0.1,600,0);
yuki0701 11:9db93bce4eef 296 MaxonControl(0,0,0,0);
yuki0701 11:9db93bce4eef 297 set_cond(2,1,-3500,1,2000);
yuki0701 11:9db93bce4eef 298 pos_correction(-3112,3500,0,0,0,15);
yuki0701 11:9db93bce4eef 299 enc_correction(1,1);
yuki0701 11:9db93bce4eef 300 MaxonControl(0,0,0,0);*/
yuki0701 11:9db93bce4eef 301
la00noix 9:7667dcfc7ce5 302 if(id1_value[0] != 1)break; //これらは他のwhileにも入れる必要あり
la00noix 9:7667dcfc7ce5 303 if(id1_value[6] != flag)break;
la00noix 9:7667dcfc7ce5 304 }
la00noix 9:7667dcfc7ce5 305 }
la00noix 6:26724c287387 306
yuki0701 11:9db93bce4eef 307
yuki0701 11:9db93bce4eef 308
la00noix 6:26724c287387 309 break;
la00noix 6:26724c287387 310 }
la00noix 6:26724c287387 311 //-----wait mode----------------------------------------------------------------------------------------------------------------------//
la00noix 6:26724c287387 312 case 0:
la00noix 6:26724c287387 313
yuki0701 11:9db93bce4eef 314 // printf("wait\n\r");
la00noix 6:26724c287387 315 calc_xy(0,1,1);
la00noix 6:26724c287387 316 ashi_led();
la00noix 6:26724c287387 317 MaxonControl(0,0,0,0);
la00noix 7:44ce34007499 318 go_waitmode = 0;
la00noix 6:26724c287387 319
la00noix 6:26724c287387 320 break;
la00noix 6:26724c287387 321 //-----manual mode--------------------------------------------------------------------------------------------------------------------//
la00noix 6:26724c287387 322 case 2:
la00noix 6:26724c287387 323
la00noix 6:26724c287387 324 ManualOut(250,100,500,200);
la00noix 7:44ce34007499 325 go_waitmode = 0;
la00noix 6:26724c287387 326
la00noix 6:26724c287387 327 break;
la00noix 0:c61c6e4775ca 328 }
la00noix 0:c61c6e4775ca 329 //------------------------------------------------------------------------------------------------------------------------------------//
la00noix 0:c61c6e4775ca 330 }
la00noix 0:c61c6e4775ca 331 }