2022_gorobo_Ateam / Mbed 2 deprecated beenbag_shoot

program to shoot beenbags

Dependencies:   mbed MOTOR

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

main.cpp@18:c61ba21bbc67, 2022-01-29 (annotated)

Committer:
gennnisi
Date:
Sat Jan 29 15:43:32 2022 +0000
Revision:
18:c61ba21bbc67
Parent:
17:9ee4c69d2d8b
Child:
19:fbb3b5331641
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gennnisi 0:0128d59ac729 1 //発射、リボルバープログラム
gennnisi 0:0128d59ac729 2 #include "mbed.h"
gennnisi 0:0128d59ac729 3 #include "rotary_inc.hpp"
gennnisi 0:0128d59ac729 4 #include "scrp_slave.hpp"
gennnisi 0:0128d59ac729 5 #include "Motor.hpp"
gennnisi 0:0128d59ac729 6
gennnisi 18:c61ba21bbc67 7 Motor motor_revolver(PA_8,PA_7);//リボルバーモーター
gennnisi 18:c61ba21bbc67 8 Motor motor_shoot(PB_6,PA_11);//発射機構モーター
gennnisi 18:c61ba21bbc67 9 PwmOut servo(PA_1);//ロック解除用サーボ
gennnisi 9:6008a52fb6ef 10
gennnisi 18:c61ba21bbc67 11 ScrpSlave slave(PA_9,PA_10,PA_12,SERIAL_TX,SERIAL_RX,3);//id=3
gennnisi 0:0128d59ac729 12
gennnisi 18:c61ba21bbc67 13 RotaryInc v(PA_0,PA_4,1,1024,2);//リボルバーロリコン
gennnisi 0:0128d59ac729 14
gennnisi 18:c61ba21bbc67 15 DigitalIn limit_1(PB_0);//リミットスイッチ１
gennnisi 18:c61ba21bbc67 16 DigitalIn limit_2(PB_1);//リミットスイッチ２
gennnisi 6:7dcc827bbe96 17
gennnisi 17:9ee4c69d2d8b 18 int shoot_phase = 0;//発射フェーズ 0:待機　1:装填動作　2:リボルバー回転、モーター復帰（同時）　3:発射　
gennnisi 18:c61ba21bbc67 19 bool turn_able = 0;//回転可否
gennnisi 18:c61ba21bbc67 20 bool back_able = 0;//復帰可否　
gennnisi 0:0128d59ac729 21
gennnisi 12:aef2a6626ea5 22 double revolver_pwm = -0.03;//リボルバー回転速度
gennnisi 9:6008a52fb6ef 23 double load_pwm = -0.3;//装填モーター速度
gennnisi 9:6008a52fb6ef 24 double back_pwm = 0.3;//復帰モーター速度
gennnisi 9:6008a52fb6ef 25
gennnisi 12:aef2a6626ea5 26 double turn = 0.0;
gennnisi 12:aef2a6626ea5 27
gennnisi 12:aef2a6626ea5 28 int theta_0 = 1290;//0度
gennnisi 12:aef2a6626ea5 29 int theta_45 = 1875;//45度
gennnisi 9:6008a52fb6ef 30 int roricon = 0;//ロリコン
gennnisi 0:0128d59ac729 31
gennnisi 0:0128d59ac729 32 bool interrupt(int rx_data,int &tx_data){
gennnisi 10:5e3c789e07dc 33 if(shoot_phase == 0){//フェーズ0以外は、発射できないように
gennnisi 18:c61ba21bbc67 34 shoot_phase = 1;
gennnisi 0:0128d59ac729 35 }
gennnisi 0:0128d59ac729 36 return true;
gennnisi 0:0128d59ac729 37 }
gennnisi 0:0128d59ac729 38
gennnisi 13:66e37e0aa786 39 void revolver_back(int count){
gennnisi 16:04f42eeb9561 40 while(turn_able == 1 || back_able == 1){
gennnisi 16:04f42eeb9561 41 //リボルバー
gennnisi 16:04f42eeb9561 42 if(count == 1){
gennnisi 16:04f42eeb9561 43 motor_revolver.output(0);
gennnisi 16:04f42eeb9561 44 turn_able = 0;
gennnisi 17:9ee4c69d2d8b 45 printf("no turn\n");
gennnisi 16:04f42eeb9561 46 }
gennnisi 18:c61ba21bbc67 47 if(count > 1){
gennnisi 16:04f42eeb9561 48 if(roricon < 341 * (count - 1)){
gennnisi 16:04f42eeb9561 49 if(turn_able == 1){
gennnisi 14:81f09ab5ed23 50 roricon = v.get();
gennnisi 14:81f09ab5ed23 51 motor_revolver.output(revolver_pwm);
gennnisi 16:04f42eeb9561 52 printf("roricon = %d\n",roricon);
gennnisi 14:81f09ab5ed23 53 }
gennnisi 13:66e37e0aa786 54 }
gennnisi 16:04f42eeb9561 55 else{
gennnisi 16:04f42eeb9561 56 motor_revolver.output(0);
gennnisi 16:04f42eeb9561 57 turn_able = 0;
gennnisi 16:04f42eeb9561 58 }
gennnisi 16:04f42eeb9561 59 }
gennnisi 16:04f42eeb9561 60 //復帰
gennnisi 16:04f42eeb9561 61 if(limit_2.read() == 0){
gennnisi 16:04f42eeb9561 62 if(back_able == 1){
gennnisi 13:66e37e0aa786 63 motor_shoot.output(back_pwm);
gennnisi 16:04f42eeb9561 64 printf("backing\n");
gennnisi 13:66e37e0aa786 65 }
gennnisi 16:04f42eeb9561 66 }
gennnisi 16:04f42eeb9561 67 if(limit_2.read() == 1){
gennnisi 16:04f42eeb9561 68 motor_shoot.output(0);
gennnisi 16:04f42eeb9561 69 back_able = 0;
gennnisi 16:04f42eeb9561 70 printf("finished backing\n");
gennnisi 9:6008a52fb6ef 71 }
gennnisi 16:04f42eeb9561 72 //ループ抜け出し
gennnisi 18:c61ba21bbc67 73 if(turn_able == 0 && back_able == 0){
gennnisi 16:04f42eeb9561 74 break;
gennnisi 16:04f42eeb9561 75 }
gennnisi 17:9ee4c69d2d8b 76 wait(0.1);
gennnisi 9:6008a52fb6ef 77 }
gennnisi 9:6008a52fb6ef 78 }
gennnisi 9:6008a52fb6ef 79
gennnisi 9:6008a52fb6ef 80 int main(){
gennnisi 9:6008a52fb6ef 81 int count = 1;//何発目か
gennnisi 9:6008a52fb6ef 82
gennnisi 9:6008a52fb6ef 83 servo.period_us(20000);
gennnisi 10:5e3c789e07dc 84 limit_1.mode(PullUp);
gennnisi 10:5e3c789e07dc 85 limit_2.mode(PullUp);
gennnisi 9:6008a52fb6ef 86
gennnisi 9:6008a52fb6ef 87 slave.addCMD(2,interrupt);
gennnisi 12:aef2a6626ea5 88
gennnisi 9:6008a52fb6ef 89 while(1){
gennnisi 10:5e3c789e07dc 90 switch (shoot_phase){
gennnisi 9:6008a52fb6ef 91
gennnisi 9:6008a52fb6ef 92 case 0://待機
gennnisi 10:5e3c789e07dc 93 while(shoot_phase == 0){
gennnisi 9:6008a52fb6ef 94 wait(0.1);
gennnisi 9:6008a52fb6ef 95 printf("waiting\n");
gennnisi 9:6008a52fb6ef 96 }
gennnisi 10:5e3c789e07dc 97 shoot_phase = 1;
gennnisi 9:6008a52fb6ef 98 printf("finished waiting\n");
gennnisi 9:6008a52fb6ef 99 break;
gennnisi 9:6008a52fb6ef 100
gennnisi 9:6008a52fb6ef 101 case 1://装填動作
gennnisi 9:6008a52fb6ef 102 while(limit_1.read() == 0){
gennnisi 10:5e3c789e07dc 103 motor_shoot.output(load_pwm);
gennnisi 11:62c85df03747 104 printf("loading\n");
gennnisi 15:7c26b01dcefd 105 if(limit_2.read() == 1){
gennnisi 15:7c26b01dcefd 106 break;
gennnisi 15:7c26b01dcefd 107 }
gennnisi 17:9ee4c69d2d8b 108 wait(0.1);
gennnisi 11:62c85df03747 109 }
gennnisi 12:aef2a6626ea5 110 motor_shoot.output(0);
gennnisi 12:aef2a6626ea5 111 shoot_phase = 2;
gennnisi 12:aef2a6626ea5 112 printf("finished loading\n");
gennnisi 16:04f42eeb9561 113 wait(3);
gennnisi 9:6008a52fb6ef 114 break;
gennnisi 12:aef2a6626ea5 115
gennnisi 13:66e37e0aa786 116 case 2://リボルバー回転,モーター復帰
gennnisi 9:6008a52fb6ef 117 turn_able = 1;
gennnisi 16:04f42eeb9561 118 back_able = 1;
gennnisi 13:66e37e0aa786 119 revolver_back(count);
gennnisi 17:9ee4c69d2d8b 120 shoot_phase = 3;
gennnisi 13:66e37e0aa786 121 printf("finished backing\n");
gennnisi 17:9ee4c69d2d8b 122 wait(3);
gennnisi 9:6008a52fb6ef 123 break;
gennnisi 12:aef2a6626ea5 124
gennnisi 13:66e37e0aa786 125 case 3://発射動作
gennnisi 12:aef2a6626ea5 126 servo.pulsewidth_us(theta_45);
gennnisi 17:9ee4c69d2d8b 127 printf("%d shot\n",count);
gennnisi 16:04f42eeb9561 128 wait(3);
gennnisi 9:6008a52fb6ef 129 servo.pulsewidth_us(theta_0);
gennnisi 15:7c26b01dcefd 130 motor_shoot.output(load_pwm);
gennnisi 16:04f42eeb9561 131 wait(0.15);
gennnisi 15:7c26b01dcefd 132 motor_shoot.output(0);
gennnisi 10:5e3c789e07dc 133 shoot_phase = 0;
gennnisi 11:62c85df03747 134 printf("finished init\n");
gennnisi 17:9ee4c69d2d8b 135 count ++;
gennnisi 12:aef2a6626ea5 136 break;
gennnisi 11:62c85df03747 137 }
gennnisi 9:6008a52fb6ef 138 }
gennnisi 12:aef2a6626ea5 139 }