2022_gorobo_Ateam / Mbed 2 deprecated MAINPROGRAM

4/2にちゃんと動いたやつ

Dependencies:   mbed SBDBT arrc_mbed

main.cpp@12:e96b0757577d, 2022-04-02 (annotated)

Committer:
gennnisi
Date:
Sat Apr 02 06:44:55 2022 +0000
Revision:
12:e96b0757577d
Parent:
11:9ae7fe40a807 
4/2

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;
gennnisi 11:9ae7fe40a807 64 bool field = 0;
guesta 10:4fe47396eb5a 65 int r_x_component;
guesta 10:4fe47396eb5a 66 int r_y_component;
guesta 10:4fe47396eb5a 67 int l_x_component;
guesta 10:4fe47396eb5a 68 int l_y_component;
guesta 3:e4b5563563af 69 int l2_num;
guesta 3:e4b5563563af 70 int r2_num;
guesta 10:4fe47396eb5a 71
guesta 10:4fe47396eb5a 72 DigitalIn status_stop(PB_14,PullUp);
guesta 10:4fe47396eb5a 73 DigitalIn sw1(PB_6);
guesta 10:4fe47396eb5a 74 sw1.mode(PullUp);
guesta 8:e76d6c16d7af 75
guesta 10:4fe47396eb5a 76 DigitalOut led(PA_10);
guesta 8:e76d6c16d7af 77
guesta 10:4fe47396eb5a 78 //RotaryInc rot1(PC_0,PC_1,M_PI*2,40*M_PI,4);
guesta 10:4fe47396eb5a 79 //RotaryInc rot2(PC_3,PC_2,M_PI*2,40*M_PI,4);
guesta 10:4fe47396eb5a 80
guesta 8:e76d6c16d7af 81 //count
guesta 8:e76d6c16d7af 82 int count_up;
guesta 8:e76d6c16d7af 83 int count_down;
guesta 8:e76d6c16d7af 84 int count_shot;
guesta 8:e76d6c16d7af 85 int count_auto;
guesta 10:4fe47396eb5a 86 int count_start;
guesta 10:4fe47396eb5a 87 int count_stop;
guesta 10:4fe47396eb5a 88 int count_circle;
guesta 3:e4b5563563af 89 //-------------------------------------
guesta 6:fd9b59f828e8 90
guesta 10:4fe47396eb5a 91 while(true) {
guesta 8:e76d6c16d7af 92 printf("%d\n",sw1.read());
guesta 3:e4b5563563af 93 sb.button_state();
guesta 10:4fe47396eb5a 94 r_x_component = sb.rsx();
guesta 10:4fe47396eb5a 95 r_y_component = sb.rsy();
guesta 10:4fe47396eb5a 96 l_x_component = sb.lsx();
guesta 10:4fe47396eb5a 97 l_y_component = sb.lsy();
guesta 3:e4b5563563af 98 l2_num = sb.l2An();
guesta 3:e4b5563563af 99 r2_num = sb.r2An();
guesta 10:4fe47396eb5a 100 if(sb.select() == 1) {
guesta 8:e76d6c16d7af 101 count_auto++;
guesta 10:4fe47396eb5a 102 printf("%d\n",count_auto);
guesta 10:4fe47396eb5a 103 if(count_auto == 1) {
guesta 10:4fe47396eb5a 104 auto_mode =! auto_mode;
guesta 10:4fe47396eb5a 105 }
guesta 10:4fe47396eb5a 106 } else {
guesta 10:4fe47396eb5a 107 count_auto = 0;
guesta 10:4fe47396eb5a 108 }
guesta 10:4fe47396eb5a 109 if(auto_mode == 1) {
guesta 10:4fe47396eb5a 110 led = 1;
guesta 10:4fe47396eb5a 111 scrp.send1(TapeLed_num,voice,4);
guesta 10:4fe47396eb5a 112 //printf("auto_mode\n");
guesta 8:e76d6c16d7af 113 }else{
guesta 10:4fe47396eb5a 114 led = 0;
guesta 10:4fe47396eb5a 115 //printf("manual_mode\n");
guesta 7:59be95fceafa 116 }
guesta 10:4fe47396eb5a 117
guesta 6:fd9b59f828e8 118 //足回り
guesta 10:4fe47396eb5a 119 scrp.send1(wheel_num,r_x_component_cmd,r_x_component);
guesta 7:59be95fceafa 120 wait_ms(2);
guesta 10:4fe47396eb5a 121 scrp.send1(wheel_num,r_y_component_cmd,r_y_component);
guesta 7:59be95fceafa 122 wait_ms(2);
guesta 3:e4b5563563af 123 scrp.send1(wheel_num,l2_cmd,l2_num);
guesta 7:59be95fceafa 124 wait_ms(2);
guesta 3:e4b5563563af 125 scrp.send1(wheel_num,r2_cmd,r2_num);
guesta 7:59be95fceafa 126 wait_ms(2);
guesta 7:59be95fceafa 127 scrp.send1(wheel_num,l1_cmd,sb.l1());
guesta 7:59be95fceafa 128 wait_ms(2);
guesta 10:4fe47396eb5a 129 scrp.send1(wheel_num,r1_cmd,sb.r1());
guesta 10:4fe47396eb5a 130 wait_ms(2);
guesta 10:4fe47396eb5a 131 scrp.send1(wheel_num,l_x_component_cmd,l_x_component);
guesta 10:4fe47396eb5a 132 wait_ms(2);
guesta 10:4fe47396eb5a 133 scrp.send1(wheel_num,l_y_component_cmd,l_y_component);
guesta 8:e76d6c16d7af 134 wait_ms(2);
guesta 10:4fe47396eb5a 135 if(auto_mode == true){
guesta 10:4fe47396eb5a 136 if(sb.circle() == 1) {
gennnisi 11:9ae7fe40a807 137 count_circle++;
guesta 10:4fe47396eb5a 138 if(count_circle == 1) {
guesta 10:4fe47396eb5a 139 auto_move =! auto_move;
gennnisi 11:9ae7fe40a807 140 scrp.send1(BeanbagShot_num,circle_cmd,auto_move);
gennnisi 11:9ae7fe40a807 141 wait_ms(2);
guesta 10:4fe47396eb5a 142 }
guesta 10:4fe47396eb5a 143 } else {
guesta 10:4fe47396eb5a 144 count_circle = 0;
guesta 10:4fe47396eb5a 145 }
guesta 10:4fe47396eb5a 146 scrp.send1(wheel_num,circle_cmd,auto_move);
guesta 10:4fe47396eb5a 147 wait_ms(2);
guesta 10:4fe47396eb5a 148 }
gennnisi 11:9ae7fe40a807 149
gennnisi 11:9ae7fe40a807 150 if(sw1.read() == 1){
gennnisi 11:9ae7fe40a807 151 field = 0;
gennnisi 11:9ae7fe40a807 152 }else{
gennnisi 11:9ae7fe40a807 153 field = 1;
gennnisi 11:9ae7fe40a807 154 }
gennnisi 11:9ae7fe40a807 155 scrp.send1(wheel_num,switch_cmd,field);
guesta 10:4fe47396eb5a 156 wait_ms(2);
guesta 10:4fe47396eb5a 157
guesta 10:4fe47396eb5a 158 //ビーンバッグ回収
guesta 10:4fe47396eb5a 159 if(auto_mode == false) {
gennnisi 11:9ae7fe40a807 160 scrp.send1(TapeLed_num,1,sb.square());
guesta 10:4fe47396eb5a 161 wait_ms(2);
guesta 10:4fe47396eb5a 162 }
guesta 10:4fe47396eb5a 163
guesta 6:fd9b59f828e8 164 //ビーンバッグ発射
guesta 10:4fe47396eb5a 165 if(auto_mode == true) {
guesta 8:e76d6c16d7af 166 scrp.send1(BeanbagShot_num,shot,sb.square());
guesta 7:59be95fceafa 167 wait_ms(2);
guesta 10:4fe47396eb5a 168 if(sb.square() == 1) {
guesta 8:e76d6c16d7af 169 count_shot++;
guesta 10:4fe47396eb5a 170 if(count_shot == 1) {
guesta 10:4fe47396eb5a 171 scrp.send1(TapeLed_num,voice,5);
guesta 8:e76d6c16d7af 172 }
guesta 10:4fe47396eb5a 173 } else {
guesta 8:e76d6c16d7af 174 count_shot = 0;
guesta 8:e76d6c16d7af 175 }
guesta 7:59be95fceafa 176 }
gennnisi 12:e96b0757577d 177
gennnisi 12:e96b0757577d 178 //装填
gennnisi 12:e96b0757577d 179 if(auto_mode == false) {
gennnisi 12:e96b0757577d 180 scrp.send1(BeanbagShot_num,10,sb.cross());
gennnisi 12:e96b0757577d 181 wait_ms(2);
gennnisi 12:e96b0757577d 182 }
gennnisi 12:e96b0757577d 183
gennnisi 12:e96b0757577d 184 if(auto_mode == true){
gennnisi 12:e96b0757577d 185 if(sb.cross() == 1){
gennnisi 12:e96b0757577d 186 scrp.send1(TapeLed_num,1,10);
gennnisi 12:e96b0757577d 187 wait_ms(2);
gennnisi 12:e96b0757577d 188 }
gennnisi 12:e96b0757577d 189 }
guesta 10:4fe47396eb5a 190
guesta 6:fd9b59f828e8 191 //農作物回収・設置
guesta 10:4fe47396eb5a 192 if(sb.up() == 1) {
guesta 10:4fe47396eb5a 193 count_up++;
guesta 10:4fe47396eb5a 194 if(count_up == 1) {
gennnisi 11:9ae7fe40a807 195 scrp.send1(TapeLed_num,voice,3);
guesta 8:e76d6c16d7af 196 }
guesta 10:4fe47396eb5a 197 } else {
guesta 10:4fe47396eb5a 198 count_up = 0;
guesta 10:4fe47396eb5a 199 }
guesta 10:4fe47396eb5a 200 if(sb.down() == 1) {
guesta 10:4fe47396eb5a 201 count_down++;
guesta 10:4fe47396eb5a 202 if(count_down == 1) {
guesta 10:4fe47396eb5a 203 scrp.send1(TapeLed_num,voice,2);
guesta 8:e76d6c16d7af 204 }
guesta 10:4fe47396eb5a 205 } else {
guesta 10:4fe47396eb5a 206 count_down = 0;
guesta 10:4fe47396eb5a 207 }
guesta 8:e76d6c16d7af 208 scrp.send1(TakoGetPut_num,up,sb.up());
guesta 8:e76d6c16d7af 209 wait_ms(2);
guesta 8:e76d6c16d7af 210 scrp.send1(TakoGetPut_num,down,sb.down());
guesta 8:e76d6c16d7af 211 wait_ms(2);
guesta 10:4fe47396eb5a 212
guesta 10:4fe47396eb5a 213 //テープLED
guesta 10:4fe47396eb5a 214 if(sb.start() == 1) {
guesta 10:4fe47396eb5a 215 count_auto++;
guesta 10:4fe47396eb5a 216 if(count_start == 1) {
guesta 10:4fe47396eb5a 217 status_start =! status_start;
guesta 10:4fe47396eb5a 218 }
guesta 10:4fe47396eb5a 219 } else {
guesta 10:4fe47396eb5a 220 count_start = 0;
guesta 8:e76d6c16d7af 221 }
guesta 10:4fe47396eb5a 222 scrp.send1(TapeLed_num,start,status_start);
guesta 8:e76d6c16d7af 223 wait_ms(2);
guesta 10:4fe47396eb5a 224
guesta 6:fd9b59f828e8 225 //全体送信
guesta 7:59be95fceafa 226 scrp.send1(send_all,select,auto_mode);//自動・手動モード切り替え
guesta 8:e76d6c16d7af 227 wait_ms(2);
guesta 10:4fe47396eb5a 228 if(status_stop.read() == 1) {
guesta 10:4fe47396eb5a 229 count_stop++;
guesta 10:4fe47396eb5a 230 if(count_stop <= 100000){
guesta 10:4fe47396eb5a 231 scrp.send1(send_all,stop_all,1);
guesta 10:4fe47396eb5a 232 }
guesta 10:4fe47396eb5a 233 } else {
guesta 10:4fe47396eb5a 234 scrp.send1(send_all,stop_all,0);
guesta 10:4fe47396eb5a 235 count_stop = 0;
guesta 10:4fe47396eb5a 236 }
guesta 10:4fe47396eb5a 237 scrp.send1(send_all,stop_all,0);
guesta 10:4fe47396eb5a 238 wait_ms(2);
guesta 1:642ee25ea489 239 }
guesta 0:d8faed605a5c 240 }