2022_gorobo_Ateam / Mbed 2 deprecated beenbag_shoot

program to shoot beenbags

Dependencies:   mbed MOTOR

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

main.cpp@3:326387780345, 2022-01-13 (annotated)

Committer:
gennnisi
Date:
Thu Jan 13 14:19:49 2022 +0000
Revision:
3:326387780345
Parent:
0:0128d59ac729
Child:
4:d71a97acbf01
aaa

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 3:326387780345 10 RotaryInc roricon(PA_6,PA_7,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 0:0128d59ac729 15 int shoot_able;//発射フェーズ 0:待機　1:引き動作　2:リボルバー1/6回転　3:発射　
gennnisi 0:0128d59ac729 16 int turn_able;//回転フェーズ　0:待機　1:1/6回転
gennnisi 0:0128d59ac729 17
gennnisi 0:0128d59ac729 18 double revolver_pwm = 0.1;//リボルバー回転速度
gennnisi 0:0128d59ac729 19 double interval = 0.1;
gennnisi 0:0128d59ac729 20
gennnisi 0:0128d59ac729 21 bool interrupt(int rx_data,int &tx_data){
gennnisi 0:0128d59ac729 22 if(shoot_able == 0){//フェーズ0以外は、発射できないように
gennnisi 0:0128d59ac729 23 shoot_able = rx_data;//1を受け取って引き動作開始
gennnisi 0:0128d59ac729 24 }
gennnisi 0:0128d59ac729 25 return true;
gennnisi 0:0128d59ac729 26 }
gennnisi 0:0128d59ac729 27
gennnisi 0:0128d59ac729 28 //リボルバーP制御関数
gennnisi 0:0128d59ac729 29 //double k_p = 0.001;
gennnisi 0:0128d59ac729 30 //double target = 171.0;
gennnisi 0:0128d59ac729 31 /*void revolver(int count){
gennnisi 0:0128d59ac729 32 if(turn_able == 1){
gennnisi 0:0128d59ac729 33 roricon = v.get() - (171 * (count - 1));
gennnisi 0:0128d59ac729 34
gennnisi 0:0128d59ac729 35 while(roricon != 171){
gennnisi 0:0128d59ac729 36 pwm = k_p * (target - roricon);
gennnisi 0:0128d59ac729 37 motor_revolver.output(pwm);
gennnisi 0:0128d59ac729 38 printf("roricon = %lf\n",roricon);
gennnisi 0:0128d59ac729 39 printf("pwm = %lf\n",pwm);
gennnisi 0:0128d59ac729 40 wait(interval);
gennnisi 0:0128d59ac729 41 }
gennnisi 0:0128d59ac729 42 motor_revolver.output(0.0);
gennnisi 0:0128d59ac729 43 turn_able = 0;
gennnisi 0:0128d59ac729 44 }
gennnisi 0:0128d59ac729 45 }*/
gennnisi 0:0128d59ac729 46
gennnisi 0:0128d59ac729 47 void revolver(int count){
gennnisi 0:0128d59ac729 48 if(turn_able == 1){
gennnisi 0:0128d59ac729 49 while(roricon.get() < 171 * count){
gennnisi 0:0128d59ac729 50 motor_revolver.output(revolver_pwm);
gennnisi 0:0128d59ac729 51 printf("roricon = %lf\n",roricon.get());
gennnisi 0:0128d59ac729 52 wait(interval);
gennnisi 0:0128d59ac729 53 }
gennnisi 0:0128d59ac729 54 motor_revolver.output(0.0);
gennnisi 0:0128d59ac729 55 turn_able = 0;
gennnisi 0:0128d59ac729 56 }
gennnisi 0:0128d59ac729 57 }
gennnisi 0:0128d59ac729 58
gennnisi 0:0128d59ac729 59 int main(){
gennnisi 0:0128d59ac729 60 int count = 1;//何発目か
gennnisi 0:0128d59ac729 61
gennnisi 0:0128d59ac729 62 slave.addCMD(2,interrupt);
gennnisi 0:0128d59ac729 63 while(1){
gennnisi 0:0128d59ac729 64 switch (shoot_able){
gennnisi 0:0128d59ac729 65 case 0:
gennnisi 0:0128d59ac729 66 //待機
gennnisi 0:0128d59ac729 67 while(shoot_able == 0){
gennnisi 0:0128d59ac729 68 wait(0.1);
gennnisi 0:0128d59ac729 69 printf("waiting\n");
gennnisi 0:0128d59ac729 70 }
gennnisi 0:0128d59ac729 71 printf("finished waiting");
gennnisi 0:0128d59ac729 72 break;
gennnisi 0:0128d59ac729 73
gennnisi 0:0128d59ac729 74 case 1:
gennnisi 0:0128d59ac729 75 //装填動作コードをここに書く！
gennnisi 0:0128d59ac729 76 shoot_able = 2;
gennnisi 0:0128d59ac729 77 printf("finished pulling");
gennnisi 0:0128d59ac729 78 break;
gennnisi 0:0128d59ac729 79
gennnisi 0:0128d59ac729 80 case 2:
gennnisi 0:0128d59ac729 81 //リボルバー回転
gennnisi 0:0128d59ac729 82 turn_able = 1;
gennnisi 0:0128d59ac729 83 revolver(count);
gennnisi 0:0128d59ac729 84 if(turn_able == 0){
gennnisi 0:0128d59ac729 85 shoot_able = 3;
gennnisi 0:0128d59ac729 86 }
gennnisi 0:0128d59ac729 87 printf("finished turnning");
gennnisi 0:0128d59ac729 88 break;
gennnisi 0:0128d59ac729 89
gennnisi 0:0128d59ac729 90 case 3:
gennnisi 0:0128d59ac729 91 //発射動作コードをここに書く！
gennnisi 0:0128d59ac729 92 shoot_able = 0;
gennnisi 0:0128d59ac729 93 count ++;
gennnisi 0:0128d59ac729 94 printf("finished shooting");
gennnisi 0:0128d59ac729 95 break;
gennnisi 0:0128d59ac729 96 }
gennnisi 0:0128d59ac729 97 }
gennnisi 0:0128d59ac729 98 }