2022_gorobo_Ateam / Mbed 2 deprecated beenbag_shoot_highspeed

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed MOTOR

main.cpp@23:a162feaea9ab, 2022-03-11 (annotated)

Committer:
gennnisi
Date:
Fri Mar 11 04:19:33 2022 +0000
Revision:
23:a162feaea9ab
Parent:
22:97328720703c
Child:
24:deda85e681a2
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gennnisi 21:9f917ee199ff 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 22:97328720703c 8 Motor motor_revolver(PA_8,PA_7);//リボルバーモーター
gennnisi 20:4db5f042a45c 9 Motor motor_shoot(PA_1,PA_3);//発射機構モーター
gennnisi 20:4db5f042a45c 10 PwmOut servo(PB_6);//ロック解除用サーボ
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 22:97328720703c 25 bool shoot_start = 0;//発射開始　0:待機　1:開始済み
gennnisi 19:fbb3b5331641 26 bool emergency = 0;//0:停止　1:停止
gennnisi 20:4db5f042a45c 27 double revolver_pwm = -0.1;//リボルバー回転速度
gennnisi 20:4db5f042a45c 28 double load_pwm = -0.5;//装填モーター速度
gennnisi 20:4db5f042a45c 29 double back_pwm = 0.5;//復帰モーター速度
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 22:97328720703c 37 //発射開始を受信
gennnisi 22:97328720703c 38 bool get_shoot(int rx_data,int &tx_data){
gennnisi 23:a162feaea9ab 39 if(shoot_phase == 0){//フェーズ0以外は、発射できないように
gennnisi 23:a162feaea9ab 40 if(rx_data == 1){
gennnisi 23:a162feaea9ab 41 shoot_phase = 1;
gennnisi 23:a162feaea9ab 42 }
gennnisi 0:0128d59ac729 43 }
gennnisi 0:0128d59ac729 44 return true;
gennnisi 0:0128d59ac729 45 }
gennnisi 0:0128d59ac729 46
gennnisi 19:fbb3b5331641 47 //非常停止を受信
gennnisi 19:fbb3b5331641 48 bool get_stop(int rx_data,int &tx_data){
gennnisi 23:a162feaea9ab 49 /*if(emergency == 0){
gennnisi 19:fbb3b5331641 50 emergency = 1;//停止
gennnisi 20:4db5f042a45c 51 shoot_phase = 10;
gennnisi 23:a162feaea9ab 52 }*/
gennnisi 22:97328720703c 53 return true;
gennnisi 19:fbb3b5331641 54 }
gennnisi 19:fbb3b5331641 55
gennnisi 19:fbb3b5331641 56 //リボルバー、ラック復帰
gennnisi 13:66e37e0aa786 57 void revolver_back(int count){
gennnisi 16:04f42eeb9561 58 while(turn_able == 1 || back_able == 1){
gennnisi 16:04f42eeb9561 59 //リボルバー
gennnisi 16:04f42eeb9561 60 if(count == 1){
gennnisi 16:04f42eeb9561 61 motor_revolver.output(0);
gennnisi 16:04f42eeb9561 62 turn_able = 0;
gennnisi 17:9ee4c69d2d8b 63 printf("no turn\n");
gennnisi 16:04f42eeb9561 64 }
gennnisi 18:c61ba21bbc67 65 if(count > 1){
gennnisi 16:04f42eeb9561 66 if(roricon < 341 * (count - 1)){
gennnisi 16:04f42eeb9561 67 if(turn_able == 1){
gennnisi 14:81f09ab5ed23 68 roricon = v.get();
gennnisi 14:81f09ab5ed23 69 motor_revolver.output(revolver_pwm);
gennnisi 16:04f42eeb9561 70 printf("roricon = %d\n",roricon);
gennnisi 14:81f09ab5ed23 71 }
gennnisi 13:66e37e0aa786 72 }
gennnisi 16:04f42eeb9561 73 else{
gennnisi 16:04f42eeb9561 74 motor_revolver.output(0);
gennnisi 16:04f42eeb9561 75 turn_able = 0;
gennnisi 16:04f42eeb9561 76 }
gennnisi 16:04f42eeb9561 77 }
gennnisi 16:04f42eeb9561 78 //復帰
gennnisi 16:04f42eeb9561 79 if(limit_2.read() == 0){
gennnisi 16:04f42eeb9561 80 if(back_able == 1){
gennnisi 13:66e37e0aa786 81 motor_shoot.output(back_pwm);
gennnisi 16:04f42eeb9561 82 printf("backing\n");
gennnisi 13:66e37e0aa786 83 }
gennnisi 16:04f42eeb9561 84 }
gennnisi 16:04f42eeb9561 85 if(limit_2.read() == 1){
gennnisi 16:04f42eeb9561 86 motor_shoot.output(0);
gennnisi 16:04f42eeb9561 87 back_able = 0;
gennnisi 16:04f42eeb9561 88 printf("finished backing\n");
gennnisi 9:6008a52fb6ef 89 }
gennnisi 16:04f42eeb9561 90 //ループ抜け出し
gennnisi 18:c61ba21bbc67 91 if(turn_able == 0 && back_able == 0){
gennnisi 16:04f42eeb9561 92 break;
gennnisi 16:04f42eeb9561 93 }
gennnisi 20:4db5f042a45c 94 wait(0.01);
gennnisi 9:6008a52fb6ef 95 }
gennnisi 9:6008a52fb6ef 96 }
gennnisi 9:6008a52fb6ef 97
gennnisi 19:fbb3b5331641 98 //------------------------------------------
gennnisi 19:fbb3b5331641 99
gennnisi 9:6008a52fb6ef 100 int main(){
gennnisi 9:6008a52fb6ef 101 int count = 1;//何発目か
gennnisi 9:6008a52fb6ef 102
gennnisi 9:6008a52fb6ef 103 servo.period_us(20000);
gennnisi 10:5e3c789e07dc 104 limit_1.mode(PullUp);
gennnisi 10:5e3c789e07dc 105 limit_2.mode(PullUp);
gennnisi 9:6008a52fb6ef 106
gennnisi 22:97328720703c 107 slave.addCMD(2,get_shoot);
gennnisi 20:4db5f042a45c 108 slave.addCMD(51,get_stop);
gennnisi 12:aef2a6626ea5 109
gennnisi 9:6008a52fb6ef 110 while(1){
gennnisi 21:9f917ee199ff 111 //----------自動発射------------------------------
gennnisi 22:97328720703c 112 switch (shoot_phase){
gennnisi 22:97328720703c 113 case 0://待機
gennnisi 19:fbb3b5331641 114 while(shoot_phase == 0){
gennnisi 19:fbb3b5331641 115 wait(0.1);
gennnisi 19:fbb3b5331641 116 printf("waiting\n");
gennnisi 15:7c26b01dcefd 117 }
gennnisi 23:a162feaea9ab 118 printf("finished waiting\n");
gennnisi 22:97328720703c 119 break;
gennnisi 19:fbb3b5331641 120
gennnisi 22:97328720703c 121 case 1://装填動作
gennnisi 23:a162feaea9ab 122 if(count < 7){
gennnisi 23:a162feaea9ab 123 while(limit_1.read() == 0){
gennnisi 23:a162feaea9ab 124 motor_shoot.output(load_pwm);
gennnisi 23:a162feaea9ab 125 printf("loading\n");
gennnisi 23:a162feaea9ab 126 wait(0.1);
gennnisi 23:a162feaea9ab 127 }
gennnisi 23:a162feaea9ab 128 motor_shoot.output(0);
gennnisi 23:a162feaea9ab 129 shoot_phase = 2;
gennnisi 23:a162feaea9ab 130 printf("finished loading\n");
gennnisi 23:a162feaea9ab 131 wait(0.1);
gennnisi 23:a162feaea9ab 132 }else{
gennnisi 23:a162feaea9ab 133 emergency = 1;
gennnisi 23:a162feaea9ab 134 shoot_phase = 10;
gennnisi 19:fbb3b5331641 135 }
gennnisi 22:97328720703c 136 break;
gennnisi 19:fbb3b5331641 137
gennnisi 22:97328720703c 138 case 2://リボルバー回転,モーター復帰
gennnisi 19:fbb3b5331641 139 turn_able = 1;
gennnisi 19:fbb3b5331641 140 back_able = 1;
gennnisi 19:fbb3b5331641 141 revolver_back(count);
gennnisi 19:fbb3b5331641 142 shoot_phase = 3;
gennnisi 19:fbb3b5331641 143 printf("finished backing\n");
gennnisi 23:a162feaea9ab 144 wait_ms(2);
gennnisi 22:97328720703c 145 wait(0.5);
gennnisi 22:97328720703c 146 break;
gennnisi 19:fbb3b5331641 147
gennnisi 22:97328720703c 148 case 3://発射動作
gennnisi 19:fbb3b5331641 149 servo.pulsewidth_us(theta_45);
gennnisi 19:fbb3b5331641 150 printf("%d shot\n",count);
gennnisi 22:97328720703c 151 wait(1);
gennnisi 20:4db5f042a45c 152 servo.pulsewidth_us(theta_0);
gennnisi 23:a162feaea9ab 153 if(count != 6){
gennnisi 23:a162feaea9ab 154 motor_shoot.output(load_pwm);
gennnisi 23:a162feaea9ab 155 wait(0.15);
gennnisi 23:a162feaea9ab 156 motor_shoot.output(0);
gennnisi 23:a162feaea9ab 157 }
gennnisi 23:a162feaea9ab 158 shoot_phase = 1;
gennnisi 19:fbb3b5331641 159 printf("finished init\n");
gennnisi 19:fbb3b5331641 160 count ++;
gennnisi 22:97328720703c 161 break;
gennnisi 19:fbb3b5331641 162
gennnisi 22:97328720703c 163 case 10://非常停止
gennnisi 19:fbb3b5331641 164 while(emergency == 1){
gennnisi 19:fbb3b5331641 165 motor_shoot.output(0);
gennnisi 19:fbb3b5331641 166 motor_revolver.output(0);
gennnisi 20:4db5f042a45c 167 printf("emergency\n");
gennnisi 19:fbb3b5331641 168 wait(0.1);
gennnisi 19:fbb3b5331641 169 }
gennnisi 22:97328720703c 170 break;
gennnisi 22:97328720703c 171 }
gennnisi 9:6008a52fb6ef 172 }
gennnisi 12:aef2a6626ea5 173 }