2022_gorobo_Ateam / Mbed 2 deprecated MAIN

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

Dependencies:   mbed SBDBT arrc_mbed

main.cpp@12:86b5327a79c4, 2022-04-08 (annotated)

Committer:
guesta
Date:
Fri Apr 08 07:49:43 2022 +0000
Revision:
12:86b5327a79c4
Parent:
11:9ae7fe40a807 
n;

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 12:86b5327a79c4 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 12:86b5327a79c4 35 const int right_cmd = 11;
guesta 10:4fe47396eb5a 36
guesta 6:fd9b59f828e8 37 //ビーンバッグ回収用
guesta 12:86b5327a79c4 38 //const int move = 1;
guesta 10:4fe47396eb5a 39
guesta 6:fd9b59f828e8 40 //ビーンバッグ発射用
guesta 8:e76d6c16d7af 41 const int shot = 2;
guesta 10:4fe47396eb5a 42
guesta 6:fd9b59f828e8 43 //農作物回収・設置用
guesta 6:fd9b59f828e8 44 const int up = 1;
guesta 6:fd9b59f828e8 45 const int down = 2;
guesta 10:4fe47396eb5a 46 /*const int limit1 = 3;
guesta 8:e76d6c16d7af 47 const int limit2 = 4;
guesta 10:4fe47396eb5a 48 const int limit3 = 5;
guesta 10:4fe47396eb5a 49 const int limit4 = 6;*/
guesta 10:4fe47396eb5a 50
guesta 6:fd9b59f828e8 51 //テープLED用
guesta 10:4fe47396eb5a 52 const int voice = 1;
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 12:86b5327a79c4 58 const int stop_all = 51;//全プログラム停止,リセット機能
guesta 10:4fe47396eb5a 59
guesta 1:642ee25ea489 60 //変数
guesta 7:59be95fceafa 61 bool auto_mode = false;
guesta 12:86b5327a79c4 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 12:86b5327a79c4 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 12:86b5327a79c4 120 wait_ms(5);
guesta 10:4fe47396eb5a 121 scrp.send1(wheel_num,r_y_component_cmd,r_y_component);
guesta 12:86b5327a79c4 122 wait_ms(5);
guesta 3:e4b5563563af 123 scrp.send1(wheel_num,l2_cmd,l2_num);
guesta 12:86b5327a79c4 124 wait_ms(5);
guesta 3:e4b5563563af 125 scrp.send1(wheel_num,r2_cmd,r2_num);
guesta 12:86b5327a79c4 126 wait_ms(5);
guesta 7:59be95fceafa 127 scrp.send1(wheel_num,l1_cmd,sb.l1());
guesta 12:86b5327a79c4 128 wait_ms(5);
guesta 10:4fe47396eb5a 129 scrp.send1(wheel_num,r1_cmd,sb.r1());
guesta 12:86b5327a79c4 130 wait_ms(5);
guesta 10:4fe47396eb5a 131 scrp.send1(wheel_num,l_x_component_cmd,l_x_component);
guesta 12:86b5327a79c4 132 wait_ms(5);
guesta 10:4fe47396eb5a 133 scrp.send1(wheel_num,l_y_component_cmd,l_y_component);
guesta 12:86b5327a79c4 134 wait_ms(5);
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);
guesta 12:86b5327a79c4 141 wait_ms(5);
guesta 10:4fe47396eb5a 142 }
guesta 10:4fe47396eb5a 143 } else {
guesta 10:4fe47396eb5a 144 count_circle = 0;
guesta 12:86b5327a79c4 145 scrp.send1(wheel_num,right_cmd,sb.right());
guesta 12:86b5327a79c4 146 wait_ms(5);
guesta 10:4fe47396eb5a 147 }
guesta 10:4fe47396eb5a 148 scrp.send1(wheel_num,circle_cmd,auto_move);
guesta 12:86b5327a79c4 149 wait_ms(5);
guesta 10:4fe47396eb5a 150 }
gennnisi 11:9ae7fe40a807 151
gennnisi 11:9ae7fe40a807 152 if(sw1.read() == 1){
gennnisi 11:9ae7fe40a807 153 field = 0;
gennnisi 11:9ae7fe40a807 154 }else{
gennnisi 11:9ae7fe40a807 155 field = 1;
gennnisi 11:9ae7fe40a807 156 }
gennnisi 11:9ae7fe40a807 157 scrp.send1(wheel_num,switch_cmd,field);
guesta 12:86b5327a79c4 158 wait_ms(5);
guesta 10:4fe47396eb5a 159
guesta 10:4fe47396eb5a 160 //ビーンバッグ回収
guesta 10:4fe47396eb5a 161 if(auto_mode == false) {
guesta 12:86b5327a79c4 162 scrp.send1(TapeLed_num,1,sb.left());
guesta 12:86b5327a79c4 163 wait_ms(5);
guesta 10:4fe47396eb5a 164 }
guesta 10:4fe47396eb5a 165
guesta 6:fd9b59f828e8 166 //ビーンバッグ発射
guesta 10:4fe47396eb5a 167 if(auto_mode == true) {
guesta 8:e76d6c16d7af 168 scrp.send1(BeanbagShot_num,shot,sb.square());
guesta 12:86b5327a79c4 169 wait_ms(5);
guesta 10:4fe47396eb5a 170 if(sb.square() == 1) {
guesta 8:e76d6c16d7af 171 count_shot++;
guesta 10:4fe47396eb5a 172 if(count_shot == 1) {
guesta 10:4fe47396eb5a 173 scrp.send1(TapeLed_num,voice,5);
guesta 8:e76d6c16d7af 174 }
guesta 10:4fe47396eb5a 175 } else {
guesta 8:e76d6c16d7af 176 count_shot = 0;
guesta 8:e76d6c16d7af 177 }
guesta 7:59be95fceafa 178 }
guesta 12:86b5327a79c4 179 //装填
guesta 12:86b5327a79c4 180 if(auto_mode == false) {
guesta 12:86b5327a79c4 181 scrp.send1(BeanbagShot_num,10,sb.cross());
guesta 12:86b5327a79c4 182 wait_ms(5);
guesta 12:86b5327a79c4 183 }
guesta 12:86b5327a79c4 184 if(auto_mode == true){
guesta 12:86b5327a79c4 185 if(sb.cross() == 1){
guesta 12:86b5327a79c4 186 scrp.send1(TapeLed_num,1,10);
guesta 12:86b5327a79c4 187 wait_ms(5);
guesta 12:86b5327a79c4 188 }
guesta 12:86b5327a79c4 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 12:86b5327a79c4 209 wait_ms(5);
guesta 8:e76d6c16d7af 210 scrp.send1(TakoGetPut_num,down,sb.down());
guesta 12:86b5327a79c4 211 wait_ms(5);
guesta 10:4fe47396eb5a 212
guesta 10:4fe47396eb5a 213 //テープLED
guesta 10:4fe47396eb5a 214
guesta 6:fd9b59f828e8 215 //全体送信
guesta 7:59be95fceafa 216 scrp.send1(send_all,select,auto_mode);//自動・手動モード切り替え
guesta 12:86b5327a79c4 217 wait_ms(5);
guesta 10:4fe47396eb5a 218 if(status_stop.read() == 1) {
guesta 10:4fe47396eb5a 219 count_stop++;
guesta 10:4fe47396eb5a 220 if(count_stop <= 100000){
guesta 10:4fe47396eb5a 221 scrp.send1(send_all,stop_all,1);
guesta 10:4fe47396eb5a 222 }
guesta 10:4fe47396eb5a 223 } else {
guesta 10:4fe47396eb5a 224 scrp.send1(send_all,stop_all,0);
guesta 10:4fe47396eb5a 225 count_stop = 0;
guesta 10:4fe47396eb5a 226 }
guesta 10:4fe47396eb5a 227 scrp.send1(send_all,stop_all,0);
guesta 12:86b5327a79c4 228 wait_ms(5);
guesta 12:86b5327a79c4 229 if(sb.start() == 1){
guesta 12:86b5327a79c4 230 scrp.send1(send_all,stop_all,3);
guesta 12:86b5327a79c4 231 wait_ms(5);
guesta 12:86b5327a79c4 232 NVIC_SystemReset();
guesta 12:86b5327a79c4 233 }
guesta 1:642ee25ea489 234 }
guesta 0:d8faed605a5c 235 }