2022_gorobo_Ateam / Mbed 2 deprecated beenbag_shoot

program to shoot beenbags

Dependencies:   mbed MOTOR

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

main.cpp@7:0e3a6da2e175, 2022-01-15 (annotated)

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