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?
User | Revision | Line number | New 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 | } |