![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
メインプログラム最新版です
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?
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; |
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 | } |