春ロボ1班(元F3RC4班+)
3/19
Dependencies: mbed move4wheel2 EC CruizCore_R1370P
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "EC.h" 00002 #include "R1370P.h" 00003 #include "move4wheel.h" 00004 #include "mbed.h" 00005 #include "math.h" 00006 #include "PathFollowing.h" 00007 #include "movement.h" 00008 #include "maxonsetting.h" 00009 #include "manual.h" 00010 #include "can.h" 00011 00012 #define PI 3.141592 00013 00014 //#define PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示する際に定義 00015 00016 int go_waitmode = 0; 00017 00018 /*------------運動時の各速度の設定----------*/ 00019 double st_speed = 500; // 移動開始時の速度 00020 double max_speed = 2500; // 直線移動時の最大速度 00021 double mid_speed = 2000; // 直線移動時の中間速度 00022 double turn_speed = 1000; // 旋回時の速度 00023 double end_speed = 200; // 停止直前の速度 00024 double fruit_speed = 200; // 果物を取る時の速度 00025 /*--------------------------------------*/ 00026 00027 //-----mbed led------------------//点灯条件-----------------------//参照場所------------------------------// 00028 //DigitalOut cansend_led(LED1); //cansend -> on //can.cpp 00029 //DigitalOut canread_led(LED2); //canread -> on //can.cpp 00030 //DigitalOut debug_led(LED3); //maxon debug programme -> on //maxonsetting.cpp 00031 00032 //////////////////////////////////////////////////////////////以下main文///////////////////////////////////////////////////////////////// 00033 00034 int main() 00035 { 00036 UserLoopSetting_maxon(); 00037 UserLoopSetting_sensor(); 00038 UserLoopSetting_can(); 00039 00040 #ifdef PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示 00041 00042 printf("ソースファイル名 : %s¥n", __FILE__); 00043 printf("作成日付 : %s¥n", __DATE__); 00044 printf("作成時刻 : %s¥n", __TIME__); 00045 00046 #endif 00047 00048 while(1) { 00049 //id1_value[0] = 1; 00050 switch(id1_value[0]) { 00051 //-----auto mode----------------------------------------------------------------------------------------------------------------------// 00052 case 1: 00053 //id1_value[6] = 0; 00054 switch(id1_value[6]) { 00055 case 0: 00056 //-----right mode-------------------------------------------------------------------------------------------------------------// 00057 //未修正 00058 00059 flag = 0; 00060 UserLoopSetting_enc_right(); 00061 if(T1 == 0) { //スタート位置からみかんの木まで移動 00062 /* 00063 gogo_straight(1,1,2962,3500,2962,2900,200,1000,5,0.1,10,0.1,600,0); 00064 purecurve(6,1,1,2962,2900,2317,2500,9,1000,5,0.1,10,0.1,600,0); 00065 purecurve(5,1,1,2317,2500,1672,2000,9,1000,5,0.1,10,0.1,600,0); 00066 set_cond(2,1,1050,1,1076); 00067 gogo_straight(0,0,1672,2000,1672,1600,1000,200,5,0.1,10,0.1,600,0); 00068 MaxonControl(0,0,0,0); 00069 pos_correction(1672,1600,0,0,0,25); 00070 enc_correction(1,1); 00071 gogo_straight(0,0,1672,1600,1672,1400,200,200,5,0.1,10,0.1,600,0); 00072 */ 00073 printf("t1 = 0\n\r"); 00074 wait(1); 00075 T1++; 00076 } 00077 if(T1 == 1) { 00078 while(1) { 00079 wait(0.1); 00080 //calc_xy(0,0,0); 00081 if(T1 == 2) { 00082 // enc_correction(1,1); 00083 break; 00084 } 00085 } 00086 } 00087 if(T1 == 2) { //みかんの木から三宝置き場まで移動 00088 /*gogo_straight(1,1,1672,1400,1672,2000,200,1000,5,0.1,10,0.1,600,0); 00089 purecurve(2,1,1,1672,2000,2317,2500,9,1000,5,0.1,10,0.1,600,0); 00090 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); 00091 gogo_straight(1,1,2962,3000,2962,4000,1000,1000,5,0.1,10,0.1,600,90); 00092 gogo_straight(1,1,2962,4000,2962,4500,1000,200,5,0.1,10,0.1,600,90); 00093 MaxonControl(0,0,0,0); 00094 // pos_correction(2962,4500,90,1,1); 00095 set_cond(2,1,2462,0,6000); 00096 gogo_straight(0,0,2962,4500,2850,4500,200,200,5,0.1,10,0.1,800,90); 00097 MaxonControl(0,0,0,0); 00098 pos_correction(2850,4500,90,0,0,10); 00099 enc_correction(1,1); 00100 wait(0.5);*/ 00101 printf("t1 = 2\n\r"); 00102 wait(1); 00103 T1++; 00104 } 00105 if(T1 == 3) { 00106 while(1) { 00107 wait(0.1); 00108 //calc_xy(90,0,0); 00109 if(T1 == 4) { 00110 // enc_correction(1,1); 00111 break; 00112 } 00113 } 00114 } 00115 if(T1 == 4) { //三宝置き場からりんごの木まで移動 00116 /*gogo_straight(1,1,2850,4500,2850,5150,200,1000,5,0.1,10,0.1,800,90); 00117 purecurve(3,1,1,2850,5150,2257,5500,9,1000,5,0.1,10,0.1,800,90); 00118 purecurve(4,1,1,2257,5500,1700,6000,9,1000,5,0.1,10,0.1,800,90); 00119 purecurve(2,1,1,1700,6000,2257,6550,9,1000,5,0.1,10,0.1,800,90); 00120 gogo_straight(1,1,2257,6550,2500,6550,1000,1000,5,0.1,10,0.1,800,90); 00121 set_cond(2,0,3500,1,6050); 00122 gogo_straight(0,0,2500,6550,2700,6600,1000,200,5,0.1,10,0.1,800,90); 00123 MaxonControl(0,0,0,0); 00124 pos_correction(2700,6600,90,0,0,15); 00125 enc_correction(1,1); 00126 wait(0.5);*/ 00127 printf("t1 = 4\n\r"); 00128 wait(1); 00129 T1++; 00130 } 00131 if(T1 == 5) { 00132 while(1) { 00133 wait(0.1); 00134 //calc_xy(90,0,0); 00135 if(T1 == 6) { 00136 // enc_correction(1,1); 00137 break; 00138 } 00139 } 00140 } 00141 if(T1 == 6) { //りんごの木からお供え台まで移動 00142 /*gogo_straight(1,1,-3112,3500,-2962,3100,st_speed,mid_speed,5,0.1,10,0.1,600,0); 00143 gogo_straight(1,1,-2962,3100,-2962,2900,mid_speed,turn_speed,5,0.1,10,0.1,600,0); 00144 purecurve(7,1,1,-2962,2900,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); 00145 purecurve(8,1,1,-2317,2500,-1672,2000,9,turn_speed,5,0.1,10,0.1,600,0); 00146 set_cond(2,0,-1050,1,1076); 00147 gogo_straight(0,0,-1672,2000,-1672,1600,turn_speed,end_speed,5,0.1,10,0.1,600,0); 00148 MaxonControl(0,0,0,0); 00149 pos_correction(-1672,1600,0,0,0,25); 00150 enc_correction(1,1); 00151 gogo_straight(0,0,-1672,1600,-1672,1375,fruit_speed,fruit_speed,5,0.1,10,0.1,600,0); 00152 MaxonControl(0,0,0,0); 00153 */ 00154 printf("t1 = 6\n\r"); 00155 wait(1); 00156 T1++; 00157 } 00158 if(T1 == 7) { 00159 while(1) { 00160 printf("ashi finished\n\r"); 00161 MaxonControl(0,0,0,0); 00162 if(id1_value[0] != 1)break; //これらは他のwhileにも入れる必要あり 00163 if(id1_value[6] != flag)break; 00164 } 00165 } 00166 break; 00167 00168 case 1: 00169 //-----left mode--------------------------------------------------------------------------------------------------------------// 00170 flag = 1; 00171 printf("start\n\r"); 00172 UserLoopSetting_enc_left(); 00173 if(T1 == 0) { //スタート位置からみかんの木まで移動 00174 00175 gogo_straight(1,1,-3112,3500,-2962,3100,st_speed,mid_speed,5,0.1,10,0.1,600,0); 00176 gogo_straight(1,1,-2962,3100,-2962,2900,mid_speed,turn_speed,5,0.1,10,0.1,600,0); 00177 purecurve(7,1,1,-2962,2900,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); 00178 purecurve(8,1,1,-2317,2500,-1672,2000,9,turn_speed,5,0.1,10,0.1,600,0); 00179 set_cond(2,0,-1050,1,1076); 00180 gogo_straight(0,0,-1672,2000,-1672,1600,turn_speed,end_speed,5,0.1,10,0.1,600,0); 00181 MaxonControl(0,0,0,0); 00182 pos_correction(-1672,1600,0,0,0,25); 00183 enc_correction(1,1); 00184 gogo_straight(0,0,-1672,1600,-1672,1375,fruit_speed,fruit_speed,5,0.1,10,0.1,600,0); 00185 MaxonControl(0,0,0,0); 00186 00187 T1++; 00188 } 00189 if(T1 == 1) { 00190 while(1) { 00191 wait(0.1); 00192 calc_xy(0,0,0); 00193 if(T1 == 2) { 00194 enc_correction(1,1); 00195 break; 00196 } 00197 } 00198 } 00199 if(T1 == 2) { //みかんの木から三宝置き場まで移動 00200 gogo_straight(1,1,-1672,1375,-1672,2000,st_speed,turn_speed,5,0.1,10,0.1,600,0); 00201 purecurve(3,1,1,-1672,2000,-2317,2500,9,turn_speed,5,0.1,10,0.1,600,0); 00202 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); 00203 gogo_straight(1,1,-2962,3000,-2962,4000,turn_speed,max_speed,5,0.1,10,0.1,600,-90); 00204 gogo_straight(1,1,-2962,4000,-2962,4820,max_speed,end_speed,5,0.1,10,0.1,600,-90); 00205 MaxonControl(0,0,0,0); 00206 set_cond(2,0,-2462,0,6000); 00207 pos_correction(-2962,4837.5,-90,0,0,7); 00208 gogo_straight(0,0,-2962,4837.5,-2800,4837.5,300,200,5,0.1,10,0.1,800,-90); 00209 MaxonControl(0,0,0,0); 00210 enc_correction(1,1); 00211 00212 T1++; 00213 } 00214 if(T1 == 3) { 00215 while(1) { 00216 wait(0.1); 00217 calc_xy(-90,0,0); 00218 if(T1 == 4) { 00219 enc_correction(1,1); 00220 break; 00221 } 00222 } 00223 } 00224 if(T1 == 4) { //三宝置き場からりんごの木まで移動 00225 gogo_straight(1,1,-2782,4837.5,-2782,5150,st_speed,turn_speed,5,0.1,10,0.1,800,-90); 00226 purecurve(2,1,1,-2850,5150,-2257,5500,9,turn_speed,5,0.1,10,0.1,800,-90); 00227 purecurve(1,1,1,-2257,5500,-1700,6000,9,turn_speed,5,0.1,10,0.1,800,-90); 00228 purecurve(3,1,1,-1700,6000,-2257,6550,9,turn_speed,5,0.1,10,0.1,800,-90); 00229 gogo_straight(1,1,-2257,6550,-2500,6593,turn_speed,end_speed,5,0.1,10,0.1,800,-90); 00230 MaxonControl(0,0,0,0); 00231 set_cond(1,0,0,1,6050); 00232 pos_correction(-2500,6600,-90,1,0,8); 00233 set_cond(2,1,-3162,1,6038); 00234 gogo_straight(0,0,-2500,6600,-2700,6600,fruit_speed,fruit_speed,5,0.1,10,0.1,800,-90); 00235 MaxonControl(0,0,0,0); 00236 enc_correction(1,1); 00237 00238 T1++; 00239 } 00240 if(T1 == 5) { 00241 while(1) { 00242 wait(0.1); 00243 calc_xy(-90,0,0); 00244 if(T1 == 6) { 00245 enc_correction(1,1); 00246 break; 00247 } 00248 } 00249 } 00250 if(T1 == 6) { //りんごの木からお供え台まで移動 00251 set_cond(1,0,0,0,6962); 00252 gogo_straight(1,0,-2700,6600,-2500,6550,st_speed,max_speed,5,0.1,10,0.1,800,-90); 00253 gogo_straight(1,0,-2500,6550,-1300,6550,max_speed,max_speed,5,0.1,10,0.1,800,-90); 00254 gogo_straight(1,0,-1300,6550,-850,6550,max_speed,turn_speed,5,0.1,10,0.1,800,-90); 00255 enc_correction(0,1); 00256 set_cond(2,0,-19,0,6962); 00257 purecurve(8,0,0,-850,6550,-519,6000,9,turn_speed,10,0.1,10,0.1,600,-180); 00258 enc_correction(1,0); 00259 set_cond(0,0,-19,0,0); 00260 gogo_straight(0,1,-519,6000,-519,5500,turn_speed,max_speed,5,0.1,10,0.1,600,-180); 00261 gogo_straight(0,1,-519,5500,-519,5000,max_speed,max_speed,5,0.1,10,0.1,600,-180); 00262 set_cond(2,1,-1000,1,4000); 00263 gogo_straight(0,0,-519,5000,-519,4600,max_speed,end_speed,5,0.1,10,0.1,800,-180); 00264 MaxonControl(0,0,0,0); 00265 pos_correction(-519,4370,-180,0,0,30); 00266 enc_correction(1,1); 00267 MaxonControl(0,0,0,0); 00268 00269 T1++; 00270 } 00271 if(T1 == 7) { 00272 while(1) { 00273 printf("ashi finished\n\r"); 00274 MaxonControl(0,0,0,0); 00275 00276 /*gogo_straight(1,1,-519,4500,-519,5000,200,1000,5,0.1,10,0.1,600,-180); 00277 gogo_straight(1,1,-519,5000,-519,6000,1000,1000,5,0.1,10,0.1,600,-180); 00278 gogo_straight(1,1,-519,6000,-519,6600,1000,200,5,0.1,10,0.1,600,-180); 00279 MaxonControl(0,0,0,0); 00280 set_cond(2,0,0,0,7000); 00281 //pos_correction(-519,6600,-180,0,0,30); 00282 calc_xy(-180,0,0); 00283 enc_correction(1,1); 00284 gogo_straight(1,1,-519,6600,-1000,6600,200,1000,5,0.1,10,0.1,600,-180); 00285 purecurve(5,1,1,-1000,6600,-1672,6000,9,1000,5,0.1,10,0.1,600,-180); 00286 purecurve(6,1,1,-1672,6000,-2462,5500,9,1000,5,0.1,10,0.1,600,-180); 00287 gogo_straight(1,1,-2462,5500,-2962,5500,1000,200,5,0.1,10,0.1,600,-180); 00288 MaxonControl(0,0,0,0); 00289 set_cond(2,1,-3500,0,6000); 00290 //pos_correction(-2962,5500,-180,0,0,30); 00291 enc_correction(1,1); 00292 calc_xy(-180,0,0); 00293 gogo_straight(1,1,-2962,5500,-2962,4700,200,1000,5,0.1,10,0.1,600,-90); 00294 gogo_straight(1,1,-2962,4700,-2962,4000,1000,1000,5,0.1,10,0.1,600,0); 00295 gogo_straight(1,1,-2962,4000,-2962,3500,1000,200,5,0.1,10,0.1,600,0); 00296 MaxonControl(0,0,0,0); 00297 set_cond(2,1,-3500,1,2000); 00298 pos_correction(-3112,3500,0,0,0,15); 00299 enc_correction(1,1); 00300 MaxonControl(0,0,0,0);*/ 00301 00302 if(id1_value[0] != 1)break; //これらは他のwhileにも入れる必要あり 00303 if(id1_value[6] != flag)break; 00304 } 00305 } 00306 00307 00308 00309 break; 00310 } 00311 //-----wait mode----------------------------------------------------------------------------------------------------------------------// 00312 case 0: 00313 00314 // printf("wait\n\r"); 00315 calc_xy(0,1,1); 00316 ashi_led(); 00317 MaxonControl(0,0,0,0); 00318 go_waitmode = 0; 00319 00320 break; 00321 //-----manual mode--------------------------------------------------------------------------------------------------------------------// 00322 case 2: 00323 00324 ManualOut(250,100,500,200); 00325 go_waitmode = 0; 00326 00327 break; 00328 } 00329 //------------------------------------------------------------------------------------------------------------------------------------// 00330 } 00331 }
Generated on Wed Jul 13 2022 01:53:48 by
1.7.2