2022_gorobo_Ateam / Mbed 2 deprecated beenbag_shoot

program to shoot beenbags

Dependencies:   mbed MOTOR

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

main.cpp@19:fbb3b5331641, 2022-01-30 (annotated)

Committer:
gennnisi
Date:
Sun Jan 30 07:25:47 2022 +0000
Revision:
19:fbb3b5331641
Parent:
18:c61ba21bbc67
Child:
20:4db5f042a45c
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gennnisi 0:0128d59ac729 1 #include "mbed.h"
gennnisi 0:0128d59ac729 2 #include "rotary_inc.hpp"
gennnisi 0:0128d59ac729 3 #include "scrp_slave.hpp"
gennnisi 0:0128d59ac729 4 #include "Motor.hpp"
gennnisi 0:0128d59ac729 5
gennnisi 19:fbb3b5331641 6 //-----インスタンス--------------------------
gennnisi 19:fbb3b5331641 7
gennnisi 18:c61ba21bbc67 8 Motor motor_revolver(PA_8,PA_7);//リボルバーモーター
gennnisi 18:c61ba21bbc67 9 Motor motor_shoot(PB_6,PA_11);//発射機構モーター
gennnisi 18:c61ba21bbc67 10 PwmOut servo(PA_1);//ロック解除用サーボ
gennnisi 9:6008a52fb6ef 11
gennnisi 18:c61ba21bbc67 12 ScrpSlave slave(PA_9,PA_10,PA_12,SERIAL_TX,SERIAL_RX,3);//id=3
gennnisi 0:0128d59ac729 13
gennnisi 18:c61ba21bbc67 14 RotaryInc v(PA_0,PA_4,1,1024,2);//リボルバーロリコン
gennnisi 0:0128d59ac729 15
gennnisi 18:c61ba21bbc67 16 DigitalIn limit_1(PB_0);//リミットスイッチ１
gennnisi 18:c61ba21bbc67 17 DigitalIn limit_2(PB_1);//リミットスイッチ２
gennnisi 6:7dcc827bbe96 18
gennnisi 19:fbb3b5331641 19 //-----変数---------------------------------
gennnisi 19:fbb3b5331641 20
gennnisi 17:9ee4c69d2d8b 21 int shoot_phase = 0;//発射フェーズ 0:待機　1:装填動作　2:リボルバー回転、モーター復帰（同時）　3:発射　
gennnisi 18:c61ba21bbc67 22 bool turn_able = 0;//回転可否
gennnisi 18:c61ba21bbc67 23 bool back_able = 0;//復帰可否　
gennnisi 0:0128d59ac729 24
gennnisi 19:fbb3b5331641 25 bool mode = 0;//0:手動　1:自動
gennnisi 19:fbb3b5331641 26 bool emergency = 0;//0:停止　1:停止
gennnisi 12:aef2a6626ea5 27 double revolver_pwm = -0.03;//リボルバー回転速度
gennnisi 9:6008a52fb6ef 28 double load_pwm = -0.3;//装填モーター速度
gennnisi 9:6008a52fb6ef 29 double back_pwm = 0.3;//復帰モーター速度
gennnisi 9:6008a52fb6ef 30
gennnisi 12:aef2a6626ea5 31 int theta_0 = 1290;//0度
gennnisi 12:aef2a6626ea5 32 int theta_45 = 1875;//45度
gennnisi 9:6008a52fb6ef 33 int roricon = 0;//ロリコン
gennnisi 0:0128d59ac729 34
gennnisi 19:fbb3b5331641 35 //-----関数---------------------------------
gennnisi 19:fbb3b5331641 36
gennnisi 19:fbb3b5331641 37 //発射開始を受信
gennnisi 19:fbb3b5331641 38 bool get_start(int rx_data,int &tx_data){
gennnisi 10:5e3c789e07dc 39 if(shoot_phase == 0){//フェーズ0以外は、発射できないように
gennnisi 18:c61ba21bbc67 40 shoot_phase = 1;
gennnisi 0:0128d59ac729 41 }
gennnisi 0:0128d59ac729 42 return true;
gennnisi 0:0128d59ac729 43 }
gennnisi 0:0128d59ac729 44
gennnisi 19:fbb3b5331641 45 //モード変更を受信
gennnisi 19:fbb3b5331641 46 bool get_changemode(int rx_data,int &tx_data){
gennnisi 19:fbb3b5331641 47 if(mode == 0){
gennnisi 19:fbb3b5331641 48 mode = 1;
gennnisi 19:fbb3b5331641 49 }else{
gennnisi 19:fbb3b5331641 50 mode = 0;
gennnisi 19:fbb3b5331641 51 }
gennnisi 19:fbb3b5331641 52 }
gennnisi 19:fbb3b5331641 53
gennnisi 19:fbb3b5331641 54 //非常停止を受信
gennnisi 19:fbb3b5331641 55 bool get_stop(int rx_data,int &tx_data){
gennnisi 19:fbb3b5331641 56 if(emergency == 0){
gennnisi 19:fbb3b5331641 57 emergency = 1;//停止
gennnisi 19:fbb3b5331641 58 shootphase = 10;
gennnisi 19:fbb3b5331641 59 }else{
gennnisi 19:fbb3b5331641 60 emergency = 0;//解除
gennnisi 19:fbb3b5331641 61 }
gennnisi 19:fbb3b5331641 62 }
gennnisi 19:fbb3b5331641 63
gennnisi 19:fbb3b5331641 64 //リボルバー、ラック復帰
gennnisi 13:66e37e0aa786 65 void revolver_back(int count){
gennnisi 16:04f42eeb9561 66 while(turn_able == 1 || back_able == 1){
gennnisi 16:04f42eeb9561 67 //リボルバー
gennnisi 16:04f42eeb9561 68 if(count == 1){
gennnisi 16:04f42eeb9561 69 motor_revolver.output(0);
gennnisi 16:04f42eeb9561 70 turn_able = 0;
gennnisi 17:9ee4c69d2d8b 71 printf("no turn\n");
gennnisi 16:04f42eeb9561 72 }
gennnisi 18:c61ba21bbc67 73 if(count > 1){
gennnisi 16:04f42eeb9561 74 if(roricon < 341 * (count - 1)){
gennnisi 16:04f42eeb9561 75 if(turn_able == 1){
gennnisi 14:81f09ab5ed23 76 roricon = v.get();
gennnisi 14:81f09ab5ed23 77 motor_revolver.output(revolver_pwm);
gennnisi 16:04f42eeb9561 78 printf("roricon = %d\n",roricon);
gennnisi 14:81f09ab5ed23 79 }
gennnisi 13:66e37e0aa786 80 }
gennnisi 16:04f42eeb9561 81 else{
gennnisi 16:04f42eeb9561 82 motor_revolver.output(0);
gennnisi 16:04f42eeb9561 83 turn_able = 0;
gennnisi 16:04f42eeb9561 84 }
gennnisi 16:04f42eeb9561 85 }
gennnisi 16:04f42eeb9561 86 //復帰
gennnisi 16:04f42eeb9561 87 if(limit_2.read() == 0){
gennnisi 16:04f42eeb9561 88 if(back_able == 1){
gennnisi 13:66e37e0aa786 89 motor_shoot.output(back_pwm);
gennnisi 16:04f42eeb9561 90 printf("backing\n");
gennnisi 13:66e37e0aa786 91 }
gennnisi 16:04f42eeb9561 92 }
gennnisi 16:04f42eeb9561 93 if(limit_2.read() == 1){
gennnisi 16:04f42eeb9561 94 motor_shoot.output(0);
gennnisi 16:04f42eeb9561 95 back_able = 0;
gennnisi 16:04f42eeb9561 96 printf("finished backing\n");
gennnisi 9:6008a52fb6ef 97 }
gennnisi 16:04f42eeb9561 98 //ループ抜け出し
gennnisi 18:c61ba21bbc67 99 if(turn_able == 0 && back_able == 0){
gennnisi 16:04f42eeb9561 100 break;
gennnisi 16:04f42eeb9561 101 }
gennnisi 17:9ee4c69d2d8b 102 wait(0.1);
gennnisi 9:6008a52fb6ef 103 }
gennnisi 9:6008a52fb6ef 104 }
gennnisi 9:6008a52fb6ef 105
gennnisi 19:fbb3b5331641 106 //------------------------------------------
gennnisi 19:fbb3b5331641 107
gennnisi 9:6008a52fb6ef 108 int main(){
gennnisi 9:6008a52fb6ef 109 int count = 1;//何発目か
gennnisi 9:6008a52fb6ef 110
gennnisi 9:6008a52fb6ef 111 servo.period_us(20000);
gennnisi 10:5e3c789e07dc 112 limit_1.mode(PullUp);
gennnisi 10:5e3c789e07dc 113 limit_2.mode(PullUp);
gennnisi 9:6008a52fb6ef 114
gennnisi 19:fbb3b5331641 115 slave.addCMD(2,get_start);
gennnisi 19:fbb3b5331641 116 slave.addCMD(50,get_changemode);
gennnisi 19:fbb3b5331641 117 slabe.addCMD(51,get_stop);
gennnisi 12:aef2a6626ea5 118
gennnisi 9:6008a52fb6ef 119 while(1){
gennnisi 19:fbb3b5331641 120 if(mode == 1){
gennnisi 19:fbb3b5331641 121 switch (shoot_phase){
gennnisi 19:fbb3b5331641 122 case 0://待機
gennnisi 19:fbb3b5331641 123 while(shoot_phase == 0){
gennnisi 19:fbb3b5331641 124 wait(0.1);
gennnisi 19:fbb3b5331641 125 printf("waiting\n");
gennnisi 15:7c26b01dcefd 126 }
gennnisi 19:fbb3b5331641 127 shoot_phase = 1;
gennnisi 19:fbb3b5331641 128 printf("finished waiting\n");
gennnisi 19:fbb3b5331641 129 break;
gennnisi 19:fbb3b5331641 130
gennnisi 19:fbb3b5331641 131 case 1://装填動作
gennnisi 19:fbb3b5331641 132 while(limit_1.read() == 0){
gennnisi 19:fbb3b5331641 133 motor_shoot.output(load_pwm);
gennnisi 19:fbb3b5331641 134 printf("loading\n");
gennnisi 19:fbb3b5331641 135 if(limit_2.read() == 1){
gennnisi 19:fbb3b5331641 136 break;
gennnisi 19:fbb3b5331641 137 }
gennnisi 19:fbb3b5331641 138 wait(0.1);
gennnisi 19:fbb3b5331641 139 }
gennnisi 19:fbb3b5331641 140 motor_shoot.output(0);
gennnisi 19:fbb3b5331641 141 shoot_phase = 2;
gennnisi 19:fbb3b5331641 142 printf("finished loading\n");
gennnisi 19:fbb3b5331641 143 wait(3);
gennnisi 19:fbb3b5331641 144 break;
gennnisi 19:fbb3b5331641 145
gennnisi 19:fbb3b5331641 146 case 2://リボルバー回転,モーター復帰
gennnisi 19:fbb3b5331641 147 turn_able = 1;
gennnisi 19:fbb3b5331641 148 back_able = 1;
gennnisi 19:fbb3b5331641 149 revolver_back(count);
gennnisi 19:fbb3b5331641 150 shoot_phase = 3;
gennnisi 19:fbb3b5331641 151 printf("finished backing\n");
gennnisi 19:fbb3b5331641 152 wait(3);
gennnisi 19:fbb3b5331641 153 break;
gennnisi 19:fbb3b5331641 154
gennnisi 19:fbb3b5331641 155 case 3://発射動作
gennnisi 19:fbb3b5331641 156 servo.pulsewidth_us(theta_45);
gennnisi 19:fbb3b5331641 157 printf("%d shot\n",count);
gennnisi 19:fbb3b5331641 158 wait(3);
gennnisi 19:fbb3b5331641 159 s
gennnisi 19:fbb3b5331641 160 ervo.pulsewidth_us(theta_0);
gennnisi 19:fbb3b5331641 161 motor_shoot.output(load_pwm);
gennnisi 19:fbb3b5331641 162 wait(0.15);
gennnisi 19:fbb3b5331641 163 motor_shoot.output(0);
gennnisi 19:fbb3b5331641 164 shoot_phase = 0;
gennnisi 19:fbb3b5331641 165 printf("finished init\n");
gennnisi 19:fbb3b5331641 166 count ++;
gennnisi 19:fbb3b5331641 167 break;
gennnisi 19:fbb3b5331641 168
gennnisi 19:fbb3b5331641 169 //非常停止
gennnisi 19:fbb3b5331641 170 case 10:
gennnisi 19:fbb3b5331641 171 while(emergency == 1){
gennnisi 19:fbb3b5331641 172 motor_shoot.output(0);
gennnisi 19:fbb3b5331641 173 motor_revolver.output(0);
gennnisi 19:fbb3b5331641 174 wait(0.1);
gennnisi 19:fbb3b5331641 175 }
gennnisi 19:fbb3b5331641 176 break;
gennnisi 19:fbb3b5331641 177 }
gennnisi 11:62c85df03747 178 }
gennnisi 9:6008a52fb6ef 179 }
gennnisi 12:aef2a6626ea5 180 }