2022_gorobo_Ateam / Mbed 2 deprecated beenbag_shoot

program to shoot beenbags

Dependencies:   mbed MOTOR

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

main.cpp@8:38db82c3492a, 2022-01-17 (annotated)

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