2022_gorobo_Ateam / Mbed 2 deprecated beenbag_shoot

program to shoot beenbags

Dependencies:   mbed MOTOR

arrcmbedをインクルードしてください。

main.cpp@21:9f917ee199ff, 2022-02-01 (annotated)

Committer:
gennnisi
Date:
Tue Feb 01 12:21:48 2022 +0000
Revision:
21:9f917ee199ff
Parent:
20:4db5f042a45c 
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gennnisi 21:9f917ee199ff 1 #include "mbed.h"
gennnisi 0:0128d59ac729 2 #include "rotary_inc.hpp"
gennnisi 0:0128d59ac729 3 #include "scrp_slave.hpp"
gennnisi 0:0128d59ac729 4 #include "Motor.hpp"
gennnisi 0:0128d59ac729 5
gennnisi 19:fbb3b5331641 6 //-----インスタンス--------------------------
gennnisi 19:fbb3b5331641 7
gennnisi 20:4db5f042a45c 8 Motor motor_revolver(PA_7,PA_8);//リボルバーモーター
gennnisi 20:4db5f042a45c 9 Motor motor_shoot(PA_1,PA_3);//発射機構モーター
gennnisi 20:4db5f042a45c 10 PwmOut servo(PB_6);//ロック解除用サーボ
gennnisi 9:6008a52fb6ef 11
gennnisi 18:c61ba21bbc67 12 ScrpSlave slave(PA_9,PA_10,PA_12,SERIAL_TX,SERIAL_RX,3);//id=3
gennnisi 0:0128d59ac729 13
gennnisi 18:c61ba21bbc67 14 RotaryInc v(PA_0,PA_4,1,1024,2);//リボルバーロリコン
gennnisi 0:0128d59ac729 15
gennnisi 18:c61ba21bbc67 16 DigitalIn limit_1(PB_0);//リミットスイッチ１
gennnisi 18:c61ba21bbc67 17 DigitalIn limit_2(PB_1);//リミットスイッチ２
gennnisi 6:7dcc827bbe96 18
gennnisi 19:fbb3b5331641 19 //-----変数---------------------------------
gennnisi 19:fbb3b5331641 20
gennnisi 17:9ee4c69d2d8b 21 int shoot_phase = 0;//発射フェーズ 0:待機　1:装填動作　2:リボルバー回転、モーター復帰（同時）　3:発射　
gennnisi 21:9f917ee199ff 22 int shoot_phase_manual = 0;//発射フェーズ 0:待機　1:装填動作　2:リボルバー回転　3;モーター復帰　4:発射　
gennnisi 18:c61ba21bbc67 23 bool turn_able = 0;//回転可否
gennnisi 18:c61ba21bbc67 24 bool back_able = 0;//復帰可否　
gennnisi 0:0128d59ac729 25
gennnisi 21:9f917ee199ff 26 bool shoot_auto = 1;//手動発射か自動発射か　0:手動　1:自動
gennnisi 19:fbb3b5331641 27 bool mode = 0;//0:手動　1:自動
gennnisi 21:9f917ee199ff 28
gennnisi 19:fbb3b5331641 29 bool emergency = 0;//0:停止　1:停止
gennnisi 20:4db5f042a45c 30 double revolver_pwm = -0.1;//リボルバー回転速度
gennnisi 20:4db5f042a45c 31 double load_pwm = -0.5;//装填モーター速度
gennnisi 20:4db5f042a45c 32 double back_pwm = 0.5;//復帰モーター速度
gennnisi 9:6008a52fb6ef 33
gennnisi 12:aef2a6626ea5 34 int theta_0 = 1290;//0度
gennnisi 12:aef2a6626ea5 35 int theta_45 = 1875;//45度
gennnisi 9:6008a52fb6ef 36 int roricon = 0;//ロリコン
gennnisi 0:0128d59ac729 37
gennnisi 19:fbb3b5331641 38 //-----関数---------------------------------
gennnisi 19:fbb3b5331641 39
gennnisi 21:9f917ee199ff 40 //自動発射開始を受信
gennnisi 21:9f917ee199ff 41 bool get_all(int rx_data,int &tx_data){
gennnisi 10:5e3c789e07dc 42 if(shoot_phase == 0){//フェーズ0以外は、発射できないように
gennnisi 21:9f917ee199ff 43 shoot_auto = 1;
gennnisi 18:c61ba21bbc67 44 shoot_phase = 1;
gennnisi 0:0128d59ac729 45 }
gennnisi 0:0128d59ac729 46 return true;
gennnisi 0:0128d59ac729 47 }
gennnisi 0:0128d59ac729 48
gennnisi 21:9f917ee199ff 49 //装填動作受信
gennnisi 21:9f917ee199ff 50 bool get_load(int rx_data,int &tx_data){
gennnisi 21:9f917ee199ff 51 shoot_auto = 0;
gennnisi 21:9f917ee199ff 52 shoot_phase_manual = 1;
gennnisi 21:9f917ee199ff 53 return true;
gennnisi 21:9f917ee199ff 54 }
gennnisi 21:9f917ee199ff 55
gennnisi 21:9f917ee199ff 56 //動作リボルバー受信
gennnisi 21:9f917ee199ff 57 bool get_revolver(int rx_data,int &tx_data){
gennnisi 21:9f917ee199ff 58 shoot_auto = 0;
gennnisi 21:9f917ee199ff 59 shoot_phase_manual = 2;
gennnisi 21:9f917ee199ff 60 return true;
gennnisi 21:9f917ee199ff 61 }
gennnisi 21:9f917ee199ff 62
gennnisi 21:9f917ee199ff 63 //復帰動作受信
gennnisi 21:9f917ee199ff 64 bool get_back(int rx_data,int &tx_data){
gennnisi 21:9f917ee199ff 65 shoot_auto = 0;
gennnisi 21:9f917ee199ff 66 shoot_phase_manual = 3;
gennnisi 21:9f917ee199ff 67 return true;
gennnisi 21:9f917ee199ff 68 }
gennnisi 21:9f917ee199ff 69
gennnisi 21:9f917ee199ff 70 //発射動作受信
gennnisi 21:9f917ee199ff 71 bool get_shoot(int rx_data,int &tx_data){
gennnisi 21:9f917ee199ff 72 shoot_phase_manual = 4;
gennnisi 21:9f917ee199ff 73 shoot_auto = 0;
gennnisi 21:9f917ee199ff 74 return true;
gennnisi 21:9f917ee199ff 75 }
gennnisi 21:9f917ee199ff 76
gennnisi 19:fbb3b5331641 77 //モード変更を受信
gennnisi 19:fbb3b5331641 78 bool get_changemode(int rx_data,int &tx_data){
gennnisi 19:fbb3b5331641 79 if(mode == 0){
gennnisi 19:fbb3b5331641 80 mode = 1;
gennnisi 19:fbb3b5331641 81 }else{
gennnisi 19:fbb3b5331641 82 mode = 0;
gennnisi 19:fbb3b5331641 83 }
gennnisi 20:4db5f042a45c 84 return 0;
gennnisi 19:fbb3b5331641 85 }
gennnisi 19:fbb3b5331641 86
gennnisi 19:fbb3b5331641 87 //非常停止を受信
gennnisi 19:fbb3b5331641 88 bool get_stop(int rx_data,int &tx_data){
gennnisi 19:fbb3b5331641 89 if(emergency == 0){
gennnisi 19:fbb3b5331641 90 emergency = 1;//停止
gennnisi 20:4db5f042a45c 91 shoot_phase = 10;
gennnisi 19:fbb3b5331641 92 }else{
gennnisi 19:fbb3b5331641 93 emergency = 0;//解除
gennnisi 19:fbb3b5331641 94 }
gennnisi 20:4db5f042a45c 95 return 0;
gennnisi 19:fbb3b5331641 96 }
gennnisi 19:fbb3b5331641 97
gennnisi 19:fbb3b5331641 98 //リボルバー、ラック復帰
gennnisi 13:66e37e0aa786 99 void revolver_back(int count){
gennnisi 16:04f42eeb9561 100 while(turn_able == 1 || back_able == 1){
gennnisi 16:04f42eeb9561 101 //リボルバー
gennnisi 16:04f42eeb9561 102 if(count == 1){
gennnisi 16:04f42eeb9561 103 motor_revolver.output(0);
gennnisi 16:04f42eeb9561 104 turn_able = 0;
gennnisi 17:9ee4c69d2d8b 105 printf("no turn\n");
gennnisi 16:04f42eeb9561 106 }
gennnisi 18:c61ba21bbc67 107 if(count > 1){
gennnisi 16:04f42eeb9561 108 if(roricon < 341 * (count - 1)){
gennnisi 16:04f42eeb9561 109 if(turn_able == 1){
gennnisi 14:81f09ab5ed23 110 roricon = v.get();
gennnisi 14:81f09ab5ed23 111 motor_revolver.output(revolver_pwm);
gennnisi 16:04f42eeb9561 112 printf("roricon = %d\n",roricon);
gennnisi 14:81f09ab5ed23 113 }
gennnisi 13:66e37e0aa786 114 }
gennnisi 16:04f42eeb9561 115 else{
gennnisi 16:04f42eeb9561 116 motor_revolver.output(0);
gennnisi 16:04f42eeb9561 117 turn_able = 0;
gennnisi 16:04f42eeb9561 118 }
gennnisi 16:04f42eeb9561 119 }
gennnisi 16:04f42eeb9561 120 //復帰
gennnisi 16:04f42eeb9561 121 if(limit_2.read() == 0){
gennnisi 16:04f42eeb9561 122 if(back_able == 1){
gennnisi 13:66e37e0aa786 123 motor_shoot.output(back_pwm);
gennnisi 16:04f42eeb9561 124 printf("backing\n");
gennnisi 13:66e37e0aa786 125 }
gennnisi 16:04f42eeb9561 126 }
gennnisi 16:04f42eeb9561 127 if(limit_2.read() == 1){
gennnisi 16:04f42eeb9561 128 motor_shoot.output(0);
gennnisi 16:04f42eeb9561 129 back_able = 0;
gennnisi 16:04f42eeb9561 130 printf("finished backing\n");
gennnisi 9:6008a52fb6ef 131 }
gennnisi 16:04f42eeb9561 132 //ループ抜け出し
gennnisi 18:c61ba21bbc67 133 if(turn_able == 0 && back_able == 0){
gennnisi 16:04f42eeb9561 134 break;
gennnisi 16:04f42eeb9561 135 }
gennnisi 20:4db5f042a45c 136 wait(0.01);
gennnisi 9:6008a52fb6ef 137 }
gennnisi 9:6008a52fb6ef 138 }
gennnisi 9:6008a52fb6ef 139
gennnisi 21:9f917ee199ff 140 //リボルバー回転のみ
gennnisi 21:9f917ee199ff 141 void revolver(int count){
gennnisi 21:9f917ee199ff 142 if(turn_able == 1){
gennnisi 21:9f917ee199ff 143 while(roricon < 341 * count){
gennnisi 21:9f917ee199ff 144 //while(1){
gennnisi 21:9f917ee199ff 145 roricon = v.get();
gennnisi 21:9f917ee199ff 146 motor_revolver.output(revolver_pwm);
gennnisi 21:9f917ee199ff 147 printf("roricon = %d\n",roricon);
gennnisi 21:9f917ee199ff 148 wait(0.01);
gennnisi 21:9f917ee199ff 149 }
gennnisi 21:9f917ee199ff 150 turn_able = 0;
gennnisi 21:9f917ee199ff 151 }
gennnisi 21:9f917ee199ff 152 }
gennnisi 21:9f917ee199ff 153
gennnisi 19:fbb3b5331641 154 //------------------------------------------
gennnisi 19:fbb3b5331641 155
gennnisi 9:6008a52fb6ef 156 int main(){
gennnisi 9:6008a52fb6ef 157 int count = 1;//何発目か
gennnisi 9:6008a52fb6ef 158
gennnisi 9:6008a52fb6ef 159 servo.period_us(20000);
gennnisi 10:5e3c789e07dc 160 limit_1.mode(PullUp);
gennnisi 10:5e3c789e07dc 161 limit_2.mode(PullUp);
gennnisi 9:6008a52fb6ef 162
gennnisi 21:9f917ee199ff 163 slave.addCMD(2,get_all);
gennnisi 21:9f917ee199ff 164 slave.addCMD(3,get_load);
gennnisi 21:9f917ee199ff 165 slave.addCMD(4,get_revolver);
gennnisi 21:9f917ee199ff 166 slave.addCMD(5,get_back);
gennnisi 21:9f917ee199ff 167 slave.addCMD(6,get_shoot);
gennnisi 21:9f917ee199ff 168
gennnisi 19:fbb3b5331641 169 slave.addCMD(50,get_changemode);
gennnisi 20:4db5f042a45c 170 slave.addCMD(51,get_stop);
gennnisi 12:aef2a6626ea5 171
gennnisi 9:6008a52fb6ef 172 while(1){
gennnisi 21:9f917ee199ff 173 //----------自動発射------------------------------
gennnisi 21:9f917ee199ff 174 if(mode == 1 && shoot_auto == 1){
gennnisi 19:fbb3b5331641 175 switch (shoot_phase){
gennnisi 19:fbb3b5331641 176 case 0://待機
gennnisi 19:fbb3b5331641 177 while(shoot_phase == 0){
gennnisi 19:fbb3b5331641 178 wait(0.1);
gennnisi 19:fbb3b5331641 179 printf("waiting\n");
gennnisi 15:7c26b01dcefd 180 }
gennnisi 19:fbb3b5331641 181 shoot_phase = 1;
gennnisi 19:fbb3b5331641 182 printf("finished waiting\n");
gennnisi 19:fbb3b5331641 183 break;
gennnisi 19:fbb3b5331641 184
gennnisi 19:fbb3b5331641 185 case 1://装填動作
gennnisi 19:fbb3b5331641 186 while(limit_1.read() == 0){
gennnisi 19:fbb3b5331641 187 motor_shoot.output(load_pwm);
gennnisi 19:fbb3b5331641 188 printf("loading\n");
gennnisi 19:fbb3b5331641 189 if(limit_2.read() == 1){
gennnisi 19:fbb3b5331641 190 break;
gennnisi 19:fbb3b5331641 191 }
gennnisi 19:fbb3b5331641 192 wait(0.1);
gennnisi 19:fbb3b5331641 193 }
gennnisi 19:fbb3b5331641 194 motor_shoot.output(0);
gennnisi 19:fbb3b5331641 195 shoot_phase = 2;
gennnisi 19:fbb3b5331641 196 printf("finished loading\n");
gennnisi 19:fbb3b5331641 197 wait(3);
gennnisi 19:fbb3b5331641 198 break;
gennnisi 19:fbb3b5331641 199
gennnisi 19:fbb3b5331641 200 case 2://リボルバー回転,モーター復帰
gennnisi 19:fbb3b5331641 201 turn_able = 1;
gennnisi 19:fbb3b5331641 202 back_able = 1;
gennnisi 19:fbb3b5331641 203 revolver_back(count);
gennnisi 19:fbb3b5331641 204 shoot_phase = 3;
gennnisi 19:fbb3b5331641 205 printf("finished backing\n");
gennnisi 19:fbb3b5331641 206 wait(3);
gennnisi 19:fbb3b5331641 207 break;
gennnisi 19:fbb3b5331641 208
gennnisi 19:fbb3b5331641 209 case 3://発射動作
gennnisi 19:fbb3b5331641 210 servo.pulsewidth_us(theta_45);
gennnisi 19:fbb3b5331641 211 printf("%d shot\n",count);
gennnisi 19:fbb3b5331641 212 wait(3);
gennnisi 20:4db5f042a45c 213 servo.pulsewidth_us(theta_0);
gennnisi 19:fbb3b5331641 214 motor_shoot.output(load_pwm);
gennnisi 19:fbb3b5331641 215 wait(0.15);
gennnisi 19:fbb3b5331641 216 motor_shoot.output(0);
gennnisi 19:fbb3b5331641 217 shoot_phase = 0;
gennnisi 19:fbb3b5331641 218 printf("finished init\n");
gennnisi 19:fbb3b5331641 219 count ++;
gennnisi 19:fbb3b5331641 220 break;
gennnisi 19:fbb3b5331641 221
gennnisi 20:4db5f042a45c 222 case 10://非常停止
gennnisi 19:fbb3b5331641 223 while(emergency == 1){
gennnisi 19:fbb3b5331641 224 motor_shoot.output(0);
gennnisi 19:fbb3b5331641 225 motor_revolver.output(0);
gennnisi 20:4db5f042a45c 226 printf("emergency\n");
gennnisi 19:fbb3b5331641 227 wait(0.1);
gennnisi 19:fbb3b5331641 228 }
gennnisi 19:fbb3b5331641 229 break;
gennnisi 19:fbb3b5331641 230 }
gennnisi 11:62c85df03747 231 }
gennnisi 21:9f917ee199ff 232
gennnisi 21:9f917ee199ff 233 //----------手動発射------------------------------
gennnisi 21:9f917ee199ff 234 if(mode == 1 && shoot_auto == 0){
gennnisi 21:9f917ee199ff 235 switch (shoot_phase_manual){
gennnisi 21:9f917ee199ff 236 case 0://待機
gennnisi 21:9f917ee199ff 237 while(shoot_phase_manual == 0){
gennnisi 21:9f917ee199ff 238 wait(0.1);
gennnisi 21:9f917ee199ff 239 printf("waiting\n");
gennnisi 21:9f917ee199ff 240 }
gennnisi 21:9f917ee199ff 241 printf("finished waiting\n");
gennnisi 21:9f917ee199ff 242 break;
gennnisi 21:9f917ee199ff 243
gennnisi 21:9f917ee199ff 244 case 1://装填動作
gennnisi 21:9f917ee199ff 245 while(limit_1.read() == 0){
gennnisi 21:9f917ee199ff 246 motor_shoot.output(load_pwm);
gennnisi 21:9f917ee199ff 247 printf("loading\n");
gennnisi 21:9f917ee199ff 248 wait(0.1);
gennnisi 21:9f917ee199ff 249 if(limit_2.read() == 1){
gennnisi 21:9f917ee199ff 250 break;
gennnisi 21:9f917ee199ff 251 }
gennnisi 21:9f917ee199ff 252 }
gennnisi 21:9f917ee199ff 253 motor_shoot.output(0);
gennnisi 21:9f917ee199ff 254 shoot_phase_manual = 0;
gennnisi 21:9f917ee199ff 255 printf("finished loading\n");
gennnisi 21:9f917ee199ff 256 wait(5);
gennnisi 21:9f917ee199ff 257 break;
gennnisi 21:9f917ee199ff 258
gennnisi 21:9f917ee199ff 259 case 2://リボルバー回転
gennnisi 21:9f917ee199ff 260 turn_able = 1;
gennnisi 21:9f917ee199ff 261 revolver(count);
gennnisi 21:9f917ee199ff 262 if(turn_able == 0){
gennnisi 21:9f917ee199ff 263 motor_revolver.output(0);
gennnisi 21:9f917ee199ff 264 shoot_phase_manual = 0;
gennnisi 21:9f917ee199ff 265 }
gennnisi 21:9f917ee199ff 266 else{
gennnisi 21:9f917ee199ff 267 printf("error\n");
gennnisi 21:9f917ee199ff 268 }
gennnisi 21:9f917ee199ff 269 count ++;
gennnisi 21:9f917ee199ff 270 printf("finished turnning\n");
gennnisi 21:9f917ee199ff 271 break;
gennnisi 21:9f917ee199ff 272
gennnisi 21:9f917ee199ff 273 case 3://モーター復帰
gennnisi 21:9f917ee199ff 274 while(limit_2.read() == 0){
gennnisi 21:9f917ee199ff 275 motor_shoot.output(back_pwm);
gennnisi 21:9f917ee199ff 276 printf("backing\n");
gennnisi 21:9f917ee199ff 277 wait(0.1);
gennnisi 21:9f917ee199ff 278 }
gennnisi 21:9f917ee199ff 279
gennnisi 21:9f917ee199ff 280 motor_shoot.output(0);
gennnisi 21:9f917ee199ff 281 shoot_phase_manual = 0;
gennnisi 21:9f917ee199ff 282 printf("finished backing\n");
gennnisi 21:9f917ee199ff 283 break;
gennnisi 21:9f917ee199ff 284
gennnisi 21:9f917ee199ff 285 case 4://発射動作
gennnisi 21:9f917ee199ff 286 servo.pulsewidth_us(theta_45);
gennnisi 21:9f917ee199ff 287 printf("finished shooting %d shot\n",count);
gennnisi 21:9f917ee199ff 288 wait(5);
gennnisi 21:9f917ee199ff 289 servo.pulsewidth_us(theta_0);
gennnisi 21:9f917ee199ff 290 motor_shoot.output(load_pwm);
gennnisi 21:9f917ee199ff 291 wait(0.15);
gennnisi 21:9f917ee199ff 292 motor_shoot.output(0);
gennnisi 21:9f917ee199ff 293 shoot_phase_manual = 0;
gennnisi 21:9f917ee199ff 294 printf("finished init\n");
gennnisi 21:9f917ee199ff 295 break;
gennnisi 21:9f917ee199ff 296 }
gennnisi 21:9f917ee199ff 297 }
gennnisi 9:6008a52fb6ef 298 }
gennnisi 12:aef2a6626ea5 299 }