2022_gorobo_Ateam / Mbed 2 deprecated MAIN

メインプログラム最新版です

Dependencies:   mbed SBDBT arrc_mbed

main.cpp@10:4fe47396eb5a, 2022-03-22 (annotated)

Committer:
guesta
Date:
Tue Mar 22 00:36:40 2022 +0000
Revision:
10:4fe47396eb5a
Parent:
8:e76d6c16d7af
Child:
11:9ae7fe40a807
new;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
guesta 0:d8faed605a5c 1 #include "mbed.h"
guesta 3:e4b5563563af 2 #include "scrp_slave.hpp"
guesta 3:e4b5563563af 3 #include "sbdbt.hpp"
guesta 10:4fe47396eb5a 4 #include "rotary_inc.hpp"
guesta 0:d8faed605a5c 5
guesta 10:4fe47396eb5a 6 int main()
guesta 10:4fe47396eb5a 7 {
guesta 1:642ee25ea489 8 //---------------初期設定---------------
guesta 3:e4b5563563af 9 //scrp_slave
guesta 3:e4b5563563af 10 ScrpSlave scrp(PC_12,PD_2,PH_1,0x0807f800);
guesta 10:4fe47396eb5a 11
guesta 3:e4b5563563af 12 //sbdbt
guesta 8:e76d6c16d7af 13 sbdbt sb(PA_0,PA_1);
guesta 10:4fe47396eb5a 14
guesta 3:e4b5563563af 15 //scrp_slaveのid設定
guesta 6:fd9b59f828e8 16 const int wheel_num = 1;
guesta 3:e4b5563563af 17 const int BeanbagGet_num = 2;
guesta 3:e4b5563563af 18 const int BeanbagShot_num = 3;
guesta 3:e4b5563563af 19 const int TakoGetPut_num = 4;
guesta 10:4fe47396eb5a 20 const int TapeLed_num = 5;
guesta 6:fd9b59f828e8 21 const int send_all = 255;
guesta 10:4fe47396eb5a 22
guesta 3:e4b5563563af 23 //コマンド番号設定
guesta 6:fd9b59f828e8 24 //足回り用
guesta 10:4fe47396eb5a 25 const int r_x_component_cmd = 1;
guesta 10:4fe47396eb5a 26 const int r_y_component_cmd = 2;
guesta 3:e4b5563563af 27 const int l2_cmd = 3;
guesta 3:e4b5563563af 28 const int r2_cmd = 4;
guesta 7:59be95fceafa 29 const int l1_cmd = 5;
guesta 10:4fe47396eb5a 30 const int r1_cmd = 8;
guesta 10:4fe47396eb5a 31 const int circle_cmd = 6;
guesta 10:4fe47396eb5a 32 const int switch_cmd = 7;
guesta 10:4fe47396eb5a 33 const int l_x_component_cmd = 9;
guesta 10:4fe47396eb5a 34 const int l_y_component_cmd = 10;
guesta 10:4fe47396eb5a 35
guesta 6:fd9b59f828e8 36 //ビーンバッグ回収用
guesta 8:e76d6c16d7af 37 const int move = 1;
guesta 10:4fe47396eb5a 38
guesta 6:fd9b59f828e8 39 //ビーンバッグ発射用
guesta 8:e76d6c16d7af 40 const int shot = 2;
guesta 10:4fe47396eb5a 41
guesta 6:fd9b59f828e8 42 //農作物回収・設置用
guesta 6:fd9b59f828e8 43 const int up = 1;
guesta 6:fd9b59f828e8 44 const int down = 2;
guesta 10:4fe47396eb5a 45 /*const int limit1 = 3;
guesta 8:e76d6c16d7af 46 const int limit2 = 4;
guesta 10:4fe47396eb5a 47 const int limit3 = 5;
guesta 10:4fe47396eb5a 48 const int limit4 = 6;*/
guesta 10:4fe47396eb5a 49
guesta 6:fd9b59f828e8 50 //テープLED用
guesta 10:4fe47396eb5a 51 const int voice = 1;
guesta 10:4fe47396eb5a 52 const int start = 2;
guesta 10:4fe47396eb5a 53
guesta 6:fd9b59f828e8 54 //遠隔非常停止受信用
guesta 10:4fe47396eb5a 55
guesta 6:fd9b59f828e8 56 //全体送信用
guesta 6:fd9b59f828e8 57 const int select = 50;//手動・自動切り替え
guesta 6:fd9b59f828e8 58 const int stop_all = 51;//全プログラム停止
guesta 10:4fe47396eb5a 59
guesta 1:642ee25ea489 60 //変数
guesta 7:59be95fceafa 61 bool auto_mode = false;
guesta 10:4fe47396eb5a 62 bool status_start = 0;
guesta 10:4fe47396eb5a 63 bool auto_move = 0;
guesta 10:4fe47396eb5a 64 int r_x_component;
guesta 10:4fe47396eb5a 65 int r_y_component;
guesta 10:4fe47396eb5a 66 int l_x_component;
guesta 10:4fe47396eb5a 67 int l_y_component;
guesta 3:e4b5563563af 68 int l2_num;
guesta 3:e4b5563563af 69 int r2_num;
guesta 10:4fe47396eb5a 70
guesta 10:4fe47396eb5a 71 DigitalIn status_stop(PB_14,PullUp);
guesta 10:4fe47396eb5a 72 DigitalIn sw1(PB_6);
guesta 10:4fe47396eb5a 73 sw1.mode(PullUp);
guesta 8:e76d6c16d7af 74
guesta 10:4fe47396eb5a 75 DigitalOut led(PA_10);
guesta 8:e76d6c16d7af 76
guesta 10:4fe47396eb5a 77 //RotaryInc rot1(PC_0,PC_1,M_PI*2,40*M_PI,4);
guesta 10:4fe47396eb5a 78 //RotaryInc rot2(PC_3,PC_2,M_PI*2,40*M_PI,4);
guesta 10:4fe47396eb5a 79
guesta 8:e76d6c16d7af 80 //count
guesta 8:e76d6c16d7af 81 int count_up;
guesta 8:e76d6c16d7af 82 int count_down;
guesta 8:e76d6c16d7af 83 int count_shot;
guesta 8:e76d6c16d7af 84 int count_auto;
guesta 10:4fe47396eb5a 85 int count_start;
guesta 10:4fe47396eb5a 86 int count_stop;
guesta 10:4fe47396eb5a 87 int count_circle;
guesta 3:e4b5563563af 88 //-------------------------------------
guesta 6:fd9b59f828e8 89
guesta 10:4fe47396eb5a 90 while(true) {
guesta 8:e76d6c16d7af 91 printf("%d\n",sw1.read());
guesta 3:e4b5563563af 92 sb.button_state();
guesta 10:4fe47396eb5a 93 r_x_component = sb.rsx();
guesta 10:4fe47396eb5a 94 r_y_component = sb.rsy();
guesta 10:4fe47396eb5a 95 l_x_component = sb.lsx();
guesta 10:4fe47396eb5a 96 l_y_component = sb.lsy();
guesta 3:e4b5563563af 97 l2_num = sb.l2An();
guesta 3:e4b5563563af 98 r2_num = sb.r2An();
guesta 10:4fe47396eb5a 99 if(sb.select() == 1) {
guesta 8:e76d6c16d7af 100 count_auto++;
guesta 10:4fe47396eb5a 101 printf("%d\n",count_auto);
guesta 10:4fe47396eb5a 102 if(count_auto == 1) {
guesta 10:4fe47396eb5a 103 auto_mode =! auto_mode;
guesta 10:4fe47396eb5a 104 }
guesta 10:4fe47396eb5a 105 } else {
guesta 10:4fe47396eb5a 106 count_auto = 0;
guesta 10:4fe47396eb5a 107 }
guesta 10:4fe47396eb5a 108 if(auto_mode == 1) {
guesta 10:4fe47396eb5a 109 led = 1;
guesta 10:4fe47396eb5a 110 scrp.send1(TapeLed_num,voice,4);
guesta 10:4fe47396eb5a 111 //printf("auto_mode\n");
guesta 8:e76d6c16d7af 112 }else{
guesta 10:4fe47396eb5a 113 led = 0;
guesta 10:4fe47396eb5a 114 //printf("manual_mode\n");
guesta 7:59be95fceafa 115 }
guesta 10:4fe47396eb5a 116
guesta 6:fd9b59f828e8 117 //足回り
guesta 10:4fe47396eb5a 118 scrp.send1(wheel_num,r_x_component_cmd,r_x_component);
guesta 7:59be95fceafa 119 wait_ms(2);
guesta 10:4fe47396eb5a 120 scrp.send1(wheel_num,r_y_component_cmd,r_y_component);
guesta 7:59be95fceafa 121 wait_ms(2);
guesta 3:e4b5563563af 122 scrp.send1(wheel_num,l2_cmd,l2_num);
guesta 7:59be95fceafa 123 wait_ms(2);
guesta 3:e4b5563563af 124 scrp.send1(wheel_num,r2_cmd,r2_num);
guesta 7:59be95fceafa 125 wait_ms(2);
guesta 7:59be95fceafa 126 scrp.send1(wheel_num,l1_cmd,sb.l1());
guesta 7:59be95fceafa 127 wait_ms(2);
guesta 10:4fe47396eb5a 128 scrp.send1(wheel_num,r1_cmd,sb.r1());
guesta 10:4fe47396eb5a 129 wait_ms(2);
guesta 10:4fe47396eb5a 130 scrp.send1(wheel_num,l_x_component_cmd,l_x_component);
guesta 10:4fe47396eb5a 131 wait_ms(2);
guesta 10:4fe47396eb5a 132 scrp.send1(wheel_num,l_y_component_cmd,l_y_component);
guesta 8:e76d6c16d7af 133 wait_ms(2);
guesta 10:4fe47396eb5a 134 if(auto_mode == true){
guesta 10:4fe47396eb5a 135 if(sb.circle() == 1) {
guesta 10:4fe47396eb5a 136 count_circle++;
guesta 10:4fe47396eb5a 137 if(count_circle == 1) {
guesta 10:4fe47396eb5a 138 auto_move =! auto_move;
guesta 10:4fe47396eb5a 139 }
guesta 10:4fe47396eb5a 140 } else {
guesta 10:4fe47396eb5a 141 count_circle = 0;
guesta 10:4fe47396eb5a 142 }
guesta 10:4fe47396eb5a 143 scrp.send1(wheel_num,circle_cmd,auto_move);
guesta 10:4fe47396eb5a 144 wait_ms(2);
guesta 10:4fe47396eb5a 145 }
guesta 10:4fe47396eb5a 146 scrp.send1(wheel_num,switch_cmd,sw1.read());
guesta 10:4fe47396eb5a 147 wait_ms(2);
guesta 10:4fe47396eb5a 148
guesta 10:4fe47396eb5a 149 //ビーンバッグ回収
guesta 10:4fe47396eb5a 150 if(auto_mode == false) {
guesta 10:4fe47396eb5a 151 scrp.send1(BeanbagGet_num,move,sb.square());
guesta 10:4fe47396eb5a 152 wait_ms(2);
guesta 10:4fe47396eb5a 153 }
guesta 10:4fe47396eb5a 154
guesta 6:fd9b59f828e8 155 //ビーンバッグ発射
guesta 10:4fe47396eb5a 156 if(auto_mode == true) {
guesta 8:e76d6c16d7af 157 scrp.send1(BeanbagShot_num,shot,sb.square());
guesta 7:59be95fceafa 158 wait_ms(2);
guesta 10:4fe47396eb5a 159 if(sb.square() == 1) {
guesta 8:e76d6c16d7af 160 count_shot++;
guesta 10:4fe47396eb5a 161 if(count_shot == 1) {
guesta 10:4fe47396eb5a 162 scrp.send1(TapeLed_num,voice,5);
guesta 8:e76d6c16d7af 163 }
guesta 10:4fe47396eb5a 164 } else {
guesta 8:e76d6c16d7af 165 count_shot = 0;
guesta 8:e76d6c16d7af 166 }
guesta 7:59be95fceafa 167 }
guesta 10:4fe47396eb5a 168
guesta 6:fd9b59f828e8 169 //農作物回収・設置
guesta 10:4fe47396eb5a 170 if(sb.up() == 1) {
guesta 10:4fe47396eb5a 171 count_up++;
guesta 10:4fe47396eb5a 172 if(count_up == 1) {
guesta 10:4fe47396eb5a 173 scrp.send1(TapeLed_num,voice,1);
guesta 8:e76d6c16d7af 174 }
guesta 10:4fe47396eb5a 175 } else {
guesta 10:4fe47396eb5a 176 count_up = 0;
guesta 10:4fe47396eb5a 177 }
guesta 10:4fe47396eb5a 178 if(sb.down() == 1) {
guesta 10:4fe47396eb5a 179 count_down++;
guesta 10:4fe47396eb5a 180 if(count_down == 1) {
guesta 10:4fe47396eb5a 181 scrp.send1(TapeLed_num,voice,2);
guesta 8:e76d6c16d7af 182 }
guesta 10:4fe47396eb5a 183 } else {
guesta 10:4fe47396eb5a 184 count_down = 0;
guesta 10:4fe47396eb5a 185 }
guesta 8:e76d6c16d7af 186 scrp.send1(TakoGetPut_num,up,sb.up());
guesta 8:e76d6c16d7af 187 wait_ms(2);
guesta 8:e76d6c16d7af 188 scrp.send1(TakoGetPut_num,down,sb.down());
guesta 8:e76d6c16d7af 189 wait_ms(2);
guesta 10:4fe47396eb5a 190
guesta 10:4fe47396eb5a 191 //テープLED
guesta 10:4fe47396eb5a 192 if(sb.start() == 1) {
guesta 10:4fe47396eb5a 193 count_auto++;
guesta 10:4fe47396eb5a 194 if(count_start == 1) {
guesta 10:4fe47396eb5a 195 status_start =! status_start;
guesta 10:4fe47396eb5a 196 }
guesta 10:4fe47396eb5a 197 } else {
guesta 10:4fe47396eb5a 198 count_start = 0;
guesta 8:e76d6c16d7af 199 }
guesta 10:4fe47396eb5a 200 scrp.send1(TapeLed_num,start,status_start);
guesta 8:e76d6c16d7af 201 wait_ms(2);
guesta 10:4fe47396eb5a 202
guesta 6:fd9b59f828e8 203 //全体送信
guesta 7:59be95fceafa 204 scrp.send1(send_all,select,auto_mode);//自動・手動モード切り替え
guesta 8:e76d6c16d7af 205 wait_ms(2);
guesta 10:4fe47396eb5a 206 if(status_stop.read() == 1) {
guesta 10:4fe47396eb5a 207 count_stop++;
guesta 10:4fe47396eb5a 208 if(count_stop <= 100000){
guesta 10:4fe47396eb5a 209 scrp.send1(send_all,stop_all,1);
guesta 10:4fe47396eb5a 210 }
guesta 10:4fe47396eb5a 211 } else {
guesta 10:4fe47396eb5a 212 scrp.send1(send_all,stop_all,0);
guesta 10:4fe47396eb5a 213 count_stop = 0;
guesta 10:4fe47396eb5a 214 }
guesta 10:4fe47396eb5a 215 scrp.send1(send_all,stop_all,0);
guesta 10:4fe47396eb5a 216 wait_ms(2);
guesta 1:642ee25ea489 217 }
guesta 0:d8faed605a5c 218 }