2022_gorobo_Ateam
/
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: mbed SBDBT arrc_mbed
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "scrp_slave.hpp" 00003 #include "sbdbt.hpp" 00004 #include "rotary_inc.hpp" 00005 00006 int main() 00007 { 00008 //---------------初期設定--------------- 00009 //scrp_slave 00010 ScrpSlave scrp(PC_12,PD_2,PH_1,0x0807f800); 00011 00012 //sbdbt 00013 sbdbt sb(PA_0,PA_1); 00014 00015 //scrp_slaveのid設定 00016 const int wheel_num = 1; 00017 //const int BeanbagGet_num = 2; 00018 const int BeanbagShot_num = 3; 00019 const int TakoGetPut_num = 4; 00020 const int TapeLed_num = 5; 00021 const int send_all = 255; 00022 00023 //コマンド番号設定 00024 //足回り用 00025 const int r_x_component_cmd = 1; 00026 const int r_y_component_cmd = 2; 00027 const int l2_cmd = 3; 00028 const int r2_cmd = 4; 00029 const int l1_cmd = 5; 00030 const int r1_cmd = 8; 00031 const int circle_cmd = 6; 00032 const int switch_cmd = 7; 00033 const int l_x_component_cmd = 9; 00034 const int l_y_component_cmd = 10; 00035 const int right_cmd = 11; 00036 00037 //ビーンバッグ回収用 00038 //const int move = 1; 00039 00040 //ビーンバッグ発射用 00041 const int shot = 2; 00042 00043 //農作物回収・設置用 00044 const int up = 1; 00045 const int down = 2; 00046 /*const int limit1 = 3; 00047 const int limit2 = 4; 00048 const int limit3 = 5; 00049 const int limit4 = 6;*/ 00050 00051 //テープLED用 00052 const int voice = 1; 00053 00054 //遠隔非常停止受信用 00055 00056 //全体送信用 00057 const int select = 50;//手動・自動切り替え 00058 const int stop_all = 51;//全プログラム停止,リセット機能 00059 00060 //変数 00061 bool auto_mode = false; 00062 //bool status_start = 0; 00063 bool auto_move = 0; 00064 bool field = 0; 00065 int r_x_component; 00066 int r_y_component; 00067 int l_x_component; 00068 int l_y_component; 00069 int l2_num; 00070 int r2_num; 00071 00072 DigitalIn status_stop(PB_14,PullUp); 00073 DigitalIn sw1(PB_6); 00074 sw1.mode(PullUp); 00075 00076 DigitalOut led(PA_10); 00077 00078 //RotaryInc rot1(PC_0,PC_1,M_PI*2,40*M_PI,4); 00079 //RotaryInc rot2(PC_3,PC_2,M_PI*2,40*M_PI,4); 00080 00081 //count 00082 int count_up; 00083 int count_down; 00084 int count_shot; 00085 int count_auto; 00086 //int count_start; 00087 int count_stop; 00088 int count_circle; 00089 //------------------------------------- 00090 00091 while(true) { 00092 printf("%d\n",sw1.read()); 00093 sb.button_state(); 00094 r_x_component = sb.rsx(); 00095 r_y_component = sb.rsy(); 00096 l_x_component = sb.lsx(); 00097 l_y_component = sb.lsy(); 00098 l2_num = sb.l2An(); 00099 r2_num = sb.r2An(); 00100 if(sb.select() == 1) { 00101 count_auto++; 00102 printf("%d\n",count_auto); 00103 if(count_auto == 1) { 00104 auto_mode =! auto_mode; 00105 } 00106 } else { 00107 count_auto = 0; 00108 } 00109 if(auto_mode == 1) { 00110 led = 1; 00111 scrp.send1(TapeLed_num,voice,4); 00112 //printf("auto_mode\n"); 00113 }else{ 00114 led = 0; 00115 //printf("manual_mode\n"); 00116 } 00117 00118 //足回り 00119 scrp.send1(wheel_num,r_x_component_cmd,r_x_component); 00120 wait_ms(5); 00121 scrp.send1(wheel_num,r_y_component_cmd,r_y_component); 00122 wait_ms(5); 00123 scrp.send1(wheel_num,l2_cmd,l2_num); 00124 wait_ms(5); 00125 scrp.send1(wheel_num,r2_cmd,r2_num); 00126 wait_ms(5); 00127 scrp.send1(wheel_num,l1_cmd,sb.l1()); 00128 wait_ms(5); 00129 scrp.send1(wheel_num,r1_cmd,sb.r1()); 00130 wait_ms(5); 00131 scrp.send1(wheel_num,l_x_component_cmd,l_x_component); 00132 wait_ms(5); 00133 scrp.send1(wheel_num,l_y_component_cmd,l_y_component); 00134 wait_ms(5); 00135 if(auto_mode == true){ 00136 if(sb.circle() == 1) { 00137 count_circle++; 00138 if(count_circle == 1) { 00139 auto_move =! auto_move; 00140 scrp.send1(BeanbagShot_num,circle_cmd,auto_move); 00141 wait_ms(5); 00142 } 00143 } else { 00144 count_circle = 0; 00145 scrp.send1(wheel_num,right_cmd,sb.right()); 00146 wait_ms(5); 00147 } 00148 scrp.send1(wheel_num,circle_cmd,auto_move); 00149 wait_ms(5); 00150 } 00151 00152 if(sw1.read() == 1){ 00153 field = 0; 00154 }else{ 00155 field = 1; 00156 } 00157 scrp.send1(wheel_num,switch_cmd,field); 00158 wait_ms(5); 00159 00160 //ビーンバッグ回収 00161 if(auto_mode == false) { 00162 scrp.send1(TapeLed_num,1,sb.left()); 00163 wait_ms(5); 00164 } 00165 00166 //ビーンバッグ発射 00167 if(auto_mode == true) { 00168 scrp.send1(BeanbagShot_num,shot,sb.square()); 00169 wait_ms(5); 00170 if(sb.square() == 1) { 00171 count_shot++; 00172 if(count_shot == 1) { 00173 scrp.send1(TapeLed_num,voice,5); 00174 } 00175 } else { 00176 count_shot = 0; 00177 } 00178 } 00179 //装填 00180 if(auto_mode == false) { 00181 scrp.send1(BeanbagShot_num,10,sb.cross()); 00182 wait_ms(5); 00183 } 00184 if(auto_mode == true){ 00185 if(sb.cross() == 1){ 00186 scrp.send1(TapeLed_num,1,10); 00187 wait_ms(5); 00188 } 00189 } 00190 00191 //農作物回収・設置 00192 if(sb.up() == 1) { 00193 count_up++; 00194 if(count_up == 1) { 00195 scrp.send1(TapeLed_num,voice,3); 00196 } 00197 } else { 00198 count_up = 0; 00199 } 00200 if(sb.down() == 1) { 00201 count_down++; 00202 if(count_down == 1) { 00203 scrp.send1(TapeLed_num,voice,2); 00204 } 00205 } else { 00206 count_down = 0; 00207 } 00208 scrp.send1(TakoGetPut_num,up,sb.up()); 00209 wait_ms(5); 00210 scrp.send1(TakoGetPut_num,down,sb.down()); 00211 wait_ms(5); 00212 00213 //テープLED 00214 00215 //全体送信 00216 scrp.send1(send_all,select,auto_mode);//自動・手動モード切り替え 00217 wait_ms(5); 00218 if(status_stop.read() == 1) { 00219 count_stop++; 00220 if(count_stop <= 100000){ 00221 scrp.send1(send_all,stop_all,1); 00222 } 00223 } else { 00224 scrp.send1(send_all,stop_all,0); 00225 count_stop = 0; 00226 } 00227 scrp.send1(send_all,stop_all,0); 00228 wait_ms(5); 00229 if(sb.start() == 1){ 00230 scrp.send1(send_all,stop_all,3); 00231 wait_ms(5); 00232 NVIC_SystemReset(); 00233 } 00234 } 00235 }
Generated on Sat Sep 10 2022 06:53:29 by
1.7.2