2022_gorobo_Ateam / Mbed 2 deprecated beenbag_shoot

program to shoot beenbags

Dependencies:   mbed MOTOR

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

main.cpp@12:aef2a6626ea5, 2022-01-20 (annotated)

Committer:
gennnisi
Date:
Thu Jan 20 13:06:53 2022 +0000
Revision:
12:aef2a6626ea5
Parent:
11:62c85df03747
Child:
13:66e37e0aa786
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 10:5e3c789e07dc 20 int shoot_phase;//発射フェーズ 0:待機　1:装填動作　2:リボルバー回転　3:モーター復帰　4:発射　
gennnisi 10:5e3c789e07dc 21 bool turn_able;//回転可否　0:待機　1:回転
gennnisi 0:0128d59ac729 22
gennnisi 12:aef2a6626ea5 23 double revolver_pwm = -0.03;//リボルバー回転速度
gennnisi 9:6008a52fb6ef 24 double load_pwm = -0.3;//装填モーター速度
gennnisi 9:6008a52fb6ef 25 double back_pwm = 0.3;//復帰モーター速度
gennnisi 9:6008a52fb6ef 26
gennnisi 12:aef2a6626ea5 27 double turn = 0.0;
gennnisi 12:aef2a6626ea5 28
gennnisi 12:aef2a6626ea5 29 int theta_0 = 1290;//0度
gennnisi 12:aef2a6626ea5 30 int theta_45 = 1875;//45度
gennnisi 9:6008a52fb6ef 31 int roricon = 0;//ロリコン
gennnisi 0:0128d59ac729 32
gennnisi 0:0128d59ac729 33 bool interrupt(int rx_data,int &tx_data){
gennnisi 10:5e3c789e07dc 34 if(shoot_phase == 0){//フェーズ0以外は、発射できないように
gennnisi 10:5e3c789e07dc 35 shoot_phase = rx_data;//1を受け取って装填動作開始
gennnisi 0:0128d59ac729 36 }
gennnisi 0:0128d59ac729 37 return true;
gennnisi 0:0128d59ac729 38 }
gennnisi 0:0128d59ac729 39
gennnisi 9:6008a52fb6ef 40 void revolver(int count){
gennnisi 9:6008a52fb6ef 41 if(turn_able == 1){
gennnisi 12:aef2a6626ea5 42 while(roricon < 341 * count){
gennnisi 12:aef2a6626ea5 43 //while(1){
gennnisi 9:6008a52fb6ef 44 roricon = v.get();
gennnisi 9:6008a52fb6ef 45 motor_revolver.output(revolver_pwm);
gennnisi 9:6008a52fb6ef 46 printf("roricon = %d\n",roricon);
gennnisi 9:6008a52fb6ef 47 wait(0.1);
gennnisi 9:6008a52fb6ef 48 }
gennnisi 9:6008a52fb6ef 49 turn_able = 0;
gennnisi 9:6008a52fb6ef 50 }
gennnisi 9:6008a52fb6ef 51 }
gennnisi 9:6008a52fb6ef 52
gennnisi 9:6008a52fb6ef 53 int main(){
gennnisi 9:6008a52fb6ef 54 int count = 1;//何発目か
gennnisi 12:aef2a6626ea5 55 shoot_phase = 0;
gennnisi 9:6008a52fb6ef 56
gennnisi 9:6008a52fb6ef 57 servo.period_us(20000);
gennnisi 10:5e3c789e07dc 58 limit_1.mode(PullUp);
gennnisi 10:5e3c789e07dc 59 limit_2.mode(PullUp);
gennnisi 9:6008a52fb6ef 60
gennnisi 9:6008a52fb6ef 61 slave.addCMD(2,interrupt);
gennnisi 12:aef2a6626ea5 62
gennnisi 9:6008a52fb6ef 63 while(1){
gennnisi 10:5e3c789e07dc 64 switch (shoot_phase){
gennnisi 9:6008a52fb6ef 65
gennnisi 9:6008a52fb6ef 66 case 0://待機
gennnisi 10:5e3c789e07dc 67 while(shoot_phase == 0){
gennnisi 9:6008a52fb6ef 68 wait(0.1);
gennnisi 9:6008a52fb6ef 69 printf("waiting\n");
gennnisi 9:6008a52fb6ef 70 }
gennnisi 10:5e3c789e07dc 71 shoot_phase = 1;
gennnisi 9:6008a52fb6ef 72 printf("finished waiting\n");
gennnisi 9:6008a52fb6ef 73 break;
gennnisi 9:6008a52fb6ef 74
gennnisi 9:6008a52fb6ef 75 case 1://装填動作
gennnisi 9:6008a52fb6ef 76 while(limit_1.read() == 0){
gennnisi 10:5e3c789e07dc 77 motor_shoot.output(load_pwm);
gennnisi 11:62c85df03747 78 printf("loading\n");
gennnisi 11:62c85df03747 79 wait(0.1);
gennnisi 11:62c85df03747 80 }
gennnisi 12:aef2a6626ea5 81 motor_shoot.output(0);
gennnisi 12:aef2a6626ea5 82 shoot_phase = 2;
gennnisi 12:aef2a6626ea5 83 printf("finished loading\n");
gennnisi 12:aef2a6626ea5 84 wait(5);
gennnisi 9:6008a52fb6ef 85 break;
gennnisi 12:aef2a6626ea5 86
gennnisi 9:6008a52fb6ef 87 case 2://リボルバー回転
gennnisi 9:6008a52fb6ef 88 turn_able = 1;
gennnisi 9:6008a52fb6ef 89 revolver(count);
gennnisi 9:6008a52fb6ef 90 if(turn_able == 0){
gennnisi 12:aef2a6626ea5 91 motor_revolver.output(0);
gennnisi 10:5e3c789e07dc 92 shoot_phase = 3;
gennnisi 9:6008a52fb6ef 93 }
gennnisi 12:aef2a6626ea5 94 else{
gennnisi 12:aef2a6626ea5 95 printf("error\n");
gennnisi 12:aef2a6626ea5 96 }
gennnisi 9:6008a52fb6ef 97 printf("finished turnning\n");
gennnisi 9:6008a52fb6ef 98 break;
gennnisi 12:aef2a6626ea5 99
gennnisi 9:6008a52fb6ef 100 case 3://モーター復帰
gennnisi 9:6008a52fb6ef 101 while(limit_2.read() == 0){
gennnisi 9:6008a52fb6ef 102 motor_shoot.output(back_pwm);
gennnisi 11:62c85df03747 103 printf("backing\n");
gennnisi 11:62c85df03747 104 wait(0.1);
gennnisi 9:6008a52fb6ef 105 }
gennnisi 12:aef2a6626ea5 106
gennnisi 12:aef2a6626ea5 107 motor_shoot.output(0);
gennnisi 10:5e3c789e07dc 108 shoot_phase = 4;
gennnisi 9:6008a52fb6ef 109 printf("finished backing\n");
gennnisi 9:6008a52fb6ef 110 break;
gennnisi 9:6008a52fb6ef 111
gennnisi 9:6008a52fb6ef 112 case 4://発射動作
gennnisi 12:aef2a6626ea5 113 wait(10);
gennnisi 12:aef2a6626ea5 114 servo.pulsewidth_us(theta_45);
gennnisi 11:62c85df03747 115 printf("finished shooting %d shot\n",count);
gennnisi 9:6008a52fb6ef 116 count ++;
gennnisi 12:aef2a6626ea5 117 wait(5);
gennnisi 9:6008a52fb6ef 118 servo.pulsewidth_us(theta_0);
gennnisi 10:5e3c789e07dc 119 shoot_phase = 0;
gennnisi 11:62c85df03747 120 printf("finished init\n");
gennnisi 12:aef2a6626ea5 121 break;
gennnisi 11:62c85df03747 122 }
gennnisi 9:6008a52fb6ef 123 }
gennnisi 12:aef2a6626ea5 124 }