3/20 13:39
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
main.cpp@14:6c1b36c55915, 2019-03-20 (annotated)
- Committer:
- yuki0701
- Date:
- Wed Mar 20 04:39:06 2019 +0000
- Revision:
- 14:6c1b36c55915
- Parent:
- 13:f9867c3b97b0
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-------------------------------------------------------------------------------------------------------------// |
la00noix | 9:7667dcfc7ce5 | 57 | flag = 0; |
yuki0701 | 10:c741191360de | 58 | UserLoopSetting_enc_right(); |
yuki0701 | 14:6c1b36c55915 | 59 | |
yuki0701 | 14:6c1b36c55915 | 60 | if(T1 == 0) { //スタート位置から少し移動 |
yuki0701 | 12:762061580168 | 61 | |
yuki0701 | 12:762061580168 | 62 | gogo_straight(1,1,3112,3500,2962,3100,st_speed,mid_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 14:6c1b36c55915 | 63 | T1++; |
yuki0701 | 14:6c1b36c55915 | 64 | } |
yuki0701 | 14:6c1b36c55915 | 65 | if(T1 == 1) { //みかんの木まで移動して掴む |
yuki0701 | 14:6c1b36c55915 | 66 | |
yuki0701 | 12:762061580168 | 67 | gogo_straight(1,1,2962,3100,2962,2900,mid_speed,turn_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 12:762061580168 | 68 | purecurve(6,1,1,2962,2900,2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 12:762061580168 | 69 | purecurve(5,1,1,2317,2500,1672,2000,9,turn_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 10:c741191360de | 70 | set_cond(2,1,1050,1,1076); |
yuki0701 | 12:762061580168 | 71 | gogo_straight(0,0,1672,2000,1672,1600,turn_speed,end_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 10:c741191360de | 72 | MaxonControl(0,0,0,0); |
yuki0701 | 11:9db93bce4eef | 73 | pos_correction(1672,1600,0,0,0,25); |
yuki0701 | 10:c741191360de | 74 | enc_correction(1,1); |
yuki0701 | 12:762061580168 | 75 | gogo_straight(0,0,1672,1600,1672,1375,fruit_speed,fruit_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 12:762061580168 | 76 | MaxonControl(0,0,0,0); |
yuki0701 | 10:c741191360de | 77 | T1++; |
yuki0701 | 10:c741191360de | 78 | } |
yuki0701 | 14:6c1b36c55915 | 79 | |
yuki0701 | 14:6c1b36c55915 | 80 | if(T1 == 2) { //みかんを持ち上げる |
yuki0701 | 10:c741191360de | 81 | while(1) { |
yuki0701 | 10:c741191360de | 82 | wait(0.1); |
yuki0701 | 14:6c1b36c55915 | 83 | if(T1 == 3) { |
yuki0701 | 10:c741191360de | 84 | break; |
yuki0701 | 10:c741191360de | 85 | } |
yuki0701 | 10:c741191360de | 86 | } |
yuki0701 | 10:c741191360de | 87 | } |
yuki0701 | 14:6c1b36c55915 | 88 | if(T1 == 3) { //みかんの木から三宝置き場まで移動 |
yuki0701 | 12:762061580168 | 89 | |
yuki0701 | 12:762061580168 | 90 | gogo_straight(1,1,1672,1375,1672,2000,st_speed,turn_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 12:762061580168 | 91 | purecurve(2,1,1,1672,2000,2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 12:762061580168 | 92 | purecurve(1,1,1,2317,2500,2962,3000,9,turn_speed,5,0.1,10,0.1,600,90); |
yuki0701 | 12:762061580168 | 93 | gogo_straight(1,1,2962,3000,2962,4000,turn_speed,max_speed,5,0.1,10,0.1,600,90); |
yuki0701 | 12:762061580168 | 94 | gogo_straight(1,1,2962,4000,2962,4810,max_speed,end_speed,5,0.1,10,0.1,600,90); |
yuki0701 | 10:c741191360de | 95 | MaxonControl(0,0,0,0); |
yuki0701 | 10:c741191360de | 96 | set_cond(2,1,2462,0,6000); |
yuki0701 | 12:762061580168 | 97 | pos_correction(2962,4837.5,90,0,0,10); |
yuki0701 | 12:762061580168 | 98 | gogo_straight(0,0,2962,4837.5,2782,4837.5,300,200,5,0.1,10,0.1,800,90); |
yuki0701 | 10:c741191360de | 99 | MaxonControl(0,0,0,0); |
yuki0701 | 10:c741191360de | 100 | enc_correction(1,1); |
yuki0701 | 12:762061580168 | 101 | |
yuki0701 | 14:6c1b36c55915 | 102 | |
yuki0701 | 10:c741191360de | 103 | T1++; |
yuki0701 | 10:c741191360de | 104 | } |
yuki0701 | 14:6c1b36c55915 | 105 | if(T1 == 4) { //三宝を掴み機体内に取り込み、みかんを三宝に乗せる |
yuki0701 | 10:c741191360de | 106 | while(1) { |
yuki0701 | 10:c741191360de | 107 | wait(0.1); |
yuki0701 | 14:6c1b36c55915 | 108 | if(T1 == 5) { |
yuki0701 | 10:c741191360de | 109 | break; |
yuki0701 | 10:c741191360de | 110 | } |
yuki0701 | 10:c741191360de | 111 | } |
yuki0701 | 10:c741191360de | 112 | } |
yuki0701 | 14:6c1b36c55915 | 113 | if(T1 == 5) { //三宝置き場からりんごの木まで移動 |
yuki0701 | 12:762061580168 | 114 | |
yuki0701 | 12:762061580168 | 115 | gogo_straight(1,1,2782,4837.5,2782,5150,st_speed,turn_speed,5,0.1,10,0.1,800,90); |
yuki0701 | 12:762061580168 | 116 | purecurve(3,1,1,2850,5150,2257,5500,9,turn_speed,5,0.1,10,0.1,800,90); |
yuki0701 | 12:762061580168 | 117 | purecurve(4,1,1,2257,5500,1700,6000,9,turn_speed,5,0.1,10,0.1,800,90); |
yuki0701 | 12:762061580168 | 118 | purecurve(2,1,1,1700,6000,2257,6550,9,turn_speed,5,0.1,10,0.1,800,90); |
yuki0701 | 12:762061580168 | 119 | gogo_straight(1,1,2257,6550,2500,6605,turn_speed,end_speed,5,0.1,10,0.1,800,90); |
yuki0701 | 10:c741191360de | 120 | MaxonControl(0,0,0,0); |
yuki0701 | 12:762061580168 | 121 | set_cond(1,0,0,1,6038); |
yuki0701 | 12:762061580168 | 122 | pos_correction(2500,6610,90,1,0,8); |
yuki0701 | 12:762061580168 | 123 | set_cond(2,0,3162,1,6038); |
yuki0701 | 12:762061580168 | 124 | gogo_straight(0,0,2500,6610,2700,6610,fruit_speed,fruit_speed,5,0.1,10,0.1,800,90); |
yuki0701 | 12:762061580168 | 125 | MaxonControl(0,0,0,0); |
yuki0701 | 10:c741191360de | 126 | enc_correction(1,1); |
yuki0701 | 12:762061580168 | 127 | |
yuki0701 | 10:c741191360de | 128 | T1++; |
yuki0701 | 10:c741191360de | 129 | } |
yuki0701 | 14:6c1b36c55915 | 130 | if(T1 == 6) { //りんごを持ち上げる |
yuki0701 | 10:c741191360de | 131 | while(1) { |
yuki0701 | 10:c741191360de | 132 | wait(0.1); |
yuki0701 | 14:6c1b36c55915 | 133 | if(T1 == 7) { |
yuki0701 | 10:c741191360de | 134 | break; |
yuki0701 | 10:c741191360de | 135 | } |
yuki0701 | 10:c741191360de | 136 | } |
yuki0701 | 10:c741191360de | 137 | } |
yuki0701 | 14:6c1b36c55915 | 138 | if(T1 == 7) { //りんごの木からお供え台まで移動 |
yuki0701 | 13:f9867c3b97b0 | 139 | |
yuki0701 | 12:762061580168 | 140 | set_cond(1,0,0,0,6962); |
yuki0701 | 12:762061580168 | 141 | gogo_straight(1,0,2700,6610,2500,6550,st_speed,max_speed,5,0.1,10,0.1,800,90); |
yuki0701 | 12:762061580168 | 142 | gogo_straight(1,0,2500,6550,1300,6550,max_speed,max_speed,5,0.1,10,0.1,800,90); |
yuki0701 | 12:762061580168 | 143 | gogo_straight(1,0,1300,6550,850,6550,max_speed,turn_speed,5,0.1,10,0.1,800,90); |
yuki0701 | 12:762061580168 | 144 | enc_correction(0,1); |
yuki0701 | 12:762061580168 | 145 | set_cond(2,1,19,0,6962); //要修正 |
yuki0701 | 12:762061580168 | 146 | purecurve(5,0,0,850,6550,620,6000,9,turn_speed,10,0.1,10,0.1,600,180); //要修正 |
yuki0701 | 12:762061580168 | 147 | enc_correction(1,0); |
yuki0701 | 12:762061580168 | 148 | set_cond(0,1,19,0,0); |
yuki0701 | 12:762061580168 | 149 | gogo_straight(0,1,620,6000,620,5500,turn_speed,max_speed,5,0.1,10,0.1,600,180); |
yuki0701 | 12:762061580168 | 150 | gogo_straight(0,1,620,5500,620,5000,max_speed,max_speed,5,0.1,10,0.1,600,180); |
yuki0701 | 12:762061580168 | 151 | set_cond(2,0,1000,1,4000); |
yuki0701 | 12:762061580168 | 152 | gogo_straight(0,0,620,5000,620,4600,max_speed,end_speed,5,0.1,10,0.1,800,180); |
yuki0701 | 10:c741191360de | 153 | MaxonControl(0,0,0,0); |
yuki0701 | 12:762061580168 | 154 | pos_correction(620,4370,180,0,0,30); |
yuki0701 | 10:c741191360de | 155 | enc_correction(1,1); |
yuki0701 | 10:c741191360de | 156 | MaxonControl(0,0,0,0); |
yuki0701 | 12:762061580168 | 157 | |
yuki0701 | 10:c741191360de | 158 | T1++; |
yuki0701 | 10:c741191360de | 159 | } |
yuki0701 | 14:6c1b36c55915 | 160 | if(T1 == 8 || T1 == 9) { //三宝をお供え台に置く |
yuki0701 | 10:c741191360de | 161 | while(1) { |
yuki0701 | 10:c741191360de | 162 | printf("ashi finished\n\r"); |
yuki0701 | 10:c741191360de | 163 | MaxonControl(0,0,0,0); |
yuki0701 | 10:c741191360de | 164 | if(id1_value[0] != 1)break; //これらは他のwhileにも入れる必要あり |
yuki0701 | 10:c741191360de | 165 | if(id1_value[6] != flag)break; |
yuki0701 | 10:c741191360de | 166 | } |
yuki0701 | 10:c741191360de | 167 | } |
yuki0701 | 10:c741191360de | 168 | break; |
yuki0701 | 10:c741191360de | 169 | case 1: |
yuki0701 | 10:c741191360de | 170 | //-----left mode--------------------------------------------------------------------------------------------------------------// |
yuki0701 | 10:c741191360de | 171 | flag = 1; |
yuki0701 | 10:c741191360de | 172 | UserLoopSetting_enc_left(); |
yuki0701 | 14:6c1b36c55915 | 173 | |
yuki0701 | 14:6c1b36c55915 | 174 | if(T1 == 0) { //スタート位置から少し移動 |
yuki0701 | 11:9db93bce4eef | 175 | |
yuki0701 | 11:9db93bce4eef | 176 | gogo_straight(1,1,-3112,3500,-2962,3100,st_speed,mid_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 14:6c1b36c55915 | 177 | |
yuki0701 | 14:6c1b36c55915 | 178 | T1++; |
yuki0701 | 14:6c1b36c55915 | 179 | } |
yuki0701 | 14:6c1b36c55915 | 180 | if(T1 == 1) { //みかんの木まで移動して掴む |
yuki0701 | 14:6c1b36c55915 | 181 | |
yuki0701 | 11:9db93bce4eef | 182 | gogo_straight(1,1,-2962,3100,-2962,2900,mid_speed,turn_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 11:9db93bce4eef | 183 | purecurve(7,1,1,-2962,2900,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 11:9db93bce4eef | 184 | purecurve(8,1,1,-2317,2500,-1672,2000,9,turn_speed,5,0.1,10,0.1,600,0); |
la00noix | 9:7667dcfc7ce5 | 185 | set_cond(2,0,-1050,1,1076); |
yuki0701 | 11:9db93bce4eef | 186 | gogo_straight(0,0,-1672,2000,-1672,1600,turn_speed,end_speed,5,0.1,10,0.1,600,0); |
la00noix | 9:7667dcfc7ce5 | 187 | MaxonControl(0,0,0,0); |
yuki0701 | 11:9db93bce4eef | 188 | pos_correction(-1672,1600,0,0,0,25); |
la00noix | 9:7667dcfc7ce5 | 189 | enc_correction(1,1); |
yuki0701 | 11:9db93bce4eef | 190 | gogo_straight(0,0,-1672,1600,-1672,1375,fruit_speed,fruit_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 11:9db93bce4eef | 191 | MaxonControl(0,0,0,0); |
yuki0701 | 11:9db93bce4eef | 192 | |
la00noix | 8:2ba338b4590e | 193 | T1++; |
la00noix | 8:2ba338b4590e | 194 | } |
yuki0701 | 14:6c1b36c55915 | 195 | |
yuki0701 | 14:6c1b36c55915 | 196 | if(T1 == 2) { //みかんを持ち上げる |
la00noix | 8:2ba338b4590e | 197 | while(1) { |
la00noix | 9:7667dcfc7ce5 | 198 | wait(0.1); |
yuki0701 | 14:6c1b36c55915 | 199 | if(T1 == 3) { |
la00noix | 8:2ba338b4590e | 200 | break; |
la00noix | 8:2ba338b4590e | 201 | } |
la00noix | 8:2ba338b4590e | 202 | } |
la00noix | 8:2ba338b4590e | 203 | } |
yuki0701 | 14:6c1b36c55915 | 204 | if(T1 == 3) { //みかんの木から三宝置き場まで移動 |
yuki0701 | 14:6c1b36c55915 | 205 | |
yuki0701 | 11:9db93bce4eef | 206 | gogo_straight(1,1,-1672,1375,-1672,2000,st_speed,turn_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 11:9db93bce4eef | 207 | purecurve(3,1,1,-1672,2000,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 11:9db93bce4eef | 208 | 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 | 209 | gogo_straight(1,1,-2962,3000,-2962,4000,turn_speed,max_speed,5,0.1,10,0.1,600,-90); |
yuki0701 | 12:762061580168 | 210 | gogo_straight(1,1,-2962,4000,-2962,4810,max_speed,end_speed,5,0.1,10,0.1,600,-90); |
la00noix | 9:7667dcfc7ce5 | 211 | MaxonControl(0,0,0,0); |
la00noix | 9:7667dcfc7ce5 | 212 | set_cond(2,0,-2462,0,6000); |
yuki0701 | 12:762061580168 | 213 | pos_correction(-2962,4837.5,-90,0,0,10); |
yuki0701 | 12:762061580168 | 214 | gogo_straight(0,0,-2962,4837.5,-2782,4837.5,300,200,5,0.1,10,0.1,800,-90); |
la00noix | 9:7667dcfc7ce5 | 215 | MaxonControl(0,0,0,0); |
la00noix | 9:7667dcfc7ce5 | 216 | enc_correction(1,1); |
yuki0701 | 11:9db93bce4eef | 217 | |
yuki0701 | 14:6c1b36c55915 | 218 | |
la00noix | 8:2ba338b4590e | 219 | T1++; |
la00noix | 8:2ba338b4590e | 220 | } |
yuki0701 | 14:6c1b36c55915 | 221 | if(T1 == 4) { //三宝を掴み機体内に取り込み、みかんを三宝に乗せる |
la00noix | 8:2ba338b4590e | 222 | while(1) { |
la00noix | 9:7667dcfc7ce5 | 223 | wait(0.1); |
yuki0701 | 14:6c1b36c55915 | 224 | if(T1 == 5) { |
la00noix | 8:2ba338b4590e | 225 | break; |
la00noix | 8:2ba338b4590e | 226 | } |
la00noix | 8:2ba338b4590e | 227 | } |
la00noix | 8:2ba338b4590e | 228 | } |
yuki0701 | 14:6c1b36c55915 | 229 | if(T1 == 5) { //三宝置き場からりんごの木まで移動 |
yuki0701 | 14:6c1b36c55915 | 230 | |
yuki0701 | 11:9db93bce4eef | 231 | gogo_straight(1,1,-2782,4837.5,-2782,5150,st_speed,turn_speed,5,0.1,10,0.1,800,-90); |
yuki0701 | 11:9db93bce4eef | 232 | purecurve(2,1,1,-2850,5150,-2257,5500,9,turn_speed,5,0.1,10,0.1,800,-90); |
yuki0701 | 11:9db93bce4eef | 233 | purecurve(1,1,1,-2257,5500,-1700,6000,9,turn_speed,5,0.1,10,0.1,800,-90); |
yuki0701 | 11:9db93bce4eef | 234 | purecurve(3,1,1,-1700,6000,-2257,6550,9,turn_speed,5,0.1,10,0.1,800,-90); |
yuki0701 | 12:762061580168 | 235 | gogo_straight(1,1,-2257,6550,-2500,6605,turn_speed,end_speed,5,0.1,10,0.1,800,-90); |
la00noix | 9:7667dcfc7ce5 | 236 | MaxonControl(0,0,0,0); |
yuki0701 | 12:762061580168 | 237 | set_cond(1,0,0,1,6038); |
yuki0701 | 12:762061580168 | 238 | pos_correction(-2500,6610,-90,1,0,8); |
yuki0701 | 11:9db93bce4eef | 239 | set_cond(2,1,-3162,1,6038); |
yuki0701 | 12:762061580168 | 240 | gogo_straight(0,0,-2500,6610,-2700,6610,fruit_speed,fruit_speed,5,0.1,10,0.1,800,-90); |
yuki0701 | 11:9db93bce4eef | 241 | MaxonControl(0,0,0,0); |
la00noix | 9:7667dcfc7ce5 | 242 | enc_correction(1,1); |
yuki0701 | 11:9db93bce4eef | 243 | |
la00noix | 8:2ba338b4590e | 244 | T1++; |
la00noix | 8:2ba338b4590e | 245 | } |
yuki0701 | 14:6c1b36c55915 | 246 | if(T1 == 6) { //りんごを持ち上げる |
la00noix | 8:2ba338b4590e | 247 | while(1) { |
la00noix | 9:7667dcfc7ce5 | 248 | wait(0.1); |
yuki0701 | 14:6c1b36c55915 | 249 | if(T1 == 7) { |
la00noix | 8:2ba338b4590e | 250 | break; |
la00noix | 8:2ba338b4590e | 251 | } |
la00noix | 8:2ba338b4590e | 252 | } |
la00noix | 8:2ba338b4590e | 253 | } |
yuki0701 | 14:6c1b36c55915 | 254 | if(T1 == 7) { //りんごの木からお供え台まで移動 |
yuki0701 | 14:6c1b36c55915 | 255 | |
yuki0701 | 11:9db93bce4eef | 256 | set_cond(1,0,0,0,6962); |
yuki0701 | 12:762061580168 | 257 | gogo_straight(1,0,-2700,6610,-2500,6550,st_speed,max_speed,5,0.1,10,0.1,800,-90); |
yuki0701 | 11:9db93bce4eef | 258 | gogo_straight(1,0,-2500,6550,-1300,6550,max_speed,max_speed,5,0.1,10,0.1,800,-90); |
yuki0701 | 12:762061580168 | 259 | |
yuki0701 | 11:9db93bce4eef | 260 | gogo_straight(1,0,-1300,6550,-850,6550,max_speed,turn_speed,5,0.1,10,0.1,800,-90); |
yuki0701 | 11:9db93bce4eef | 261 | enc_correction(0,1); |
yuki0701 | 11:9db93bce4eef | 262 | set_cond(2,0,-19,0,6962); |
yuki0701 | 12:762061580168 | 263 | purecurve(8,0,0,-850,6550,-620,6000,9,turn_speed,10,0.1,10,0.1,600,-180); //要修正 |
yuki0701 | 11:9db93bce4eef | 264 | enc_correction(1,0); |
yuki0701 | 11:9db93bce4eef | 265 | set_cond(0,0,-19,0,0); |
yuki0701 | 12:762061580168 | 266 | gogo_straight(0,1,-620,6000,-620,5500,turn_speed,max_speed,5,0.1,10,0.1,600,-180); |
yuki0701 | 12:762061580168 | 267 | gogo_straight(0,1,-620,5500,-620,5000,max_speed,max_speed,5,0.1,10,0.1,600,-180); |
la00noix | 9:7667dcfc7ce5 | 268 | set_cond(2,1,-1000,1,4000); |
yuki0701 | 12:762061580168 | 269 | gogo_straight(0,0,-620,5000,-620,4600,max_speed,end_speed,5,0.1,10,0.1,800,-180); |
la00noix | 9:7667dcfc7ce5 | 270 | MaxonControl(0,0,0,0); |
yuki0701 | 12:762061580168 | 271 | pos_correction(-620,4370,-180,0,0,30); |
la00noix | 9:7667dcfc7ce5 | 272 | enc_correction(1,1); |
la00noix | 9:7667dcfc7ce5 | 273 | MaxonControl(0,0,0,0); |
yuki0701 | 11:9db93bce4eef | 274 | |
la00noix | 8:2ba338b4590e | 275 | T1++; |
la00noix | 8:2ba338b4590e | 276 | } |
yuki0701 | 14:6c1b36c55915 | 277 | if(T1 == 8 || T1 == 9) { //三宝をお供え台に置く |
la00noix | 9:7667dcfc7ce5 | 278 | while(1) { |
la00noix | 9:7667dcfc7ce5 | 279 | printf("ashi finished\n\r"); |
la00noix | 9:7667dcfc7ce5 | 280 | MaxonControl(0,0,0,0); |
la00noix | 9:7667dcfc7ce5 | 281 | if(id1_value[0] != 1)break; //これらは他のwhileにも入れる必要あり |
la00noix | 9:7667dcfc7ce5 | 282 | if(id1_value[6] != flag)break; |
la00noix | 9:7667dcfc7ce5 | 283 | } |
la00noix | 9:7667dcfc7ce5 | 284 | } |
la00noix | 6:26724c287387 | 285 | break; |
la00noix | 6:26724c287387 | 286 | } |
la00noix | 6:26724c287387 | 287 | //-----wait mode----------------------------------------------------------------------------------------------------------------------// |
la00noix | 6:26724c287387 | 288 | case 0: |
la00noix | 6:26724c287387 | 289 | |
yuki0701 | 11:9db93bce4eef | 290 | // printf("wait\n\r"); |
la00noix | 6:26724c287387 | 291 | calc_xy(0,1,1); |
la00noix | 6:26724c287387 | 292 | ashi_led(); |
la00noix | 6:26724c287387 | 293 | MaxonControl(0,0,0,0); |
la00noix | 7:44ce34007499 | 294 | go_waitmode = 0; |
la00noix | 6:26724c287387 | 295 | |
la00noix | 6:26724c287387 | 296 | break; |
la00noix | 6:26724c287387 | 297 | //-----manual mode--------------------------------------------------------------------------------------------------------------------// |
la00noix | 6:26724c287387 | 298 | case 2: |
yuki0701 | 13:f9867c3b97b0 | 299 | switch(id1_value[6]) { |
yuki0701 | 13:f9867c3b97b0 | 300 | case 0: |
yuki0701 | 13:f9867c3b97b0 | 301 | //-----right mode-------------------------------------------------------------------------------------------------------------// |
yuki0701 | 13:f9867c3b97b0 | 302 | //未修正 |
la00noix | 6:26724c287387 | 303 | |
yuki0701 | 13:f9867c3b97b0 | 304 | flag = 0; |
yuki0701 | 13:f9867c3b97b0 | 305 | UserLoopSetting_enc_right(); |
yuki0701 | 13:f9867c3b97b0 | 306 | if(T1 == 0) { //スタート位置からみかんの木まで移動 |
yuki0701 | 13:f9867c3b97b0 | 307 | |
yuki0701 | 13:f9867c3b97b0 | 308 | gogo_straight(1,1,3112,3500,2962,3100,st_speed,mid_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 13:f9867c3b97b0 | 309 | gogo_straight(1,1,2962,3100,2962,2900,mid_speed,turn_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 13:f9867c3b97b0 | 310 | purecurve(6,1,1,2962,2900,2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 13:f9867c3b97b0 | 311 | purecurve(5,1,1,2317,2500,1672,2000,9,turn_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 13:f9867c3b97b0 | 312 | set_cond(2,1,1050,1,1076); |
yuki0701 | 13:f9867c3b97b0 | 313 | gogo_straight(0,0,1672,2000,1672,1600,turn_speed,end_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 13:f9867c3b97b0 | 314 | MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 315 | pos_correction(1672,1600,0,0,0,25); |
yuki0701 | 13:f9867c3b97b0 | 316 | enc_correction(1,1); |
yuki0701 | 13:f9867c3b97b0 | 317 | gogo_straight(0,0,1672,1600,1672,1375,fruit_speed,fruit_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 13:f9867c3b97b0 | 318 | MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 319 | |
yuki0701 | 13:f9867c3b97b0 | 320 | //printf("t1 = 0\n\r"); |
yuki0701 | 13:f9867c3b97b0 | 321 | //wait(1); |
yuki0701 | 13:f9867c3b97b0 | 322 | T1++; |
yuki0701 | 13:f9867c3b97b0 | 323 | } |
yuki0701 | 13:f9867c3b97b0 | 324 | if(T1 == 1) { |
yuki0701 | 13:f9867c3b97b0 | 325 | while(1) { |
yuki0701 | 13:f9867c3b97b0 | 326 | ManualOut(250,100,500,200); |
yuki0701 | 13:f9867c3b97b0 | 327 | //calc_xy(0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 328 | if(T1 == 2) { |
yuki0701 | 13:f9867c3b97b0 | 329 | // enc_correction(1,1); |
yuki0701 | 13:f9867c3b97b0 | 330 | break; |
yuki0701 | 13:f9867c3b97b0 | 331 | } |
yuki0701 | 13:f9867c3b97b0 | 332 | } |
yuki0701 | 13:f9867c3b97b0 | 333 | } |
yuki0701 | 13:f9867c3b97b0 | 334 | if(T1 == 2) { //みかんの木から三宝置き場まで移動 |
yuki0701 | 13:f9867c3b97b0 | 335 | |
yuki0701 | 13:f9867c3b97b0 | 336 | gogo_straight(1,1,1672,1375,1672,2000,st_speed,turn_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 13:f9867c3b97b0 | 337 | purecurve(2,1,1,1672,2000,2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 13:f9867c3b97b0 | 338 | purecurve(1,1,1,2317,2500,2962,3000,9,turn_speed,5,0.1,10,0.1,600,90); |
yuki0701 | 13:f9867c3b97b0 | 339 | gogo_straight(1,1,2962,3000,2962,4000,turn_speed,max_speed,5,0.1,10,0.1,600,90); |
yuki0701 | 13:f9867c3b97b0 | 340 | gogo_straight(1,1,2962,4000,2962,4810,max_speed,end_speed,5,0.1,10,0.1,600,90); |
yuki0701 | 13:f9867c3b97b0 | 341 | MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 342 | set_cond(2,1,2462,0,6000); |
yuki0701 | 13:f9867c3b97b0 | 343 | pos_correction(2962,4837.5,90,0,0,10); |
yuki0701 | 13:f9867c3b97b0 | 344 | gogo_straight(0,0,2962,4837.5,2782,4837.5,300,200,5,0.1,10,0.1,800,90); |
yuki0701 | 13:f9867c3b97b0 | 345 | MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 346 | enc_correction(1,1); |
yuki0701 | 13:f9867c3b97b0 | 347 | |
yuki0701 | 13:f9867c3b97b0 | 348 | //printf("t1 = 2\n\r"); |
yuki0701 | 13:f9867c3b97b0 | 349 | //wait(1); |
yuki0701 | 13:f9867c3b97b0 | 350 | T1++; |
yuki0701 | 13:f9867c3b97b0 | 351 | } |
yuki0701 | 13:f9867c3b97b0 | 352 | if(T1 == 3) { |
yuki0701 | 13:f9867c3b97b0 | 353 | while(1) { |
yuki0701 | 13:f9867c3b97b0 | 354 | ManualOut(250,100,500,200); |
yuki0701 | 13:f9867c3b97b0 | 355 | //calc_xy(90,0,0); |
yuki0701 | 13:f9867c3b97b0 | 356 | if(T1 == 4) { |
yuki0701 | 13:f9867c3b97b0 | 357 | // enc_correction(1,1); |
yuki0701 | 13:f9867c3b97b0 | 358 | break; |
yuki0701 | 13:f9867c3b97b0 | 359 | } |
yuki0701 | 13:f9867c3b97b0 | 360 | } |
yuki0701 | 13:f9867c3b97b0 | 361 | } |
yuki0701 | 13:f9867c3b97b0 | 362 | if(T1 == 4) { //三宝置き場からりんごの木まで移動 |
yuki0701 | 13:f9867c3b97b0 | 363 | |
yuki0701 | 13:f9867c3b97b0 | 364 | gogo_straight(1,1,2782,4837.5,2782,5150,st_speed,turn_speed,5,0.1,10,0.1,800,90); |
yuki0701 | 13:f9867c3b97b0 | 365 | purecurve(3,1,1,2850,5150,2257,5500,9,turn_speed,5,0.1,10,0.1,800,90); |
yuki0701 | 13:f9867c3b97b0 | 366 | purecurve(4,1,1,2257,5500,1700,6000,9,turn_speed,5,0.1,10,0.1,800,90); |
yuki0701 | 13:f9867c3b97b0 | 367 | purecurve(2,1,1,1700,6000,2257,6550,9,turn_speed,5,0.1,10,0.1,800,90); |
yuki0701 | 13:f9867c3b97b0 | 368 | gogo_straight(1,1,2257,6550,2500,6605,turn_speed,end_speed,5,0.1,10,0.1,800,90); |
yuki0701 | 13:f9867c3b97b0 | 369 | MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 370 | set_cond(1,0,0,1,6038); |
yuki0701 | 13:f9867c3b97b0 | 371 | pos_correction(2500,6610,90,1,0,8); |
yuki0701 | 13:f9867c3b97b0 | 372 | set_cond(2,0,3162,1,6038); |
yuki0701 | 13:f9867c3b97b0 | 373 | gogo_straight(0,0,2500,6610,2700,6610,fruit_speed,fruit_speed,5,0.1,10,0.1,800,90); |
yuki0701 | 13:f9867c3b97b0 | 374 | MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 375 | enc_correction(1,1); |
yuki0701 | 13:f9867c3b97b0 | 376 | |
yuki0701 | 13:f9867c3b97b0 | 377 | // printf("t1 = 4\n\r"); |
yuki0701 | 13:f9867c3b97b0 | 378 | // wait(1); |
yuki0701 | 13:f9867c3b97b0 | 379 | T1++; |
yuki0701 | 13:f9867c3b97b0 | 380 | } |
yuki0701 | 13:f9867c3b97b0 | 381 | if(T1 == 5) { |
yuki0701 | 13:f9867c3b97b0 | 382 | while(1) { |
yuki0701 | 13:f9867c3b97b0 | 383 | ManualOut(250,100,500,200); |
yuki0701 | 13:f9867c3b97b0 | 384 | //calc_xy(90,0,0); |
yuki0701 | 13:f9867c3b97b0 | 385 | if(T1 == 6) { |
yuki0701 | 13:f9867c3b97b0 | 386 | // enc_correction(1,1); |
yuki0701 | 13:f9867c3b97b0 | 387 | break; |
yuki0701 | 13:f9867c3b97b0 | 388 | } |
yuki0701 | 13:f9867c3b97b0 | 389 | } |
yuki0701 | 13:f9867c3b97b0 | 390 | } |
yuki0701 | 13:f9867c3b97b0 | 391 | if(T1 == 6) { //りんごの木からお供え台まで移動 |
yuki0701 | 13:f9867c3b97b0 | 392 | |
yuki0701 | 13:f9867c3b97b0 | 393 | set_cond(1,0,0,0,6962); |
yuki0701 | 13:f9867c3b97b0 | 394 | gogo_straight(1,0,2700,6610,2500,6550,st_speed,max_speed,5,0.1,10,0.1,800,90); |
yuki0701 | 13:f9867c3b97b0 | 395 | gogo_straight(1,0,2500,6550,1300,6550,max_speed,max_speed,5,0.1,10,0.1,800,90); |
yuki0701 | 13:f9867c3b97b0 | 396 | gogo_straight(1,0,1300,6550,850,6550,max_speed,turn_speed,5,0.1,10,0.1,800,90); |
yuki0701 | 13:f9867c3b97b0 | 397 | enc_correction(0,1); |
yuki0701 | 13:f9867c3b97b0 | 398 | set_cond(2,1,19,0,6962); //要修正 |
yuki0701 | 13:f9867c3b97b0 | 399 | purecurve(5,0,0,850,6550,620,6000,9,turn_speed,10,0.1,10,0.1,600,180); //要修正 |
yuki0701 | 13:f9867c3b97b0 | 400 | enc_correction(1,0); |
yuki0701 | 13:f9867c3b97b0 | 401 | set_cond(0,1,19,0,0); |
yuki0701 | 13:f9867c3b97b0 | 402 | gogo_straight(0,1,620,6000,620,5500,turn_speed,max_speed,5,0.1,10,0.1,600,180); |
yuki0701 | 13:f9867c3b97b0 | 403 | gogo_straight(0,1,620,5500,620,5000,max_speed,max_speed,5,0.1,10,0.1,600,180); |
yuki0701 | 13:f9867c3b97b0 | 404 | set_cond(2,0,1000,1,4000); |
yuki0701 | 13:f9867c3b97b0 | 405 | gogo_straight(0,0,620,5000,620,4600,max_speed,end_speed,5,0.1,10,0.1,800,180); |
yuki0701 | 13:f9867c3b97b0 | 406 | MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 407 | pos_correction(620,4370,180,0,0,30); |
yuki0701 | 13:f9867c3b97b0 | 408 | enc_correction(1,1); |
yuki0701 | 13:f9867c3b97b0 | 409 | MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 410 | |
yuki0701 | 13:f9867c3b97b0 | 411 | // printf("t1 = 6\n\r"); |
yuki0701 | 13:f9867c3b97b0 | 412 | // wait(1); |
yuki0701 | 13:f9867c3b97b0 | 413 | T1++; |
yuki0701 | 13:f9867c3b97b0 | 414 | } |
yuki0701 | 13:f9867c3b97b0 | 415 | if(T1 == 7) { |
yuki0701 | 13:f9867c3b97b0 | 416 | while(1) { |
yuki0701 | 13:f9867c3b97b0 | 417 | ManualOut(250,100,500,200); |
yuki0701 | 13:f9867c3b97b0 | 418 | //printf("ashi finished\n\r"); |
yuki0701 | 13:f9867c3b97b0 | 419 | //MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 420 | if(id1_value[0] != 1)break; //これらは他のwhileにも入れる必要あり |
yuki0701 | 13:f9867c3b97b0 | 421 | if(id1_value[6] != flag)break; |
yuki0701 | 13:f9867c3b97b0 | 422 | } |
yuki0701 | 13:f9867c3b97b0 | 423 | } |
yuki0701 | 13:f9867c3b97b0 | 424 | break; |
yuki0701 | 13:f9867c3b97b0 | 425 | |
yuki0701 | 13:f9867c3b97b0 | 426 | case 1: |
yuki0701 | 13:f9867c3b97b0 | 427 | //-----left mode--------------------------------------------------------------------------------------------------------------// |
yuki0701 | 13:f9867c3b97b0 | 428 | flag = 1; |
yuki0701 | 13:f9867c3b97b0 | 429 | printf("start\n\r"); |
yuki0701 | 13:f9867c3b97b0 | 430 | UserLoopSetting_enc_left(); |
yuki0701 | 13:f9867c3b97b0 | 431 | if(T1 == 0) { //スタート位置からみかんの木まで移動 |
yuki0701 | 13:f9867c3b97b0 | 432 | |
yuki0701 | 13:f9867c3b97b0 | 433 | gogo_straight(1,1,-3112,3500,-2962,3100,st_speed,mid_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 13:f9867c3b97b0 | 434 | gogo_straight(1,1,-2962,3100,-2962,2900,mid_speed,turn_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 13:f9867c3b97b0 | 435 | purecurve(7,1,1,-2962,2900,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 13:f9867c3b97b0 | 436 | purecurve(8,1,1,-2317,2500,-1672,2000,9,turn_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 13:f9867c3b97b0 | 437 | set_cond(2,0,-1050,1,1076); |
yuki0701 | 13:f9867c3b97b0 | 438 | gogo_straight(0,0,-1672,2000,-1672,1600,turn_speed,end_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 13:f9867c3b97b0 | 439 | MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 440 | pos_correction(-1672,1600,0,0,0,25); |
yuki0701 | 13:f9867c3b97b0 | 441 | enc_correction(1,1); |
yuki0701 | 13:f9867c3b97b0 | 442 | gogo_straight(0,0,-1672,1600,-1672,1375,fruit_speed,fruit_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 13:f9867c3b97b0 | 443 | MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 444 | |
yuki0701 | 13:f9867c3b97b0 | 445 | T1++; |
yuki0701 | 13:f9867c3b97b0 | 446 | } |
yuki0701 | 14:6c1b36c55915 | 447 | |
yuki0701 | 13:f9867c3b97b0 | 448 | if(T1 == 1) { |
yuki0701 | 13:f9867c3b97b0 | 449 | while(1) { |
yuki0701 | 13:f9867c3b97b0 | 450 | ManualOut(250,100,500,200); |
yuki0701 | 13:f9867c3b97b0 | 451 | if(T1 == 2) { |
yuki0701 | 13:f9867c3b97b0 | 452 | break; |
yuki0701 | 13:f9867c3b97b0 | 453 | } |
yuki0701 | 13:f9867c3b97b0 | 454 | } |
yuki0701 | 13:f9867c3b97b0 | 455 | } |
yuki0701 | 13:f9867c3b97b0 | 456 | if(T1 == 2) { //みかんの木から三宝置き場まで移動 |
yuki0701 | 13:f9867c3b97b0 | 457 | gogo_straight(1,1,-1672,1375,-1672,2000,st_speed,turn_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 13:f9867c3b97b0 | 458 | purecurve(3,1,1,-1672,2000,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); |
yuki0701 | 13:f9867c3b97b0 | 459 | 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 | 460 | gogo_straight(1,1,-2962,3000,-2962,4000,turn_speed,max_speed,5,0.1,10,0.1,600,-90); |
yuki0701 | 13:f9867c3b97b0 | 461 | gogo_straight(1,1,-2962,4000,-2962,4810,max_speed,end_speed,5,0.1,10,0.1,600,-90); |
yuki0701 | 13:f9867c3b97b0 | 462 | MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 463 | set_cond(2,0,-2462,0,6000); |
yuki0701 | 13:f9867c3b97b0 | 464 | pos_correction(-2962,4837.5,-90,0,0,10); |
yuki0701 | 13:f9867c3b97b0 | 465 | gogo_straight(0,0,-2962,4837.5,-2782,4837.5,300,200,5,0.1,10,0.1,800,-90); |
yuki0701 | 13:f9867c3b97b0 | 466 | MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 467 | enc_correction(1,1); |
yuki0701 | 13:f9867c3b97b0 | 468 | |
yuki0701 | 13:f9867c3b97b0 | 469 | T1++; |
yuki0701 | 13:f9867c3b97b0 | 470 | } |
yuki0701 | 13:f9867c3b97b0 | 471 | if(T1 == 3) { |
yuki0701 | 13:f9867c3b97b0 | 472 | while(1) { |
yuki0701 | 13:f9867c3b97b0 | 473 | ManualOut(250,100,500,200); |
yuki0701 | 13:f9867c3b97b0 | 474 | if(T1 == 4) { |
yuki0701 | 13:f9867c3b97b0 | 475 | break; |
yuki0701 | 13:f9867c3b97b0 | 476 | } |
yuki0701 | 13:f9867c3b97b0 | 477 | } |
yuki0701 | 13:f9867c3b97b0 | 478 | } |
yuki0701 | 13:f9867c3b97b0 | 479 | if(T1 == 4) { //三宝置き場からりんごの木まで移動 |
yuki0701 | 13:f9867c3b97b0 | 480 | gogo_straight(1,1,-2782,4837.5,-2782,5150,st_speed,turn_speed,5,0.1,10,0.1,800,-90); |
yuki0701 | 13:f9867c3b97b0 | 481 | purecurve(2,1,1,-2850,5150,-2257,5500,9,turn_speed,5,0.1,10,0.1,800,-90); |
yuki0701 | 13:f9867c3b97b0 | 482 | purecurve(1,1,1,-2257,5500,-1700,6000,9,turn_speed,5,0.1,10,0.1,800,-90); |
yuki0701 | 13:f9867c3b97b0 | 483 | purecurve(3,1,1,-1700,6000,-2257,6550,9,turn_speed,5,0.1,10,0.1,800,-90); |
yuki0701 | 13:f9867c3b97b0 | 484 | gogo_straight(1,1,-2257,6550,-2500,6605,turn_speed,end_speed,5,0.1,10,0.1,800,-90); |
yuki0701 | 13:f9867c3b97b0 | 485 | MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 486 | set_cond(1,0,0,1,6038); |
yuki0701 | 13:f9867c3b97b0 | 487 | pos_correction(-2500,6610,-90,1,0,8); |
yuki0701 | 13:f9867c3b97b0 | 488 | set_cond(2,1,-3162,1,6038); |
yuki0701 | 13:f9867c3b97b0 | 489 | gogo_straight(0,0,-2500,6610,-2700,6610,fruit_speed,fruit_speed,5,0.1,10,0.1,800,-90); |
yuki0701 | 13:f9867c3b97b0 | 490 | MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 491 | enc_correction(1,1); |
yuki0701 | 13:f9867c3b97b0 | 492 | |
yuki0701 | 13:f9867c3b97b0 | 493 | T1++; |
yuki0701 | 13:f9867c3b97b0 | 494 | } |
yuki0701 | 13:f9867c3b97b0 | 495 | if(T1 == 5) { |
yuki0701 | 13:f9867c3b97b0 | 496 | while(1) { |
yuki0701 | 13:f9867c3b97b0 | 497 | ManualOut(250,100,500,200); |
yuki0701 | 13:f9867c3b97b0 | 498 | if(T1 == 6) { |
yuki0701 | 13:f9867c3b97b0 | 499 | break; |
yuki0701 | 13:f9867c3b97b0 | 500 | } |
yuki0701 | 13:f9867c3b97b0 | 501 | } |
yuki0701 | 13:f9867c3b97b0 | 502 | } |
yuki0701 | 13:f9867c3b97b0 | 503 | if(T1 == 6) { //りんごの木からお供え台まで移動 |
yuki0701 | 13:f9867c3b97b0 | 504 | set_cond(1,0,0,0,6962); |
yuki0701 | 13:f9867c3b97b0 | 505 | gogo_straight(1,0,-2700,6610,-2500,6550,st_speed,max_speed,5,0.1,10,0.1,800,-90); |
yuki0701 | 13:f9867c3b97b0 | 506 | gogo_straight(1,0,-2500,6550,-1300,6550,max_speed,max_speed,5,0.1,10,0.1,800,-90); |
yuki0701 | 13:f9867c3b97b0 | 507 | gogo_straight(1,0,-1300,6550,-850,6550,max_speed,turn_speed,5,0.1,10,0.1,800,-90); |
yuki0701 | 13:f9867c3b97b0 | 508 | enc_correction(0,1); |
yuki0701 | 13:f9867c3b97b0 | 509 | set_cond(2,0,-19,0,6962); |
yuki0701 | 13:f9867c3b97b0 | 510 | purecurve(8,0,0,-850,6550,-620,6000,9,turn_speed,10,0.1,10,0.1,600,-180); //要修正 |
yuki0701 | 13:f9867c3b97b0 | 511 | enc_correction(1,0); |
yuki0701 | 13:f9867c3b97b0 | 512 | set_cond(0,0,-19,0,0); |
yuki0701 | 13:f9867c3b97b0 | 513 | gogo_straight(0,1,-620,6000,-620,5500,turn_speed,max_speed,5,0.1,10,0.1,600,-180); |
yuki0701 | 13:f9867c3b97b0 | 514 | gogo_straight(0,1,-620,5500,-620,5000,max_speed,max_speed,5,0.1,10,0.1,600,-180); |
yuki0701 | 13:f9867c3b97b0 | 515 | set_cond(2,1,-1000,1,4000); |
yuki0701 | 13:f9867c3b97b0 | 516 | gogo_straight(0,0,-620,5000,-620,4600,max_speed,end_speed,5,0.1,10,0.1,800,-180); |
yuki0701 | 13:f9867c3b97b0 | 517 | MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 518 | pos_correction(-620,4370,-180,0,0,30); |
yuki0701 | 13:f9867c3b97b0 | 519 | enc_correction(1,1); |
yuki0701 | 13:f9867c3b97b0 | 520 | MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 521 | |
yuki0701 | 13:f9867c3b97b0 | 522 | T1++; |
yuki0701 | 13:f9867c3b97b0 | 523 | } |
yuki0701 | 13:f9867c3b97b0 | 524 | if(T1 == 7) { |
yuki0701 | 13:f9867c3b97b0 | 525 | while(1) { |
yuki0701 | 13:f9867c3b97b0 | 526 | //printf("ashi finished\n\r"); |
yuki0701 | 13:f9867c3b97b0 | 527 | ManualOut(250,100,500,200); |
yuki0701 | 13:f9867c3b97b0 | 528 | //MaxonControl(0,0,0,0); |
yuki0701 | 13:f9867c3b97b0 | 529 | |
yuki0701 | 13:f9867c3b97b0 | 530 | if(id1_value[0] != 1)break; //これらは他のwhileにも入れる必要あり |
yuki0701 | 13:f9867c3b97b0 | 531 | if(id1_value[6] != flag)break; |
yuki0701 | 13:f9867c3b97b0 | 532 | } |
yuki0701 | 13:f9867c3b97b0 | 533 | } |
yuki0701 | 13:f9867c3b97b0 | 534 | |
yuki0701 | 13:f9867c3b97b0 | 535 | |
yuki0701 | 13:f9867c3b97b0 | 536 | |
yuki0701 | 13:f9867c3b97b0 | 537 | break; |
yuki0701 | 13:f9867c3b97b0 | 538 | } |
yuki0701 | 14:6c1b36c55915 | 539 | // ManualOut(250,100,500,200); |
yuki0701 | 13:f9867c3b97b0 | 540 | //go_waitmode = 0; |
la00noix | 6:26724c287387 | 541 | |
la00noix | 6:26724c287387 | 542 | break; |
la00noix | 0:c61c6e4775ca | 543 | } |
la00noix | 0:c61c6e4775ca | 544 | //------------------------------------------------------------------------------------------------------------------------------------// |
la00noix | 0:c61c6e4775ca | 545 | } |
la00noix | 0:c61c6e4775ca | 546 | } |