3/20 13:39
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
- Child:
- 12:762061580168
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 | 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 | } |