春ロボ1班(元F3RC4班+)
/
harurobo_ashi_3_6
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
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 #define HARUROBO_TEST_MODE 00016 //#define HARUROBO_MAIN_MODE 00017 00018 int go_waitmode = 0; 00019 00020 //-----mbed led------------------//点灯条件-----------------------//参照場所------------------------------// 00021 //DigitalOut cansend_led(LED1); //canread -> on //can.cpp 00022 //DigitalOut canread_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 #ifdef HARUROBO_TEST_MODE 00042 T1 = 0; 00043 printf("start\n\r"); 00044 //printf("T1 = %d\n\r",T1); 00045 if(T1 == 0) { 00046 00047 //スタート位置からみかんの木まで移動 00048 printf("T1 = 0\n\r"); 00049 T1++; 00050 //printf("t = %d\n\r",T1); 00051 } 00052 00053 if(T1 == 1) { 00054 while(1) { 00055 printf("T1 = 1\n\r"); 00056 wait(0.5); 00057 if(T1 == 2) { 00058 break; 00059 } 00060 } 00061 } 00062 00063 if(T1 == 2) { 00064 00065 //みかんの木から三宝置き場まで移動 00066 printf("T1 = 2\n\r"); 00067 T1++; 00068 } 00069 00070 if(T1 == 3) { 00071 while(1) { 00072 printf("T1 = 3\n\r"); 00073 if(T1 == 4) { 00074 00075 break; 00076 } 00077 } 00078 } 00079 00080 if(T1 == 4) { 00081 00082 //三宝置き場からりんごの木まで移動 00083 printf("t = 4"); 00084 T1++; 00085 } 00086 00087 if(T1 == 5) { 00088 while(1) { 00089 if(T1 == 6) { 00090 break; 00091 } 00092 } 00093 } 00094 00095 if(T1 == 6) { 00096 00097 //りんごの木からお供え台まで移動 00098 printf("t = 6"); 00099 T1++; 00100 } 00101 #endif 00102 00103 #ifdef HARUROBO_MAIN_MODE 00104 while(1) { 00105 00106 id1_value[0] = 1; 00107 switch(id1_value[0]) { 00108 //-----auto mode----------------------------------------------------------------------------------------------------------------------// 00109 case 1: 00110 00111 id1_value[6] = 1; 00112 switch(id1_value[6]) { 00113 case 0: 00114 //-----right mode-------------------------------------------------------------------------------------------------------------// 00115 00116 break; 00117 case 1: 00118 //-----left mode--------------------------------------------------------------------------------------------------------------// 00119 00120 go_waitmode = 0; 00121 if(go_waitmode == 0) { 00122 00123 00124 00125 //can_start(); 00126 gogo_straight(1,1,-2962,3500,-2962,2900,200,1000,5,0.1,10,0.1,600,0); 00127 purecurve(7,1,1,-2962,2900,-2317,2500,9,1000,5,0.1,10,0.1,600,0); 00128 purecurve(8,1,1,-2317,2500,-1672,2000,9,1000,5,0.1,10,0.1,600,0); 00129 gogo_straight(1,1,-1672,2000,-1672,1400,1000,200,5,0.1,10,0.1,600,0); 00130 MaxonControl(0,0,0,0); 00131 set_cond(2,0,-1243,1,1076); 00132 pos_correction(-1672,1400,0,0,0); 00133 enc_correction(1,1); 00134 wait(0.5); 00135 00136 gogo_straight(1,1,-1672,1400,-1672,2000,200,1000,5,0.1,10,0.1,600,0); 00137 purecurve(3,1,1,-1672,2000,-2317,2500,9,1000,5,0.1,10,0.1,600,0); 00138 purecurve(4,1,1,-2317,2500,-2962,3000,9,1000,5,0.1,10,0.1,600,-90); 00139 gogo_straight(1,1,-2962,3000,-2962,4000,1000,1000,5,0.1,10,0.1,600,-90); 00140 gogo_straight(1,1,-2962,4000,-2962,4500,1000,200,5,0.1,10,0.1,600,-90); 00141 MaxonControl(0,0,0,0); 00142 pos_correction(-2962,4500,-90,1,1); 00143 00144 set_cond(0,0,-2462,0,0); 00145 gogo_straight(0,1,-2962,4500,-2850,4500,200,200,5,0.1,10,0.1,800,-90); 00146 MaxonControl(0,0,0,0); 00147 pos_correction(-2850,4500,-90,0,1); 00148 enc_correction(1,0); 00149 wait(0.5); 00150 00151 gogo_straight(1,1,-2850,4500,-2850,5150,200,1000,5,0.1,10,0.1,800,-90); 00152 purecurve(2,1,1,-2850,5150,-2257,5500,9,1000,5,0.1,10,0.1,800,-90); 00153 purecurve(1,1,1,-2257,5500,-1700,6000,9,1000,5,0.1,10,0.1,800,-90); 00154 purecurve(3,1,1,-1700,6000,-2257,6647,9,1000,5,0.1,10,0.1,800,-90); 00155 gogo_straight(1,1,-2257,6647,-2500,6647,1000,1000,5,0.1,10,0.1,800,-90); 00156 set_cond(2,1,-3500,1,6324); 00157 gogo_straight(0,0,-2500,6647,-2700,6647,1000,200,5,0.1,10,0.1,800,-90); 00158 MaxonControl(0,0,0,0); 00159 pos_correction(-2700,6647,-90,0,0); 00160 enc_correction(1,1); 00161 wait(0.5); 00162 00163 gogo_straight(1,1,-2700,6647,-2500,6647,200,1000,5,0.1,10,0.1,800,-90); 00164 gogo_straight(1,1,-2500,6647,-1000,6647,1000,1000,5,0.1,10,0.1,800,-90); 00165 purecurve(8,1,1,-1000,6647,-519,6000,9,1000,5,0.1,10,0.1,600,-180); 00166 gogo_straight(1,1,-519,6000,-519,4700,1000,1000,5,0.1,10,0.1,600,-180); 00167 set_cond(2,1,1243,1,4000); 00168 gogo_straight(0,0,-519,4700,-519,4500,1000,200,5,0.1,10,0.1,800,-180); 00169 MaxonControl(0,0,0,0); 00170 pos_correction(-519,4500,-180,0,0); 00171 MaxonControl(0,0,0,0); 00172 00173 go_waitmode = 1; 00174 00175 } else if(go_waitmode == 1) { 00176 00177 MaxonControl(0,0,0,0); 00178 00179 } 00180 break; 00181 } 00182 //-----wait mode----------------------------------------------------------------------------------------------------------------------// 00183 case 0: 00184 00185 calc_xy(0,1,1); 00186 ashi_led(); 00187 MaxonControl(0,0,0,0); 00188 go_waitmode = 0; 00189 00190 break; 00191 //-----manual mode--------------------------------------------------------------------------------------------------------------------// 00192 case 2: 00193 00194 ManualOut(250,100,500,200); 00195 go_waitmode = 0; 00196 00197 break; 00198 } 00199 //------------------------------------------------------------------------------------------------------------------------------------// 00200 } 00201 #endif 00202 } 00203
Generated on Mon Aug 8 2022 01:32:49 by
1.7.2