2022_gorobo_Ateam / Mbed 2 deprecated beenbag_shoot

program to shoot beenbags

Dependencies:   mbed MOTOR

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

main.cpp@9:6008a52fb6ef, 2022-01-18 (annotated)

Committer:
gennnisi
Date:
Tue Jan 18 14:04:54 2022 +0000
Revision:
9:6008a52fb6ef
Parent:
8:38db82c3492a
Child:
10:5e3c789e07dc
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 9:6008a52fb6ef 8 Motor motor_shoot(PB_13,PB_14);//発射機構モーター
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 0:0128d59ac729 12 ScrpSlave slave(PC_12,PD_2,PH_1,SERIAL_TX,SERIAL_RX,0x0807f800);//f446re
gennnisi 0:0128d59ac729 13
gennnisi 9:6008a52fb6ef 14 RotaryInc v(PA_14,PA_15,1,1024,4);//リボルバーロリコン
gennnisi 0:0128d59ac729 15
gennnisi 9:6008a52fb6ef 16 DigitalIn limit_1(PA_9);//リミットスイッチ１
gennnisi 9:6008a52fb6ef 17 DigitalIn limit_2(PA_8);//リミットスイッチ２
gennnisi 6:7dcc827bbe96 18
gennnisi 9:6008a52fb6ef 19 int shoot_able;//発射フェーズ 0:待機　1:装填動作　2:リボルバー回転　3:モーター復帰　4:発射　
gennnisi 9:6008a52fb6ef 20 bool turn_able;//回転フェーズ　0:待機　1:回転
gennnisi 0:0128d59ac729 21
gennnisi 0:0128d59ac729 22 double revolver_pwm = 0.1;//リボルバー回転速度
gennnisi 9:6008a52fb6ef 23 double load_pwm = -0.3;//装填モーター速度
gennnisi 9:6008a52fb6ef 24 double back_pwm = 0.3;//復帰モーター速度
gennnisi 9:6008a52fb6ef 25
gennnisi 9:6008a52fb6ef 26 int theta_0 = 1450;//0度
gennnisi 9:6008a52fb6ef 27 int theta_90 = 2400;//90度
gennnisi 9:6008a52fb6ef 28 int roricon = 0;//ロリコン
gennnisi 0:0128d59ac729 29
gennnisi 0:0128d59ac729 30 bool interrupt(int rx_data,int &tx_data){
gennnisi 0:0128d59ac729 31 if(shoot_able == 0){//フェーズ0以外は、発射できないように
gennnisi 9:6008a52fb6ef 32 shoot_able = rx_data;//1を受け取って装填動作開始
gennnisi 0:0128d59ac729 33 }
gennnisi 0:0128d59ac729 34 return true;
gennnisi 0:0128d59ac729 35 }
gennnisi 0:0128d59ac729 36
gennnisi 9:6008a52fb6ef 37 void revolver(int count){
gennnisi 9:6008a52fb6ef 38 if(turn_able == 1){
gennnisi 9:6008a52fb6ef 39 while(roricon < 171 * count){
gennnisi 9:6008a52fb6ef 40 roricon = v.get();
gennnisi 9:6008a52fb6ef 41 motor_revolver.output(revolver_pwm);
gennnisi 9:6008a52fb6ef 42 printf("roricon = %d\n",roricon);
gennnisi 9:6008a52fb6ef 43 wait(0.1);
gennnisi 9:6008a52fb6ef 44 }
gennnisi 9:6008a52fb6ef 45 motor_revolver.output(0.0);
gennnisi 9:6008a52fb6ef 46 turn_able = 0;
gennnisi 9:6008a52fb6ef 47 }
gennnisi 9:6008a52fb6ef 48 }
gennnisi 9:6008a52fb6ef 49
gennnisi 9:6008a52fb6ef 50 int main(){
gennnisi 9:6008a52fb6ef 51 int count = 1;//何発目か
gennnisi 9:6008a52fb6ef 52
gennnisi 9:6008a52fb6ef 53 servo.period_us(20000);
gennnisi 9:6008a52fb6ef 54
gennnisi 9:6008a52fb6ef 55 slave.addCMD(2,interrupt);
gennnisi 9:6008a52fb6ef 56 while(1){
gennnisi 9:6008a52fb6ef 57 switch (shoot_able){
gennnisi 9:6008a52fb6ef 58
gennnisi 9:6008a52fb6ef 59 case 0://待機
gennnisi 9:6008a52fb6ef 60 while(shoot_able == 0){
gennnisi 9:6008a52fb6ef 61 wait(0.1);
gennnisi 9:6008a52fb6ef 62 printf("waiting\n");
gennnisi 9:6008a52fb6ef 63 }
gennnisi 9:6008a52fb6ef 64 shoot_able = 1;
gennnisi 9:6008a52fb6ef 65 printf("finished waiting\n");
gennnisi 9:6008a52fb6ef 66 break;
gennnisi 9:6008a52fb6ef 67
gennnisi 9:6008a52fb6ef 68 case 1://装填動作
gennnisi 9:6008a52fb6ef 69 while(limit_1.read() == 0){
gennnisi 9:6008a52fb6ef 70 motor_shoot.output(shoot_pwm);
gennnisi 9:6008a52fb6ef 71 printf("loading\n");
gennnisi 9:6008a52fb6ef 72 }
gennnisi 9:6008a52fb6ef 73 motor_shoot.output(0.0);
gennnisi 9:6008a52fb6ef 74 shoot_able = 2;
gennnisi 9:6008a52fb6ef 75 printf("finished loading\n");
gennnisi 9:6008a52fb6ef 76 break;
gennnisi 9:6008a52fb6ef 77
gennnisi 9:6008a52fb6ef 78 case 2://リボルバー回転
gennnisi 9:6008a52fb6ef 79 turn_able = 1;
gennnisi 9:6008a52fb6ef 80 revolver(count);
gennnisi 9:6008a52fb6ef 81 if(turn_able == 0){
gennnisi 9:6008a52fb6ef 82 shoot_able = 3;
gennnisi 9:6008a52fb6ef 83 }
gennnisi 9:6008a52fb6ef 84 printf("finished turnning\n");
gennnisi 9:6008a52fb6ef 85 break;
gennnisi 9:6008a52fb6ef 86
gennnisi 9:6008a52fb6ef 87 case 3://モーター復帰
gennnisi 9:6008a52fb6ef 88 while(limit_2.read() == 0){
gennnisi 9:6008a52fb6ef 89 motor_shoot.output(back_pwm);
gennnisi 9:6008a52fb6ef 90 printf("backing\n");
gennnisi 9:6008a52fb6ef 91 }
gennnisi 9:6008a52fb6ef 92 motor_shoot.output(0.0);
gennnisi 9:6008a52fb6ef 93 shoot_able = 4;
gennnisi 9:6008a52fb6ef 94 printf("finished backing\n");
gennnisi 9:6008a52fb6ef 95 break;
gennnisi 9:6008a52fb6ef 96
gennnisi 9:6008a52fb6ef 97 case 4://発射動作
gennnisi 9:6008a52fb6ef 98 servo.pulsewidth_us(theta_90);
gennnisi 9:6008a52fb6ef 99 shoot_able = 0;
gennnisi 9:6008a52fb6ef 100 printf("finished shooting %dst shot\n",count);
gennnisi 9:6008a52fb6ef 101 count ++;
gennnisi 9:6008a52fb6ef 102 wait(1);
gennnisi 9:6008a52fb6ef 103 servo.pulsewidth_us(theta_0);
gennnisi 9:6008a52fb6ef 104 printf("finished initialize %d\n");
gennnisi 9:6008a52fb6ef 105 break;
gennnisi 9:6008a52fb6ef 106
gennnisi 9:6008a52fb6ef 107 }
gennnisi 9:6008a52fb6ef 108 }
gennnisi 9:6008a52fb6ef 109 }
gennnisi 9:6008a52fb6ef 110
gennnisi 0:0128d59ac729 111 //リボルバーP制御関数
gennnisi 0:0128d59ac729 112 //double k_p = 0.001;
gennnisi 0:0128d59ac729 113 //double target = 171.0;
gennnisi 0:0128d59ac729 114 /*void revolver(int count){
gennnisi 0:0128d59ac729 115 if(turn_able == 1){
gennnisi 0:0128d59ac729 116 roricon = v.get() - (171 * (count - 1));
gennnisi 0:0128d59ac729 117
gennnisi 0:0128d59ac729 118 while(roricon != 171){
gennnisi 0:0128d59ac729 119 pwm = k_p * (target - roricon);
gennnisi 0:0128d59ac729 120 motor_revolver.output(pwm);
gennnisi 0:0128d59ac729 121 printf("roricon = %lf\n",roricon);
gennnisi 0:0128d59ac729 122 printf("pwm = %lf\n",pwm);
gennnisi 0:0128d59ac729 123 wait(interval);
gennnisi 0:0128d59ac729 124 }
gennnisi 0:0128d59ac729 125 motor_revolver.output(0.0);
gennnisi 0:0128d59ac729 126 turn_able = 0;
gennnisi 0:0128d59ac729 127 }
gennnisi 9:6008a52fb6ef 128 }*/