2022_gorobo_Ateam / Mbed 2 deprecated beenbag_shoot

program to shoot beenbags

Dependencies:   mbed MOTOR

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

main.cpp@17:9ee4c69d2d8b, 2022-01-27 (annotated)

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