2022_gorobo_Ateam / Mbed 2 deprecated beenbag_shoot

program to shoot beenbags

Dependencies:   mbed MOTOR

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

main.cpp@6:7dcc827bbe96, 2022-01-14 (annotated)

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