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@25:430aa10d6f13, 2022-04-02 (annotated)

Committer:
gennnisi
Date:
Sat Apr 02 06:48:29 2022 +0000
Revision:
25:430aa10d6f13
Parent:
24:deda85e681a2 
4/2

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 24:deda85e681a2 8 ScrpSlave slave(PA_9,PA_10,PA_12,SERIAL_TX,SERIAL_RX,3);//ID=3
gennnisi 24:deda85e681a2 9
gennnisi 24:deda85e681a2 10 Motor motor_revolver(PA_7,PA_8);//リボルバーモーター
gennnisi 20:4db5f042a45c 11 Motor motor_shoot(PA_1,PA_3);//発射機構モーター
gennnisi 20:4db5f042a45c 12 PwmOut servo(PB_6);//ロック解除用サーボ
gennnisi 9:6008a52fb6ef 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 24:deda85e681a2 19 DigitalOut led[4] = {
gennnisi 24:deda85e681a2 20 DigitalOut(PB_7),
gennnisi 24:deda85e681a2 21 DigitalOut(PB_5),
gennnisi 24:deda85e681a2 22 DigitalOut(PB_4),
gennnisi 24:deda85e681a2 23 DigitalOut(PB_3)
gennnisi 24:deda85e681a2 24 };
gennnisi 24:deda85e681a2 25
gennnisi 19:fbb3b5331641 26 //-----変数---------------------------------
gennnisi 19:fbb3b5331641 27
gennnisi 24:deda85e681a2 28 int shoot_phase = 10;//発射フェーズ 0:待機　1:装填動作　2:リボルバー回転、モーター復帰（同時）　3:発射　10:停止　　100:位置合わせ開始
gennnisi 18:c61ba21bbc67 29 bool turn_able = 0;//回転可否
gennnisi 18:c61ba21bbc67 30 bool back_able = 0;//復帰可否　
gennnisi 0:0128d59ac729 31
gennnisi 22:97328720703c 32 bool shoot_start = 0;//発射開始　0:待機　1:開始済み
gennnisi 19:fbb3b5331641 33 bool emergency = 0;//0:停止　1:停止
gennnisi 24:deda85e681a2 34 bool back = 0;
gennnisi 9:6008a52fb6ef 35
gennnisi 9:6008a52fb6ef 36 int roricon = 0;//ロリコン
gennnisi 0:0128d59ac729 37
gennnisi 24:deda85e681a2 38 //-----定数---------------------------------
gennnisi 24:deda85e681a2 39 const double revolver_pwm = -0.1;//リボルバー回転速度
gennnisi 24:deda85e681a2 40 const double load_pwm = -0.5;//装填モーター速度
gennnisi 24:deda85e681a2 41 const double back_pwm = 0.5;//復帰モーター速度
gennnisi 24:deda85e681a2 42
gennnisi 24:deda85e681a2 43 const int theta_0 = 1290;//0度
gennnisi 24:deda85e681a2 44 const int theta_45 = 1875;//45度
gennnisi 24:deda85e681a2 45
gennnisi 19:fbb3b5331641 46 //-----関数---------------------------------
gennnisi 19:fbb3b5331641 47
gennnisi 24:deda85e681a2 48 //位置合わせ開始を受信
gennnisi 24:deda85e681a2 49 bool get_move(int rx_data,int &tx_data){
gennnisi 24:deda85e681a2 50 if(shoot_phase == 0){//フェーズ0以外は、発射できないように
gennnisi 24:deda85e681a2 51 if(rx_data == 1){
gennnisi 24:deda85e681a2 52 shoot_phase = 100;
gennnisi 24:deda85e681a2 53 }
gennnisi 24:deda85e681a2 54 }
gennnisi 24:deda85e681a2 55 return true;
gennnisi 24:deda85e681a2 56 }
gennnisi 24:deda85e681a2 57
gennnisi 22:97328720703c 58 //発射開始を受信
gennnisi 22:97328720703c 59 bool get_shoot(int rx_data,int &tx_data){
gennnisi 23:a162feaea9ab 60 if(shoot_phase == 0){//フェーズ0以外は、発射できないように
gennnisi 23:a162feaea9ab 61 if(rx_data == 1){
gennnisi 23:a162feaea9ab 62 shoot_phase = 1;
gennnisi 23:a162feaea9ab 63 }
gennnisi 0:0128d59ac729 64 }
gennnisi 0:0128d59ac729 65 return true;
gennnisi 0:0128d59ac729 66 }
gennnisi 0:0128d59ac729 67
gennnisi 19:fbb3b5331641 68 //非常停止を受信
gennnisi 19:fbb3b5331641 69 bool get_stop(int rx_data,int &tx_data){
gennnisi 24:deda85e681a2 70 if(rx_data == 0){
gennnisi 24:deda85e681a2 71 if(shoot_phase == 10){
gennnisi 24:deda85e681a2 72 shoot_phase = 0;
gennnisi 24:deda85e681a2 73 }
gennnisi 24:deda85e681a2 74 }
gennnisi 24:deda85e681a2 75 if(rx_data == 1){
gennnisi 24:deda85e681a2 76 while(rx_data == 1){
gennnisi 24:deda85e681a2 77 emergency = 1;
gennnisi 24:deda85e681a2 78 shoot_phase = 10;
gennnisi 24:deda85e681a2 79 }
gennnisi 24:deda85e681a2 80 }
gennnisi 22:97328720703c 81 return true;
gennnisi 19:fbb3b5331641 82 }
gennnisi 19:fbb3b5331641 83
gennnisi 24:deda85e681a2 84 //初期化を受信
gennnisi 24:deda85e681a2 85 bool init(int rx_data,int &tx_data){
gennnisi 24:deda85e681a2 86 if(rx_data == 1){
gennnisi 24:deda85e681a2 87 while(limit_1.read() == 0){
gennnisi 24:deda85e681a2 88 motor_shoot.output(load_pwm);
gennnisi 24:deda85e681a2 89 printf("loading\n");
gennnisi 24:deda85e681a2 90 wait(0.1);
gennnisi 24:deda85e681a2 91 }
gennnisi 24:deda85e681a2 92 motor_shoot.output(0);
gennnisi 25:430aa10d6f13 93 shoot_phase = 0;
gennnisi 24:deda85e681a2 94 return true;
gennnisi 24:deda85e681a2 95 }
gennnisi 24:deda85e681a2 96 }
gennnisi 24:deda85e681a2 97
gennnisi 19:fbb3b5331641 98 //リボルバー、ラック復帰
gennnisi 13:66e37e0aa786 99 void revolver_back(int count){
gennnisi 16:04f42eeb9561 100 while(turn_able == 1 || back_able == 1){
gennnisi 16:04f42eeb9561 101 //リボルバー
gennnisi 24:deda85e681a2 102 //一射目
gennnisi 16:04f42eeb9561 103 if(count == 1){
gennnisi 16:04f42eeb9561 104 motor_revolver.output(0);
gennnisi 16:04f42eeb9561 105 turn_able = 0;
gennnisi 24:deda85e681a2 106 back_able = 0;
gennnisi 24:deda85e681a2 107 printf("no turn and back\n");
gennnisi 16:04f42eeb9561 108 }
gennnisi 18:c61ba21bbc67 109 if(count > 1){
gennnisi 16:04f42eeb9561 110 if(roricon < 341 * (count - 1)){
gennnisi 16:04f42eeb9561 111 if(turn_able == 1){
gennnisi 14:81f09ab5ed23 112 roricon = v.get();
gennnisi 14:81f09ab5ed23 113 motor_revolver.output(revolver_pwm);
gennnisi 16:04f42eeb9561 114 printf("roricon = %d\n",roricon);
gennnisi 14:81f09ab5ed23 115 }
gennnisi 13:66e37e0aa786 116 }
gennnisi 16:04f42eeb9561 117 else{
gennnisi 16:04f42eeb9561 118 motor_revolver.output(0);
gennnisi 16:04f42eeb9561 119 turn_able = 0;
gennnisi 16:04f42eeb9561 120 }
gennnisi 16:04f42eeb9561 121 }
gennnisi 16:04f42eeb9561 122 //復帰
gennnisi 24:deda85e681a2 123 if(count > 1){
gennnisi 24:deda85e681a2 124 if(limit_2.read() == 0){
gennnisi 24:deda85e681a2 125 if(back_able == 1){
gennnisi 24:deda85e681a2 126 motor_shoot.output(back_pwm);
gennnisi 24:deda85e681a2 127 printf("backing\n");
gennnisi 24:deda85e681a2 128 }
gennnisi 13:66e37e0aa786 129 }
gennnisi 24:deda85e681a2 130 if(limit_2.read() == 1){
gennnisi 24:deda85e681a2 131 motor_shoot.output(0);
gennnisi 24:deda85e681a2 132 back_able = 0;
gennnisi 24:deda85e681a2 133 printf("finished backing\n");
gennnisi 24:deda85e681a2 134 }
gennnisi 9:6008a52fb6ef 135 }
gennnisi 16:04f42eeb9561 136 //ループ抜け出し
gennnisi 18:c61ba21bbc67 137 if(turn_able == 0 && back_able == 0){
gennnisi 16:04f42eeb9561 138 break;
gennnisi 16:04f42eeb9561 139 }
gennnisi 20:4db5f042a45c 140 wait(0.01);
gennnisi 9:6008a52fb6ef 141 }
gennnisi 9:6008a52fb6ef 142 }
gennnisi 9:6008a52fb6ef 143
gennnisi 19:fbb3b5331641 144 //------------------------------------------
gennnisi 19:fbb3b5331641 145
gennnisi 9:6008a52fb6ef 146 int main(){
gennnisi 9:6008a52fb6ef 147 int count = 1;//何発目か
gennnisi 9:6008a52fb6ef 148
gennnisi 9:6008a52fb6ef 149 servo.period_us(20000);
gennnisi 10:5e3c789e07dc 150 limit_1.mode(PullUp);
gennnisi 10:5e3c789e07dc 151 limit_2.mode(PullUp);
gennnisi 9:6008a52fb6ef 152
gennnisi 22:97328720703c 153 slave.addCMD(2,get_shoot);
gennnisi 24:deda85e681a2 154 slave.addCMD(6,get_move);
gennnisi 20:4db5f042a45c 155 slave.addCMD(51,get_stop);
gennnisi 12:aef2a6626ea5 156
gennnisi 24:deda85e681a2 157 slave.addCMD(10,init);
gennnisi 24:deda85e681a2 158
gennnisi 9:6008a52fb6ef 159 while(1){
gennnisi 21:9f917ee199ff 160 //----------自動発射------------------------------
gennnisi 22:97328720703c 161 switch (shoot_phase){
gennnisi 22:97328720703c 162 case 0://待機
gennnisi 24:deda85e681a2 163 led[0].write(1);
gennnisi 24:deda85e681a2 164 led[1].write(1);
gennnisi 24:deda85e681a2 165 led[2].write(1);
gennnisi 24:deda85e681a2 166 led[3].write(1);
gennnisi 19:fbb3b5331641 167 while(shoot_phase == 0){
gennnisi 19:fbb3b5331641 168 wait(0.1);
gennnisi 19:fbb3b5331641 169 printf("waiting\n");
gennnisi 15:7c26b01dcefd 170 }
gennnisi 23:a162feaea9ab 171 printf("finished waiting\n");
gennnisi 22:97328720703c 172 break;
gennnisi 19:fbb3b5331641 173
gennnisi 22:97328720703c 174 case 1://装填動作
gennnisi 24:deda85e681a2 175 led[0].write(0);
gennnisi 24:deda85e681a2 176 led[1].write(0);
gennnisi 24:deda85e681a2 177 led[2].write(0);
gennnisi 24:deda85e681a2 178 led[3].write(0);
gennnisi 23:a162feaea9ab 179 if(count < 7){
gennnisi 23:a162feaea9ab 180 while(limit_1.read() == 0){
gennnisi 24:deda85e681a2 181 if(count == 1){
gennnisi 24:deda85e681a2 182 break;
gennnisi 24:deda85e681a2 183 }
gennnisi 23:a162feaea9ab 184 motor_shoot.output(load_pwm);
gennnisi 23:a162feaea9ab 185 printf("loading\n");
gennnisi 23:a162feaea9ab 186 wait(0.1);
gennnisi 23:a162feaea9ab 187 }
gennnisi 23:a162feaea9ab 188 motor_shoot.output(0);
gennnisi 23:a162feaea9ab 189 shoot_phase = 2;
gennnisi 23:a162feaea9ab 190 printf("finished loading\n");
gennnisi 23:a162feaea9ab 191 wait(0.1);
gennnisi 23:a162feaea9ab 192 }else{
gennnisi 23:a162feaea9ab 193 emergency = 1;
gennnisi 23:a162feaea9ab 194 shoot_phase = 10;
gennnisi 19:fbb3b5331641 195 }
gennnisi 22:97328720703c 196 break;
gennnisi 19:fbb3b5331641 197
gennnisi 22:97328720703c 198 case 2://リボルバー回転,モーター復帰
gennnisi 19:fbb3b5331641 199 turn_able = 1;
gennnisi 19:fbb3b5331641 200 back_able = 1;
gennnisi 19:fbb3b5331641 201 revolver_back(count);
gennnisi 19:fbb3b5331641 202 shoot_phase = 3;
gennnisi 19:fbb3b5331641 203 printf("finished backing\n");
gennnisi 22:97328720703c 204 wait(0.5);
gennnisi 22:97328720703c 205 break;
gennnisi 19:fbb3b5331641 206
gennnisi 22:97328720703c 207 case 3://発射動作
gennnisi 19:fbb3b5331641 208 servo.pulsewidth_us(theta_45);
gennnisi 19:fbb3b5331641 209 printf("%d shot\n",count);
gennnisi 22:97328720703c 210 wait(1);
gennnisi 20:4db5f042a45c 211 servo.pulsewidth_us(theta_0);
gennnisi 23:a162feaea9ab 212 if(count != 6){
gennnisi 23:a162feaea9ab 213 motor_shoot.output(load_pwm);
gennnisi 23:a162feaea9ab 214 wait(0.15);
gennnisi 23:a162feaea9ab 215 motor_shoot.output(0);
gennnisi 23:a162feaea9ab 216 }
gennnisi 24:deda85e681a2 217 shoot_phase = 0;
gennnisi 19:fbb3b5331641 218 printf("finished init\n");
gennnisi 19:fbb3b5331641 219 count ++;
gennnisi 22:97328720703c 220 break;
gennnisi 19:fbb3b5331641 221
gennnisi 22:97328720703c 222 case 10://非常停止
gennnisi 19:fbb3b5331641 223 while(emergency == 1){
gennnisi 19:fbb3b5331641 224 motor_shoot.output(0);
gennnisi 19:fbb3b5331641 225 motor_revolver.output(0);
gennnisi 20:4db5f042a45c 226 printf("emergency\n");
gennnisi 19:fbb3b5331641 227 wait(0.1);
gennnisi 19:fbb3b5331641 228 }
gennnisi 24:deda85e681a2 229 break;
gennnisi 24:deda85e681a2 230
gennnisi 24:deda85e681a2 231 case 100://位置合わせ開始、装填
gennnisi 24:deda85e681a2 232 led[0].write(0);
gennnisi 24:deda85e681a2 233 led[1].write(0);
gennnisi 24:deda85e681a2 234 led[2].write(0);
gennnisi 24:deda85e681a2 235 led[3].write(0);
gennnisi 24:deda85e681a2 236 while(limit_2.read() == 0){
gennnisi 24:deda85e681a2 237 motor_shoot.output(back_pwm);
gennnisi 24:deda85e681a2 238 printf("loading\n");
gennnisi 24:deda85e681a2 239 wait(0.1);
gennnisi 24:deda85e681a2 240 }
gennnisi 24:deda85e681a2 241 motor_shoot.output(0);
gennnisi 24:deda85e681a2 242 shoot_phase = 0;
gennnisi 24:deda85e681a2 243 break;
gennnisi 22:97328720703c 244 }
gennnisi 9:6008a52fb6ef 245 }
gennnisi 12:aef2a6626ea5 246 }