春ロボ1班(元F3RC4班+)
3/1
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 #define HARUROBO_TEST_MODE //テスト自動プログラム（練習・動作確認用）使用時に定義 00017 00018 int go_waitmode = 0; 00019 00020 //-----mbed led------------------//点灯条件-----------------------//参照場所------------------------------// 00021 //DigitalOut canread_led(LED1); //canread -> on //can.cpp 00022 //DigitalOut cansend_led(LED2); //cansend -> on //can.cpp 00023 //DigitalOut debug_led(LED3); //maxon debug programme -> on //maxonsetting.cpp 00024 00025 //////////////////////////////////////////////////////////////以下main文///////////////////////////////////////////////////////////////// 00026 00027 int main(){ 00028 00029 UserLoopSetting_maxon(); 00030 UserLoopSetting_sensor(); 00031 UserLoopSetting_can(); 00032 00033 #ifdef PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示 00034 00035 printf("ソースファイル名 : %s¥n", __FILE__); 00036 printf("作成日付 : %s¥n", __DATE__); 00037 printf("作成時刻 : %s¥n", __TIME__); 00038 00039 #endif 00040 00041 while(1){ 00042 00043 id1_value[0] = 1; 00044 00045 switch(id1_value[0]){ 00046 00047 //-----auto mode----------------------------------------------------------------------------------------------------------------------// 00048 case 1: 00049 00050 //-----テスト自動プログラム（練習・動作確認用--------------------------------------------------------------------// 00051 #ifdef HARUROBO_TEST_MODE 00052 00053 if(go_waitmode == 0){ 00054 00055 //can_start(); 00056 //set_cond(2,1,-700,1,-700); 00057 gogo_straight(1,1,-2962,3500,-2962,2900,200,1000,5,0.1,10,0.1,600,0); 00058 purecurve(7,1,1,-2962,2900,-2317,2500,9,1000,5,0.1,10,0.1,600,0); 00059 purecurve(8,1,1,-2317,2500,-1610,2000,9,1000,5,0.1,10,0.1,600,0); 00060 gogo_straight(1,1,-1610,2000,-1610,1250,1000,200,5,0.1,10,0.1,600,0); 00061 MaxonControl(0,0,0,0); 00062 set_cond(2,0,-1243,1,1080); 00063 pos_correction(-1610,1250,0,1,0); 00064 wait(0.5); 00065 00066 gogo_straight(1,1,-1610,1250,-1610,2000,200,1000,5,0.1,10,0.1,600,0); 00067 purecurve(3,1,1,-1610,2000,-2317,2500,9,1000,5,0.1,10,0.1,600,0); 00068 purecurve(4,1,1,-2317,2500,-2962,3000,9,1000,5,0.1,10,0.1,600,-90); 00069 gogo_straight(1,1,-2962,3000,-2962,4000,1000,1000,5,0.1,10,0.1,600,-90); 00070 gogo_straight(1,1,-2962,4000,-2962,4500,1000,200,5,0.1,10,0.1,600,-90); 00071 MaxonControl(0,0,0,0); 00072 pos_correction(-2962,4500,-90,1,1); 00073 00074 set_cond(0,0,-2462,0,0); 00075 gogo_straight(1,1,-2962,4500,-2850,4500,200,200,5,0.1,10,0.1,800,-90); 00076 MaxonControl(0,0,0,0); 00077 pos_correction(-2850,4500,-90,1,1); 00078 wait(0.5); 00079 00080 gogo_straight(1,1,-2850,4500,-2850,5150,200,1000,5,0.1,10,0.1,800,-90); 00081 purecurve(2,1,1,-2850,5150,-2257,5500,9,1000,5,0.1,10,0.1,800,-90); 00082 purecurve(1,1,1,-2257,5500,-1700,6000,9,1000,5,0.1,10,0.1,800,-90); 00083 purecurve(3,1,1,-1700,6000,-2257,6647,9,1000,5,0.1,10,0.1,800,-90); 00084 gogo_straight(1,1,-2257,6647,-2500,6647,1000,1000,5,0.1,10,0.1,800,-90); 00085 set_cond(2,1,-3500,1,6324); 00086 gogo_straight(1,1,-2500,6647,-2700,6647,1000,200,5,0.1,10,0.1,800,-90); 00087 MaxonControl(0,0,0,0); 00088 pos_correction(-2700,6647,-90,1,1); 00089 wait(0.5); 00090 00091 gogo_straight(1,1,-2700,6647,-2500,6647,200,1000,5,0.1,10,0.1,800,-90); 00092 gogo_straight(1,1,-2500,6647,-1000,6647,1000,1000,5,0.1,10,0.1,800,-90); 00093 purecurve(8,1,1,-1000,6647,-350,6000,9,1000,5,0.1,10,0.1,600,-180); 00094 gogo_straight(1,1,-350,6000,-350,4700,1000,1000,5,0.1,10,0.1,600,-180); 00095 set_cond(2,1,1243,1,4000); 00096 gogo_straight(1,1,-350,4700,-350,4500,1000,200,5,0.1,10,0.1,800,-180); 00097 MaxonControl(0,0,0,0); 00098 pos_correction(-400,4500,-180,1,1); 00099 00100 go_waitmode = 1; 00101 00102 }else if(go_waitmode == 1){ 00103 00104 MaxonControl(0,0,0,0); 00105 00106 } 00107 00108 00109 #endif 00110 //--------------------------------------------------------------------------------------------------------// 00111 00112 00113 00114 00115 //-----本番用自動プログラム（右側フィールド用）-------------------------------------------------------------------// 00116 #ifdef HARUROBO_RIGHT_MODE 00117 00118 //ここに本番用自動プログラム（右側フィールド用）を書く 00119 00120 #endif 00121 //---------------------------------------------------------------------------------------------------------// 00122 00123 00124 00125 //-----本番用自動プログラム（左側フィールド用）-------------------------------------------------------------------// 00126 #ifdef HARUROBO_LEFT_MODE // 00127 00128 //ここに本番用自動プログラム（左側フィールド用）を書く 00129 00130 #endif 00131 //---------------------------------------------------------------------------------------------------------// 00132 MaxonControl(0,0,0,0); 00133 break; 00134 00135 //-----wait mode----------------------------------------------------------------------------------------------------------------------// 00136 case 0: 00137 00138 calc_xy(0,1,1); 00139 ashi_led(); 00140 MaxonControl(0,0,0,0); 00141 printf("now_angle: %f, now_x: %f, now_y: %f\n\r",now_angle,now_x,now_y); 00142 00143 break; 00144 00145 //-----manual mode--------------------------------------------------------------------------------------------------------------------// 00146 case 2: 00147 00148 ManualOut(250,100,500,200); 00149 00150 break; 00151 //------------------------------------------------------------------------------------------------------------------------------------// 00152 } 00153 } 00154 }
Generated on Sat Jul 16 2022 00:32:08 by
1.7.2