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

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?

UserRevisionLine numberNew contents of line
la00noix 0:c61c6e4775ca 1 #include "EC.h"
la00noix 0:c61c6e4775ca 2 #include "R1370P.h"
la00noix 0:c61c6e4775ca 3 #include "move4wheel.h"
la00noix 0:c61c6e4775ca 4 #include "mbed.h"
la00noix 0:c61c6e4775ca 5 #include "math.h"
la00noix 0:c61c6e4775ca 6 #include "PathFollowing.h"
la00noix 0:c61c6e4775ca 7 #include "movement.h"
la00noix 0:c61c6e4775ca 8 #include "maxonsetting.h"
la00noix 0:c61c6e4775ca 9 #include "manual.h"
la00noix 0:c61c6e4775ca 10 #include "can.h"
la00noix 0:c61c6e4775ca 11
la00noix 0:c61c6e4775ca 12 #define PI 3.141592
la00noix 0:c61c6e4775ca 13
la00noix 0:c61c6e4775ca 14 //#define PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示する際に定義
la00noix 0:c61c6e4775ca 15
la00noix 0:c61c6e4775ca 16 int go_waitmode = 0;
la00noix 0:c61c6e4775ca 17
yuki0701 11:9db93bce4eef 18 /*------------運動時の各速度の設定----------*/
yuki0701 11:9db93bce4eef 19 double st_speed = 500; // 移動開始時の速度
yuki0701 11:9db93bce4eef 20 double max_speed = 2500; // 直線移動時の最大速度
yuki0701 11:9db93bce4eef 21 double mid_speed = 2000; // 直線移動時の中間速度
yuki0701 11:9db93bce4eef 22 double turn_speed = 1000; // 旋回時の速度
yuki0701 11:9db93bce4eef 23 double end_speed = 200; // 停止直前の速度
yuki0701 11:9db93bce4eef 24 double fruit_speed = 200; // 果物を取る時の速度
yuki0701 11:9db93bce4eef 25 /*--------------------------------------*/
yuki0701 11:9db93bce4eef 26
la00noix 0:c61c6e4775ca 27 //-----mbed led------------------//点灯条件-----------------------//参照場所------------------------------//
la00noix 9:7667dcfc7ce5 28 //DigitalOut cansend_led(LED1); //cansend -> on //can.cpp
la00noix 9:7667dcfc7ce5 29 //DigitalOut canread_led(LED2); //canread -> on //can.cpp
la00noix 0:c61c6e4775ca 30 //DigitalOut debug_led(LED3); //maxon debug programme -> on //maxonsetting.cpp
la00noix 0:c61c6e4775ca 31
la00noix 0:c61c6e4775ca 32 //////////////////////////////////////////////////////////////以下main文/////////////////////////////////////////////////////////////////
la00noix 0:c61c6e4775ca 33
la00noix 6:26724c287387 34 int main()
la00noix 6:26724c287387 35 {
la00noix 0:c61c6e4775ca 36 UserLoopSetting_maxon();
la00noix 0:c61c6e4775ca 37 UserLoopSetting_sensor();
la00noix 0:c61c6e4775ca 38 UserLoopSetting_can();
la00noix 6:26724c287387 39
la00noix 0:c61c6e4775ca 40 #ifdef PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示
la00noix 0:c61c6e4775ca 41
la00noix 6:26724c287387 42 printf("ソースファイル名 : %s¥n", __FILE__);
la00noix 6:26724c287387 43 printf("作成日付 : %s¥n", __DATE__);
la00noix 6:26724c287387 44 printf("作成時刻 : %s¥n", __TIME__);
la00noix 6:26724c287387 45
la00noix 0:c61c6e4775ca 46 #endif
la00noix 0:c61c6e4775ca 47
la00noix 6:26724c287387 48 while(1) {
yuki0701 11:9db93bce4eef 49 //id1_value[0] = 1;
la00noix 6:26724c287387 50 switch(id1_value[0]) {
la00noix 0:c61c6e4775ca 51 //-----auto mode----------------------------------------------------------------------------------------------------------------------//
la00noix 6:26724c287387 52 case 1:
yuki0701 11:9db93bce4eef 53 //id1_value[6] = 0;
la00noix 6:26724c287387 54 switch(id1_value[6]) {
la00noix 6:26724c287387 55 case 0:
la00noix 6:26724c287387 56 //-----right mode-------------------------------------------------------------------------------------------------------------//
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 }