3/20 13:39
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
main.cpp@13:f9867c3b97b0, 2019-03-19 (annotated)
- Committer:
- yuki0701
- Date:
- Tue Mar 19 17:03:27 2019 +0000
- Revision:
- 13:f9867c3b97b0
- Parent:
- 12:762061580168
- Child:
- 14:6c1b36c55915
a
Who changed what in which revision?
User | Revision | Line number | New 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 | 13:f9867c3b97b0 | 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: |
yuki0701 | 13:f9867c3b97b0 | 310 | switch(id1_value[6]) { |
yuki0701 | 13:f9867c3b97b0 | 311 | case 0: |
yuki0701 | 13:f9867c3b97b0 | 312 | //-----right mode-------------------------------------------------------------------------------------------------------------// |
yuki0701 | 13:f9867c3b97b0 | 313 | //未修正 |
la00noix | 6:26724c287387 | 314 | |
yuki0701 | 13:f9867c3b97b0 | 315 | flag = 0; |
yuki0701 | 13:f9867c3b97b0 | 316 | UserLoopSetting_enc_right(); |
yuki0701 | 13:f9867c3b97b0 | 317 | if(T1 == 0) { //スタート位置からみかんの木まで移動 |
yuki0701 | 13:f9867c3b97b0 | 318 | |
yuki0701 | 13:f9867c3b97b0 | 319 | gogo_straight(1,1,3112,3500,2962,3100,st_speed,mid_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 13:f9867c3b97b0 | 320 | gogo_straight(1,1,2962,3100,2962,2900,mid_speed,turn_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 13:f9867c3b97b0 | 321 | purecurve(6,1,1,2962,2900,2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 13:f9867c3b97b0 | 322 | purecurve(5,1,1,2317,2500,1672,2000,9,turn_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 13:f9867c3b97b0 | 323 | set_cond(2,1,1050,1,1076); |
yuki0701 | 13:f9867c3b97b0 | 324 | gogo_straight(0,0,1672,2000,1672,1600,turn_speed,end_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 13:f9867c3b97b0 | 325 | MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 326 | pos_correction(1672,1600,0,0,0,25); |
yuki0701 | 13:f9867c3b97b0 | 327 | enc_correction(1,1); |
yuki0701 | 13:f9867c3b97b0 | 328 | gogo_straight(0,0,1672,1600,1672,1375,fruit_speed,fruit_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 13:f9867c3b97b0 | 329 | MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 330 | |
yuki0701 | 13:f9867c3b97b0 | 331 | //printf("t1 = 0\n\r"); |
yuki0701 | 13:f9867c3b97b0 | 332 | //wait(1); |
yuki0701 | 13:f9867c3b97b0 | 333 | T1++; |
yuki0701 | 13:f9867c3b97b0 | 334 | } |
yuki0701 | 13:f9867c3b97b0 | 335 | if(T1 == 1) { |
yuki0701 | 13:f9867c3b97b0 | 336 | while(1) { |
yuki0701 | 13:f9867c3b97b0 | 337 | ManualOut(250,100,500,200); |
yuki0701 | 13:f9867c3b97b0 | 338 | //calc_xy(0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 339 | if(T1 == 2) { |
yuki0701 | 13:f9867c3b97b0 | 340 | // enc_correction(1,1); |
yuki0701 | 13:f9867c3b97b0 | 341 | break; |
yuki0701 | 13:f9867c3b97b0 | 342 | } |
yuki0701 | 13:f9867c3b97b0 | 343 | } |
yuki0701 | 13:f9867c3b97b0 | 344 | } |
yuki0701 | 13:f9867c3b97b0 | 345 | if(T1 == 2) { //みかんの木から三宝置き場まで移動 |
yuki0701 | 13:f9867c3b97b0 | 346 | |
yuki0701 | 13:f9867c3b97b0 | 347 | gogo_straight(1,1,1672,1375,1672,2000,st_speed,turn_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 13:f9867c3b97b0 | 348 | purecurve(2,1,1,1672,2000,2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 13:f9867c3b97b0 | 349 | purecurve(1,1,1,2317,2500,2962,3000,9,turn_speed,5,0.1,10,0.1,600,90); |
yuki0701 | 13:f9867c3b97b0 | 350 | gogo_straight(1,1,2962,3000,2962,4000,turn_speed,max_speed,5,0.1,10,0.1,600,90); |
yuki0701 | 13:f9867c3b97b0 | 351 | gogo_straight(1,1,2962,4000,2962,4810,max_speed,end_speed,5,0.1,10,0.1,600,90); |
yuki0701 | 13:f9867c3b97b0 | 352 | MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 353 | set_cond(2,1,2462,0,6000); |
yuki0701 | 13:f9867c3b97b0 | 354 | pos_correction(2962,4837.5,90,0,0,10); |
yuki0701 | 13:f9867c3b97b0 | 355 | gogo_straight(0,0,2962,4837.5,2782,4837.5,300,200,5,0.1,10,0.1,800,90); |
yuki0701 | 13:f9867c3b97b0 | 356 | MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 357 | enc_correction(1,1); |
yuki0701 | 13:f9867c3b97b0 | 358 | |
yuki0701 | 13:f9867c3b97b0 | 359 | //printf("t1 = 2\n\r"); |
yuki0701 | 13:f9867c3b97b0 | 360 | //wait(1); |
yuki0701 | 13:f9867c3b97b0 | 361 | T1++; |
yuki0701 | 13:f9867c3b97b0 | 362 | } |
yuki0701 | 13:f9867c3b97b0 | 363 | if(T1 == 3) { |
yuki0701 | 13:f9867c3b97b0 | 364 | while(1) { |
yuki0701 | 13:f9867c3b97b0 | 365 | ManualOut(250,100,500,200); |
yuki0701 | 13:f9867c3b97b0 | 366 | //calc_xy(90,0,0); |
yuki0701 | 13:f9867c3b97b0 | 367 | if(T1 == 4) { |
yuki0701 | 13:f9867c3b97b0 | 368 | // enc_correction(1,1); |
yuki0701 | 13:f9867c3b97b0 | 369 | break; |
yuki0701 | 13:f9867c3b97b0 | 370 | } |
yuki0701 | 13:f9867c3b97b0 | 371 | } |
yuki0701 | 13:f9867c3b97b0 | 372 | } |
yuki0701 | 13:f9867c3b97b0 | 373 | if(T1 == 4) { //三宝置き場からりんごの木まで移動 |
yuki0701 | 13:f9867c3b97b0 | 374 | |
yuki0701 | 13:f9867c3b97b0 | 375 | gogo_straight(1,1,2782,4837.5,2782,5150,st_speed,turn_speed,5,0.1,10,0.1,800,90); |
yuki0701 | 13:f9867c3b97b0 | 376 | purecurve(3,1,1,2850,5150,2257,5500,9,turn_speed,5,0.1,10,0.1,800,90); |
yuki0701 | 13:f9867c3b97b0 | 377 | purecurve(4,1,1,2257,5500,1700,6000,9,turn_speed,5,0.1,10,0.1,800,90); |
yuki0701 | 13:f9867c3b97b0 | 378 | purecurve(2,1,1,1700,6000,2257,6550,9,turn_speed,5,0.1,10,0.1,800,90); |
yuki0701 | 13:f9867c3b97b0 | 379 | gogo_straight(1,1,2257,6550,2500,6605,turn_speed,end_speed,5,0.1,10,0.1,800,90); |
yuki0701 | 13:f9867c3b97b0 | 380 | MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 381 | set_cond(1,0,0,1,6038); |
yuki0701 | 13:f9867c3b97b0 | 382 | pos_correction(2500,6610,90,1,0,8); |
yuki0701 | 13:f9867c3b97b0 | 383 | set_cond(2,0,3162,1,6038); |
yuki0701 | 13:f9867c3b97b0 | 384 | gogo_straight(0,0,2500,6610,2700,6610,fruit_speed,fruit_speed,5,0.1,10,0.1,800,90); |
yuki0701 | 13:f9867c3b97b0 | 385 | MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 386 | enc_correction(1,1); |
yuki0701 | 13:f9867c3b97b0 | 387 | |
yuki0701 | 13:f9867c3b97b0 | 388 | // printf("t1 = 4\n\r"); |
yuki0701 | 13:f9867c3b97b0 | 389 | // wait(1); |
yuki0701 | 13:f9867c3b97b0 | 390 | T1++; |
yuki0701 | 13:f9867c3b97b0 | 391 | } |
yuki0701 | 13:f9867c3b97b0 | 392 | if(T1 == 5) { |
yuki0701 | 13:f9867c3b97b0 | 393 | while(1) { |
yuki0701 | 13:f9867c3b97b0 | 394 | ManualOut(250,100,500,200); |
yuki0701 | 13:f9867c3b97b0 | 395 | //calc_xy(90,0,0); |
yuki0701 | 13:f9867c3b97b0 | 396 | if(T1 == 6) { |
yuki0701 | 13:f9867c3b97b0 | 397 | // enc_correction(1,1); |
yuki0701 | 13:f9867c3b97b0 | 398 | break; |
yuki0701 | 13:f9867c3b97b0 | 399 | } |
yuki0701 | 13:f9867c3b97b0 | 400 | } |
yuki0701 | 13:f9867c3b97b0 | 401 | } |
yuki0701 | 13:f9867c3b97b0 | 402 | if(T1 == 6) { //りんごの木からお供え台まで移動 |
yuki0701 | 13:f9867c3b97b0 | 403 | |
yuki0701 | 13:f9867c3b97b0 | 404 | set_cond(1,0,0,0,6962); |
yuki0701 | 13:f9867c3b97b0 | 405 | gogo_straight(1,0,2700,6610,2500,6550,st_speed,max_speed,5,0.1,10,0.1,800,90); |
yuki0701 | 13:f9867c3b97b0 | 406 | gogo_straight(1,0,2500,6550,1300,6550,max_speed,max_speed,5,0.1,10,0.1,800,90); |
yuki0701 | 13:f9867c3b97b0 | 407 | gogo_straight(1,0,1300,6550,850,6550,max_speed,turn_speed,5,0.1,10,0.1,800,90); |
yuki0701 | 13:f9867c3b97b0 | 408 | enc_correction(0,1); |
yuki0701 | 13:f9867c3b97b0 | 409 | set_cond(2,1,19,0,6962); //要修正 |
yuki0701 | 13:f9867c3b97b0 | 410 | purecurve(5,0,0,850,6550,620,6000,9,turn_speed,10,0.1,10,0.1,600,180); //要修正 |
yuki0701 | 13:f9867c3b97b0 | 411 | enc_correction(1,0); |
yuki0701 | 13:f9867c3b97b0 | 412 | set_cond(0,1,19,0,0); |
yuki0701 | 13:f9867c3b97b0 | 413 | gogo_straight(0,1,620,6000,620,5500,turn_speed,max_speed,5,0.1,10,0.1,600,180); |
yuki0701 | 13:f9867c3b97b0 | 414 | gogo_straight(0,1,620,5500,620,5000,max_speed,max_speed,5,0.1,10,0.1,600,180); |
yuki0701 | 13:f9867c3b97b0 | 415 | set_cond(2,0,1000,1,4000); |
yuki0701 | 13:f9867c3b97b0 | 416 | gogo_straight(0,0,620,5000,620,4600,max_speed,end_speed,5,0.1,10,0.1,800,180); |
yuki0701 | 13:f9867c3b97b0 | 417 | MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 418 | pos_correction(620,4370,180,0,0,30); |
yuki0701 | 13:f9867c3b97b0 | 419 | enc_correction(1,1); |
yuki0701 | 13:f9867c3b97b0 | 420 | MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 421 | |
yuki0701 | 13:f9867c3b97b0 | 422 | // printf("t1 = 6\n\r"); |
yuki0701 | 13:f9867c3b97b0 | 423 | // wait(1); |
yuki0701 | 13:f9867c3b97b0 | 424 | T1++; |
yuki0701 | 13:f9867c3b97b0 | 425 | } |
yuki0701 | 13:f9867c3b97b0 | 426 | if(T1 == 7) { |
yuki0701 | 13:f9867c3b97b0 | 427 | while(1) { |
yuki0701 | 13:f9867c3b97b0 | 428 | ManualOut(250,100,500,200); |
yuki0701 | 13:f9867c3b97b0 | 429 | //printf("ashi finished\n\r"); |
yuki0701 | 13:f9867c3b97b0 | 430 | //MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 431 | if(id1_value[0] != 1)break; //これらは他のwhileにも入れる必要あり |
yuki0701 | 13:f9867c3b97b0 | 432 | if(id1_value[6] != flag)break; |
yuki0701 | 13:f9867c3b97b0 | 433 | } |
yuki0701 | 13:f9867c3b97b0 | 434 | } |
yuki0701 | 13:f9867c3b97b0 | 435 | break; |
yuki0701 | 13:f9867c3b97b0 | 436 | |
yuki0701 | 13:f9867c3b97b0 | 437 | case 1: |
yuki0701 | 13:f9867c3b97b0 | 438 | //-----left mode--------------------------------------------------------------------------------------------------------------// |
yuki0701 | 13:f9867c3b97b0 | 439 | flag = 1; |
yuki0701 | 13:f9867c3b97b0 | 440 | printf("start\n\r"); |
yuki0701 | 13:f9867c3b97b0 | 441 | UserLoopSetting_enc_left(); |
yuki0701 | 13:f9867c3b97b0 | 442 | if(T1 == 0) { //スタート位置からみかんの木まで移動 |
yuki0701 | 13:f9867c3b97b0 | 443 | |
yuki0701 | 13:f9867c3b97b0 | 444 | gogo_straight(1,1,-3112,3500,-2962,3100,st_speed,mid_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 13:f9867c3b97b0 | 445 | gogo_straight(1,1,-2962,3100,-2962,2900,mid_speed,turn_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 13:f9867c3b97b0 | 446 | purecurve(7,1,1,-2962,2900,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 13:f9867c3b97b0 | 447 | purecurve(8,1,1,-2317,2500,-1672,2000,9,turn_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 13:f9867c3b97b0 | 448 | set_cond(2,0,-1050,1,1076); |
yuki0701 | 13:f9867c3b97b0 | 449 | gogo_straight(0,0,-1672,2000,-1672,1600,turn_speed,end_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 13:f9867c3b97b0 | 450 | MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 451 | pos_correction(-1672,1600,0,0,0,25); |
yuki0701 | 13:f9867c3b97b0 | 452 | enc_correction(1,1); |
yuki0701 | 13:f9867c3b97b0 | 453 | gogo_straight(0,0,-1672,1600,-1672,1375,fruit_speed,fruit_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 13:f9867c3b97b0 | 454 | MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 455 | |
yuki0701 | 13:f9867c3b97b0 | 456 | T1++; |
yuki0701 | 13:f9867c3b97b0 | 457 | } |
yuki0701 | 13:f9867c3b97b0 | 458 | |
yuki0701 | 13:f9867c3b97b0 | 459 | if(T1 == 1) { |
yuki0701 | 13:f9867c3b97b0 | 460 | while(1) { |
yuki0701 | 13:f9867c3b97b0 | 461 | ManualOut(250,100,500,200); |
yuki0701 | 13:f9867c3b97b0 | 462 | if(T1 == 2) { |
yuki0701 | 13:f9867c3b97b0 | 463 | break; |
yuki0701 | 13:f9867c3b97b0 | 464 | } |
yuki0701 | 13:f9867c3b97b0 | 465 | } |
yuki0701 | 13:f9867c3b97b0 | 466 | } |
yuki0701 | 13:f9867c3b97b0 | 467 | if(T1 == 2) { //みかんの木から三宝置き場まで移動 |
yuki0701 | 13:f9867c3b97b0 | 468 | gogo_straight(1,1,-1672,1375,-1672,2000,st_speed,turn_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 13:f9867c3b97b0 | 469 | purecurve(3,1,1,-1672,2000,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 13:f9867c3b97b0 | 470 | 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 | 13:f9867c3b97b0 | 471 | gogo_straight(1,1,-2962,3000,-2962,4000,turn_speed,max_speed,5,0.1,10,0.1,600,-90); |
yuki0701 | 13:f9867c3b97b0 | 472 | gogo_straight(1,1,-2962,4000,-2962,4810,max_speed,end_speed,5,0.1,10,0.1,600,-90); |
yuki0701 | 13:f9867c3b97b0 | 473 | MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 474 | set_cond(2,0,-2462,0,6000); |
yuki0701 | 13:f9867c3b97b0 | 475 | pos_correction(-2962,4837.5,-90,0,0,10); |
yuki0701 | 13:f9867c3b97b0 | 476 | gogo_straight(0,0,-2962,4837.5,-2782,4837.5,300,200,5,0.1,10,0.1,800,-90); |
yuki0701 | 13:f9867c3b97b0 | 477 | MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 478 | enc_correction(1,1); |
yuki0701 | 13:f9867c3b97b0 | 479 | |
yuki0701 | 13:f9867c3b97b0 | 480 | T1++; |
yuki0701 | 13:f9867c3b97b0 | 481 | } |
yuki0701 | 13:f9867c3b97b0 | 482 | if(T1 == 3) { |
yuki0701 | 13:f9867c3b97b0 | 483 | while(1) { |
yuki0701 | 13:f9867c3b97b0 | 484 | ManualOut(250,100,500,200); |
yuki0701 | 13:f9867c3b97b0 | 485 | if(T1 == 4) { |
yuki0701 | 13:f9867c3b97b0 | 486 | break; |
yuki0701 | 13:f9867c3b97b0 | 487 | } |
yuki0701 | 13:f9867c3b97b0 | 488 | } |
yuki0701 | 13:f9867c3b97b0 | 489 | } |
yuki0701 | 13:f9867c3b97b0 | 490 | if(T1 == 4) { //三宝置き場からりんごの木まで移動 |
yuki0701 | 13:f9867c3b97b0 | 491 | gogo_straight(1,1,-2782,4837.5,-2782,5150,st_speed,turn_speed,5,0.1,10,0.1,800,-90); |
yuki0701 | 13:f9867c3b97b0 | 492 | purecurve(2,1,1,-2850,5150,-2257,5500,9,turn_speed,5,0.1,10,0.1,800,-90); |
yuki0701 | 13:f9867c3b97b0 | 493 | purecurve(1,1,1,-2257,5500,-1700,6000,9,turn_speed,5,0.1,10,0.1,800,-90); |
yuki0701 | 13:f9867c3b97b0 | 494 | purecurve(3,1,1,-1700,6000,-2257,6550,9,turn_speed,5,0.1,10,0.1,800,-90); |
yuki0701 | 13:f9867c3b97b0 | 495 | gogo_straight(1,1,-2257,6550,-2500,6605,turn_speed,end_speed,5,0.1,10,0.1,800,-90); |
yuki0701 | 13:f9867c3b97b0 | 496 | MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 497 | set_cond(1,0,0,1,6038); |
yuki0701 | 13:f9867c3b97b0 | 498 | pos_correction(-2500,6610,-90,1,0,8); |
yuki0701 | 13:f9867c3b97b0 | 499 | set_cond(2,1,-3162,1,6038); |
yuki0701 | 13:f9867c3b97b0 | 500 | gogo_straight(0,0,-2500,6610,-2700,6610,fruit_speed,fruit_speed,5,0.1,10,0.1,800,-90); |
yuki0701 | 13:f9867c3b97b0 | 501 | MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 502 | enc_correction(1,1); |
yuki0701 | 13:f9867c3b97b0 | 503 | |
yuki0701 | 13:f9867c3b97b0 | 504 | T1++; |
yuki0701 | 13:f9867c3b97b0 | 505 | } |
yuki0701 | 13:f9867c3b97b0 | 506 | if(T1 == 5) { |
yuki0701 | 13:f9867c3b97b0 | 507 | while(1) { |
yuki0701 | 13:f9867c3b97b0 | 508 | ManualOut(250,100,500,200); |
yuki0701 | 13:f9867c3b97b0 | 509 | if(T1 == 6) { |
yuki0701 | 13:f9867c3b97b0 | 510 | break; |
yuki0701 | 13:f9867c3b97b0 | 511 | } |
yuki0701 | 13:f9867c3b97b0 | 512 | } |
yuki0701 | 13:f9867c3b97b0 | 513 | } |
yuki0701 | 13:f9867c3b97b0 | 514 | if(T1 == 6) { //りんごの木からお供え台まで移動 |
yuki0701 | 13:f9867c3b97b0 | 515 | set_cond(1,0,0,0,6962); |
yuki0701 | 13:f9867c3b97b0 | 516 | gogo_straight(1,0,-2700,6610,-2500,6550,st_speed,max_speed,5,0.1,10,0.1,800,-90); |
yuki0701 | 13:f9867c3b97b0 | 517 | gogo_straight(1,0,-2500,6550,-1300,6550,max_speed,max_speed,5,0.1,10,0.1,800,-90); |
yuki0701 | 13:f9867c3b97b0 | 518 | gogo_straight(1,0,-1300,6550,-850,6550,max_speed,turn_speed,5,0.1,10,0.1,800,-90); |
yuki0701 | 13:f9867c3b97b0 | 519 | enc_correction(0,1); |
yuki0701 | 13:f9867c3b97b0 | 520 | set_cond(2,0,-19,0,6962); |
yuki0701 | 13:f9867c3b97b0 | 521 | purecurve(8,0,0,-850,6550,-620,6000,9,turn_speed,10,0.1,10,0.1,600,-180); //要修正 |
yuki0701 | 13:f9867c3b97b0 | 522 | enc_correction(1,0); |
yuki0701 | 13:f9867c3b97b0 | 523 | set_cond(0,0,-19,0,0); |
yuki0701 | 13:f9867c3b97b0 | 524 | gogo_straight(0,1,-620,6000,-620,5500,turn_speed,max_speed,5,0.1,10,0.1,600,-180); |
yuki0701 | 13:f9867c3b97b0 | 525 | gogo_straight(0,1,-620,5500,-620,5000,max_speed,max_speed,5,0.1,10,0.1,600,-180); |
yuki0701 | 13:f9867c3b97b0 | 526 | set_cond(2,1,-1000,1,4000); |
yuki0701 | 13:f9867c3b97b0 | 527 | gogo_straight(0,0,-620,5000,-620,4600,max_speed,end_speed,5,0.1,10,0.1,800,-180); |
yuki0701 | 13:f9867c3b97b0 | 528 | MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 529 | pos_correction(-620,4370,-180,0,0,30); |
yuki0701 | 13:f9867c3b97b0 | 530 | enc_correction(1,1); |
yuki0701 | 13:f9867c3b97b0 | 531 | MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 532 | |
yuki0701 | 13:f9867c3b97b0 | 533 | T1++; |
yuki0701 | 13:f9867c3b97b0 | 534 | } |
yuki0701 | 13:f9867c3b97b0 | 535 | if(T1 == 7) { |
yuki0701 | 13:f9867c3b97b0 | 536 | while(1) { |
yuki0701 | 13:f9867c3b97b0 | 537 | //printf("ashi finished\n\r"); |
yuki0701 | 13:f9867c3b97b0 | 538 | ManualOut(250,100,500,200); |
yuki0701 | 13:f9867c3b97b0 | 539 | //MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 540 | |
yuki0701 | 13:f9867c3b97b0 | 541 | if(id1_value[0] != 1)break; //これらは他のwhileにも入れる必要あり |
yuki0701 | 13:f9867c3b97b0 | 542 | if(id1_value[6] != flag)break; |
yuki0701 | 13:f9867c3b97b0 | 543 | } |
yuki0701 | 13:f9867c3b97b0 | 544 | } |
yuki0701 | 13:f9867c3b97b0 | 545 | |
yuki0701 | 13:f9867c3b97b0 | 546 | |
yuki0701 | 13:f9867c3b97b0 | 547 | |
yuki0701 | 13:f9867c3b97b0 | 548 | break; |
yuki0701 | 13:f9867c3b97b0 | 549 | } |
yuki0701 | 13:f9867c3b97b0 | 550 | // ManualOut(250,100,500,200); |
yuki0701 | 13:f9867c3b97b0 | 551 | //go_waitmode = 0; |
la00noix | 6:26724c287387 | 552 | |
la00noix | 6:26724c287387 | 553 | break; |
la00noix | 0:c61c6e4775ca | 554 | } |
la00noix | 0:c61c6e4775ca | 555 | //------------------------------------------------------------------------------------------------------------------------------------// |
la00noix | 0:c61c6e4775ca | 556 | } |
la00noix | 0:c61c6e4775ca | 557 | } |