2022_gorobo_Ateam
メインプログラム最新版です
Dependencies: mbed SBDBT arrc_mbed
Diff: main.cpp
- Revision:
- 10:4fe47396eb5a
- Parent:
- 8:e76d6c16d7af
- Child:
- 11:9ae7fe40a807
diff -r dcdd3776fa2b -r 4fe47396eb5a main.cpp
--- a/main.cpp Fri Mar 11 04:23:20 2022 +0000
+++ b/main.cpp Tue Mar 22 00:36:40 2022 +0000
@@ -1,100 +1,123 @@
#include "mbed.h"
#include "scrp_slave.hpp"
#include "sbdbt.hpp"
-#include "mp3.hpp"
+#include "rotary_inc.hpp"
-int main(){
+int main()
+{
//---------------初期設定---------------
//scrp_slave
ScrpSlave scrp(PC_12,PD_2,PH_1,0x0807f800);
-
+
//sbdbt
sbdbt sb(PA_0,PA_1);
-
+
//scrp_slaveのid設定
const int wheel_num = 1;
const int BeanbagGet_num = 2;
const int BeanbagShot_num = 3;
const int TakoGetPut_num = 4;
- const int remort_stop_num = 5;
+ const int TapeLed_num = 5;
const int send_all = 255;
-
+
//コマンド番号設定
//足回り用
- const int x_component_cmd = 1;
- const int y_component_cmd = 2;
+ const int r_x_component_cmd = 1;
+ const int r_y_component_cmd = 2;
const int l2_cmd = 3;
const int r2_cmd = 4;
const int l1_cmd = 5;
-
+ const int r1_cmd = 8;
+ const int circle_cmd = 6;
+ const int switch_cmd = 7;
+ const int l_x_component_cmd = 9;
+ const int l_y_component_cmd = 10;
+
//ビーンバッグ回収用
const int move = 1;
-
+
//ビーンバッグ発射用
const int shot = 2;
-
+
//農作物回収・設置用
const int up = 1;
const int down = 2;
- const int limit1 = 3;
+ /*const int limit1 = 3;
const int limit2 = 4;
-
+ const int limit3 = 5;
+ const int limit4 = 6;*/
+
//テープLED用
-
+ const int voice = 1;
+ const int start = 2;
+
//遠隔非常停止受信用
-
+
//全体送信用
const int select = 50;//手動・自動切り替え
const int stop_all = 51;//全プログラム停止
-
+
//変数
bool auto_mode = false;
- bool auto_start;
- int x_component;
- int y_component;
+ bool status_start = 0;
+ bool auto_move = 0;
+ int r_x_component;
+ int r_y_component;
+ int l_x_component;
+ int l_y_component;
int l2_num;
int r2_num;
+
+ DigitalIn status_stop(PB_14,PullUp);
+ DigitalIn sw1(PB_6);
+ sw1.mode(PullUp);
- //mp3
- Playmp3 voice(PA_9,PA_10);
- int volume = 200;//音量
- int track;
- voice.set_volume(200);
- voice.quiet_mode(false);
+ DigitalOut led(PA_10);
- DigitalIn sw1(PB_4);
- DigitalIn sw2(PB_5);
- sw1.mode(PullUp);
- sw2.mode(PullUp);
-
+ //RotaryInc rot1(PC_0,PC_1,M_PI*2,40*M_PI,4);
+ //RotaryInc rot2(PC_3,PC_2,M_PI*2,40*M_PI,4);
+
//count
int count_up;
int count_down;
int count_shot;
int count_auto;
+ int count_start;
+ int count_stop;
+ int count_circle;
//-------------------------------------
- while(true){
+ while(true) {
printf("%d\n",sw1.read());
-
sb.button_state();
- x_component = sb.rsx();
- y_component = sb.rsy();
+ r_x_component = sb.rsx();
+ r_y_component = sb.rsy();
+ l_x_component = sb.lsx();
+ l_y_component = sb.lsy();
l2_num = sb.l2An();
r2_num = sb.r2An();
- if(sb.select() == 1){
+ if(sb.select() == 1) {
count_auto++;
- if(count_auto == 1){
- auto_mode =! auto_mode;
- }
+ printf("%d\n",count_auto);
+ if(count_auto == 1) {
+ auto_mode =! auto_mode;
+ }
+ } else {
+ count_auto = 0;
+ }
+ if(auto_mode == 1) {
+ led = 1;
+ scrp.send1(TapeLed_num,voice,4);
+ //printf("auto_mode\n");
}else{
- count_auto = 0;
+ led = 0;
+ //printf("manual_mode\n");
}
-
+
//足回り
- scrp.send1(wheel_num,x_component_cmd,x_component);
+ scrp.send1(wheel_num,r_x_component_cmd,r_x_component);
wait_ms(2);
- scrp.send1(wheel_num,y_component_cmd,y_component);
+ scrp.send1(wheel_num,r_y_component_cmd,r_y_component);
wait_ms(2);
scrp.send1(wheel_num,l2_cmd,l2_num);
wait_ms(2);
@@ -102,61 +125,94 @@
wait_ms(2);
scrp.send1(wheel_num,l1_cmd,sb.l1());
wait_ms(2);
-
- //ビーンバッグ回収
- scrp.send1(BeanbagGet_num,move,sb.square());
+ scrp.send1(wheel_num,r1_cmd,sb.r1());
+ wait_ms(2);
+ scrp.send1(wheel_num,l_x_component_cmd,l_x_component);
+ wait_ms(2);
+ scrp.send1(wheel_num,l_y_component_cmd,l_y_component);
wait_ms(2);
-
+ if(auto_mode == true){
+ if(sb.circle() == 1) {
+ count_circle++;
+ if(count_circle == 1) {
+ auto_move =! auto_move;
+ }
+ } else {
+ count_circle = 0;
+ }
+ scrp.send1(wheel_num,circle_cmd,auto_move);
+ wait_ms(2);
+ }
+ scrp.send1(wheel_num,switch_cmd,sw1.read());
+ wait_ms(2);
+
+ //ビーンバッグ回収
+ if(auto_mode == false) {
+ scrp.send1(BeanbagGet_num,move,sb.square());
+ wait_ms(2);
+ }
+
//ビーンバッグ発射
- if(auto_mode == true){
+ if(auto_mode == true) {
scrp.send1(BeanbagShot_num,shot,sb.square());
wait_ms(2);
- if(sb.square() == 1){
+ if(sb.square() == 1) {
count_shot++;
- if(count_shot == 1){
- voice.set_number_of_tracks(5);
+ if(count_shot == 1) {
+ scrp.send1(TapeLed_num,voice,5);
}
- }else{
+ } else {
count_shot = 0;
}
}
-
+
//農作物回収・設置
- if(sb.up() == 1){
- count_up++;
- if(count_up == 1){
- voice.set_number_of_tracks(1);
- }
- }else{
- count_up = 0;
+ if(sb.up() == 1) {
+ count_up++;
+ if(count_up == 1) {
+ scrp.send1(TapeLed_num,voice,1);
}
- if(sb.down() == 1){
- count_down++;
- if(count_down == 1){
- voice.set_number_of_tracks(2);
- }
- }else{
- count_down = 0;
+ } else {
+ count_up = 0;
+ }
+ if(sb.down() == 1) {
+ count_down++;
+ if(count_down == 1) {
+ scrp.send1(TapeLed_num,voice,2);
}
+ } else {
+ count_down = 0;
+ }
scrp.send1(TakoGetPut_num,up,sb.up());
wait_ms(2);
scrp.send1(TakoGetPut_num,down,sb.down());
wait_ms(2);
- if(sw1.read() == 0){
- scrp.send1(TakoGetPut_num,limit1,1);
- }else{
- scrp.send1(TakoGetPut_num,limit1,0);
+
+ //テープLED
+ if(sb.start() == 1) {
+ count_auto++;
+ if(count_start == 1) {
+ status_start =! status_start;
+ }
+ } else {
+ count_start = 0;
}
+ scrp.send1(TapeLed_num,start,status_start);
wait_ms(2);
- if(sw2.read() == 0){
- scrp.send1(TakoGetPut_num,limit2,1);
- }else{
- scrp.send1(TakoGetPut_num,limit2,0);
- }
- wait_ms(2);
-
+
//全体送信
scrp.send1(send_all,select,auto_mode);//自動・手動モード切り替え
wait_ms(2);
+ if(status_stop.read() == 1) {
+ count_stop++;
+ if(count_stop <= 100000){
+ scrp.send1(send_all,stop_all,1);
+ }
+ } else {
+ scrp.send1(send_all,stop_all,0);
+ count_stop = 0;
+ }
+ scrp.send1(send_all,stop_all,0);
+ wait_ms(2);
}
}
\ No newline at end of file