ロボステ6期
/
NHK2020-main１
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: uw_28015 mbed move4wheel2 EC CruizCore_R6093U 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 "manual.h" 00009 #include "can.h" 00010 00011 #define PI 3.141592 00012 00013 //#define PROGRAM_INFO //プログラム使用時に使用プログラムの情報を最初に表示する際に定義 00014 00015 int go_waitmode = 0; 00016 00017 /*------------運動時の各速度の設定----------*/ 00018 double st_speed = 500; // 移動開始時の速度 00019 double max_speed = 2500; // 直線移動時の最大速度 00020 double mid_speed = 2000; // 直線移動時の中間速度 00021 double turn_speed = 1000; // 旋回時の速度 00022 double end_speed = 200; // 停止直前の速度 00023 double fruit_speed = 200; // 果物を取る時の速度 00024 /*--------------------------------------*/ 00025 00026 //-----mbed led------------------//点灯条件-----------------------//参照場所------------------------------// 00027 //DigitalOut cansend_led(LED1); //cansend -> on //can.cpp 00028 //DigitalOut canread_led(LED2); //canread -> on //can.cpp 00029 //DigitalOut debug_led(LED3); //maxon debug programme -> on //maxonsetting.cpp 00030 00031 //////////////////////////////////////////////////////////////以下main文///////////////////////////////////////////////////////////////// 00032 00033 int main() 00034 { 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 int move_flag = 0; 00049 while(1) { 00050 00051 id1_value[0] = 1; 00052 switch(id1_value[0]) { 00053 //-----auto mode----------------------------------------------------------------------------------------------------------------------// 00054 case 1: 00055 00056 00057 //set_condの引数詳細 00058 //xy_type:（0:Y軸平行の壁を読む/1:X軸平行の壁を読む/2:X,Y軸平行の壁を共に読む） 00059 //pm_typeX:（0:各軸正方向側の壁を読む/1:各軸負方向側の壁を読む） 00060 //x_base:超音波センサーで読む壁の座標(y軸並行の壁のx座標) 00061 //pm_typeY:（0:各軸正方向側の壁を読む/1:各軸負方向側の壁を読む） 00062 //y_base:超音波センサーで読む壁の座標(x軸平行の壁のy座標) 00063 00064 00065 00066 // set_cond(2,0,0,1,0); 00067 // int f = 0; 00068 // while(1) { 00069 // if(f > 20) break; 00070 // f++; 00071 // printf("wait\n\r"); 00072 // } 00073 //gogo_straight(0,0,0,0,100000,100000,200,800,5,0.1,10,0.1,250,0, 4095, 30); 00074 // wait(100000); 00075 00076 set_cond(2,0,1950,1,0); 00077 uwflag_change(1,0,0,1); 00078 gogo_straight(1,1,457,457,700,463,300,300,5,0.1,10,0.1,250,0, 4095, 200); 00079 gogo_straight(0,0,700,463,1300,500,300,150,5,0.1,10,0.1,250,0, 4095, 200); 00080 mt_stop(); 00081 wait(0.2); 00082 pos_correction(1450,500,0,0,0,100); 00083 enc_correction(1,1); 00084 wait(2); 00085 00086 gogo_straight(0,0,1500,500,700,500,300,300,5,0.1,10,0.1,250,0, 4095, 200); 00087 gogo_straight(0,0,700,500,530,500,300,100,5,0.1,10,0.1,250,0, 4095, 200); 00088 mt_stop(); 00089 uwflag_reset(); 00090 wait(1000); 00091 00092 00093 00094 /* spline_move(1, 1, 0, 0, 200,1000, 0,300,200,700,200, 800,5,0.1,10,0.1,500,0, 4095, 500, 10); 00095 gogo_straight(1,1,200,1000,200,2750,800,800,5,0.1,10,0.1,250,0, 4095, 200); 00096 purecurve(3,1,1,200,2750,-800,3500,9,800,10,0.1,10,0.1,250,-90,4095, 700); 00097 purecurve(5,1,1,-800,3500,-1800,2750,9,800,10,0.1,10,0.1,250,-90,4095, 700); 00098 gogo_straight(1,1,-1800,2750,-1800,1000,800,500,5,0.1,10,0.1,250,-90, 4095, 500); 00099 set_cond(1,0,-465,1,-415); 00100 gogo_straight(1,0,-1800,1000,-1800,300,500,100,5,0.1,10,0.1,150,-90, 4095, 50); 00101 mt_stop(); 00102 wait(0.2); 00103 enc_correction(0,1); 00104 pos_correction(-1800,0,-90,1,1,50); 00105 enc_correction2(-1800, 0); 00106 uwflag_reset(); 00107 00108 wait(2); 00109 gogo_straight(1,1,-1800,0,-1800,400,200,500,5,0.1,10,0.1,100,-90, 4095, 30); 00110 gogo_straight(1,1,-1800,400,-1800,2750,500,500,5,0.1,10,0.1,250,-90, 4095, 200); 00111 purecurve(2,1,1,-1800,2750,-800,3500,9,500,5,0.1,10,0.1,250,0,4095, 300); 00112 purecurve(3,1,1,-800,3500,200,2750,9,500,5,0.1,10,0.1,250,0,4095, 300); 00113 gogo_straight(1,1,200,2750,200,1000,500,500,5,0.1,10,0.1,250,0, 4095, 200); 00114 gogo_straight(1,1,200,1000,200,200,500,100,5,0.1,10,0.1,250,0, 4095, 30); 00115 mt_stop(); 00116 wait(0.2); 00117 pos_correction(0,0,0,1,1,50);*/ 00118 00119 00120 00121 /* gogo_straight(1,1,0,0,0,400,200,800,5,0.1,10,0.1,250,0, 4095, 30); 00122 gogo_straight(1,1,0,400,0,3000,800,800,5,0.1,10,0.1,250,0, 4095, 200); 00123 purecurve(3,1,1,0,3000,-1000,3750,9,800,5,0.1,10,0.1,250,-90,4095, 500); 00124 purecurve(5,1,1,-1000,3750,-2000,3000,9,800,5,0.1,10,0.1,250,-90,4095, 500); 00125 gogo_straight(1,1,-2000,3000,-2000,1000,800,800,5,0.1,10,0.1,250,-90, 4095, 500); 00126 //////set_cond(2,0,-600,1,-600); 00127 gogo_straight(1,1,-2000,1000,-2000,200,800,100,5,0.1,10,0.1,100,-90, 4095, 50); 00128 mt_stop(); 00129 wait(0.2); 00130 pos_correction(-2000,0,-90,1,1,50); 00131 enc_correction2(-2000, 0); 00132 00133 wait(2); 00134 gogo_straight(1,1,-2000,0,-2000,400,200,500,5,0.1,10,0.1,100,-90, 4095, 30); 00135 gogo_straight(1,1,-2000,400,-2000,3000,500,500,5,0.1,10,0.1,250,-90, 4095, 200); 00136 purecurve(2,1,1,-2000,3000,-1000,3750,9,500,5,0.1,10,0.1,250,0,4095, 300); 00137 purecurve(3,1,1,-1000,3750,0,3000,9,500,5,0.1,10,0.1,250,0,4095, 300); 00138 gogo_straight(1,1,0,3000,0,1000,500,500,5,0.1,10,0.1,250,0, 4095, 200); 00139 gogo_straight(1,1,0,1000,0,200,500,100,5,0.1,10,0.1,250,0, 4095, 30); 00140 mt_stop(); 00141 wait(0.2); 00142 pos_correction(0,0,0,1,1,50);*/ 00143 00144 00145 00146 /* gogo_straight(1,1,0,0,0,400,50,200,5,0.1,10,0.1,250,0, 4095, 30); 00147 gogo_straight(1,1,0,400,0,2000,500,500,5,0.1,10,0.1,250,0, 4095, 100); 00148 gogo_straight(1,1,0,2000,0,2700,500,100,5,0.1,10,0.1,250,0, 4095, 50); 00149 mt_stop(); 00150 wait(2); 00151 pos_correction(0,3000,0,1,1,50); 00152 00153 gogo_straight(1,1,0,3000,0,2600,50,500,5,0.1,10,0.1,0,0, 4095,30); 00154 gogo_straight(1,1,0,2600,0,1000,500,500,5,0.1,10,0.1,100,0, 4095, 100); 00155 gogo_straight(1,1,0,1000,0,300,500,100,5,0.1,10,0.1,0,0, 4095, 50); 00156 mt_stop(); 00157 wait(2); 00158 pos_correction(0,0,0,1,1,50);*/ 00159 00160 while(1) mt_stop(); 00161 00162 00163 move_flag = 1; 00164 break; 00165 00166 //-----wait mode----------------------------------------------------------------------------------------------------------------------// 00167 case 0: 00168 00169 calc_xy(0,1,1); 00170 ashi_led(); 00171 //MaxonControl(0,0,0,0); 00172 go_waitmode = 0; 00173 00174 break; 00175 //-----manual mode--------------------------------------------------------------------------------------------------------------------// 00176 case 2: 00177 00178 ManualOut(250,100,500,200); 00179 go_waitmode = 0; 00180 00181 break; 00182 } 00183 //------------------------------------------------------------------------------------------------------------------------------------// 00184 if(move_flag == 1)break; 00185 } 00186 00187 }
Generated on Fri Jul 15 2022 11:24:46 by
1.7.2