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@24:deda85e681a2, 2022-03-31 (annotated)

Committer:
gennnisi
Date:
Thu Mar 31 09:47:36 2022 +0000
Revision:
24:deda85e681a2
Parent:
23:a162feaea9ab
Child:
25:430aa10d6f13
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 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 24:deda85e681a2 93 return true;
gennnisi 24:deda85e681a2 94 }
gennnisi 24:deda85e681a2 95 }
gennnisi 24:deda85e681a2 96
gennnisi 19:fbb3b5331641 97 //リボルバー、ラック復帰
gennnisi 13:66e37e0aa786 98 void revolver_back(int count){
gennnisi 16:04f42eeb9561 99 while(turn_able == 1 || back_able == 1){
gennnisi 16:04f42eeb9561 100 //リボルバー
gennnisi 24:deda85e681a2 101 //一射目
gennnisi 16:04f42eeb9561 102 if(count == 1){
gennnisi 16:04f42eeb9561 103 motor_revolver.output(0);
gennnisi 16:04f42eeb9561 104 turn_able = 0;
gennnisi 24:deda85e681a2 105 back_able = 0;
gennnisi 24:deda85e681a2 106 printf("no turn and back\n");
gennnisi 16:04f42eeb9561 107 }
gennnisi 18:c61ba21bbc67 108 if(count > 1){
gennnisi 16:04f42eeb9561 109 if(roricon < 341 * (count - 1)){
gennnisi 16:04f42eeb9561 110 if(turn_able == 1){
gennnisi 14:81f09ab5ed23 111 roricon = v.get();
gennnisi 14:81f09ab5ed23 112 motor_revolver.output(revolver_pwm);
gennnisi 16:04f42eeb9561 113 printf("roricon = %d\n",roricon);
gennnisi 14:81f09ab5ed23 114 }
gennnisi 13:66e37e0aa786 115 }
gennnisi 16:04f42eeb9561 116 else{
gennnisi 16:04f42eeb9561 117 motor_revolver.output(0);
gennnisi 16:04f42eeb9561 118 turn_able = 0;
gennnisi 16:04f42eeb9561 119 }
gennnisi 16:04f42eeb9561 120 }
gennnisi 16:04f42eeb9561 121 //復帰
gennnisi 24:deda85e681a2 122 if(count > 1){
gennnisi 24:deda85e681a2 123 if(limit_2.read() == 0){
gennnisi 24:deda85e681a2 124 if(back_able == 1){
gennnisi 24:deda85e681a2 125 motor_shoot.output(back_pwm);
gennnisi 24:deda85e681a2 126 printf("backing\n");
gennnisi 24:deda85e681a2 127 }
gennnisi 13:66e37e0aa786 128 }
gennnisi 24:deda85e681a2 129 if(limit_2.read() == 1){
gennnisi 24:deda85e681a2 130 motor_shoot.output(0);
gennnisi 24:deda85e681a2 131 back_able = 0;
gennnisi 24:deda85e681a2 132 printf("finished backing\n");
gennnisi 24:deda85e681a2 133 }
gennnisi 9:6008a52fb6ef 134 }
gennnisi 16:04f42eeb9561 135 //ループ抜け出し
gennnisi 18:c61ba21bbc67 136 if(turn_able == 0 && back_able == 0){
gennnisi 16:04f42eeb9561 137 break;
gennnisi 16:04f42eeb9561 138 }
gennnisi 20:4db5f042a45c 139 wait(0.01);
gennnisi 9:6008a52fb6ef 140 }
gennnisi 9:6008a52fb6ef 141 }
gennnisi 9:6008a52fb6ef 142
gennnisi 19:fbb3b5331641 143 //------------------------------------------
gennnisi 19:fbb3b5331641 144
gennnisi 9:6008a52fb6ef 145 int main(){
gennnisi 9:6008a52fb6ef 146 int count = 1;//何発目か
gennnisi 9:6008a52fb6ef 147
gennnisi 9:6008a52fb6ef 148 servo.period_us(20000);
gennnisi 10:5e3c789e07dc 149 limit_1.mode(PullUp);
gennnisi 10:5e3c789e07dc 150 limit_2.mode(PullUp);
gennnisi 9:6008a52fb6ef 151
gennnisi 22:97328720703c 152 slave.addCMD(2,get_shoot);
gennnisi 24:deda85e681a2 153 slave.addCMD(6,get_move);
gennnisi 20:4db5f042a45c 154 slave.addCMD(51,get_stop);
gennnisi 12:aef2a6626ea5 155
gennnisi 24:deda85e681a2 156 slave.addCMD(10,init);
gennnisi 24:deda85e681a2 157
gennnisi 9:6008a52fb6ef 158 while(1){
gennnisi 21:9f917ee199ff 159 //----------自動発射------------------------------
gennnisi 22:97328720703c 160 switch (shoot_phase){
gennnisi 22:97328720703c 161 case 0://待機
gennnisi 24:deda85e681a2 162 led[0].write(1);
gennnisi 24:deda85e681a2 163 led[1].write(1);
gennnisi 24:deda85e681a2 164 led[2].write(1);
gennnisi 24:deda85e681a2 165 led[3].write(1);
gennnisi 19:fbb3b5331641 166 while(shoot_phase == 0){
gennnisi 19:fbb3b5331641 167 wait(0.1);
gennnisi 19:fbb3b5331641 168 printf("waiting\n");
gennnisi 15:7c26b01dcefd 169 }
gennnisi 23:a162feaea9ab 170 printf("finished waiting\n");
gennnisi 22:97328720703c 171 break;
gennnisi 19:fbb3b5331641 172
gennnisi 22:97328720703c 173 case 1://装填動作
gennnisi 24:deda85e681a2 174 led[0].write(0);
gennnisi 24:deda85e681a2 175 led[1].write(0);
gennnisi 24:deda85e681a2 176 led[2].write(0);
gennnisi 24:deda85e681a2 177 led[3].write(0);
gennnisi 23:a162feaea9ab 178 if(count < 7){
gennnisi 23:a162feaea9ab 179 while(limit_1.read() == 0){
gennnisi 24:deda85e681a2 180 if(count == 1){
gennnisi 24:deda85e681a2 181 break;
gennnisi 24:deda85e681a2 182 }
gennnisi 23:a162feaea9ab 183 motor_shoot.output(load_pwm);
gennnisi 23:a162feaea9ab 184 printf("loading\n");
gennnisi 23:a162feaea9ab 185 wait(0.1);
gennnisi 23:a162feaea9ab 186 }
gennnisi 23:a162feaea9ab 187 motor_shoot.output(0);
gennnisi 23:a162feaea9ab 188 shoot_phase = 2;
gennnisi 23:a162feaea9ab 189 printf("finished loading\n");
gennnisi 23:a162feaea9ab 190 wait(0.1);
gennnisi 23:a162feaea9ab 191 }else{
gennnisi 23:a162feaea9ab 192 emergency = 1;
gennnisi 23:a162feaea9ab 193 shoot_phase = 10;
gennnisi 19:fbb3b5331641 194 }
gennnisi 22:97328720703c 195 break;
gennnisi 19:fbb3b5331641 196
gennnisi 22:97328720703c 197 case 2://リボルバー回転,モーター復帰
gennnisi 19:fbb3b5331641 198 turn_able = 1;
gennnisi 19:fbb3b5331641 199 back_able = 1;
gennnisi 19:fbb3b5331641 200 revolver_back(count);
gennnisi 19:fbb3b5331641 201 shoot_phase = 3;
gennnisi 19:fbb3b5331641 202 printf("finished backing\n");
gennnisi 22:97328720703c 203 wait(0.5);
gennnisi 22:97328720703c 204 break;
gennnisi 19:fbb3b5331641 205
gennnisi 22:97328720703c 206 case 3://発射動作
gennnisi 19:fbb3b5331641 207 servo.pulsewidth_us(theta_45);
gennnisi 19:fbb3b5331641 208 printf("%d shot\n",count);
gennnisi 22:97328720703c 209 wait(1);
gennnisi 20:4db5f042a45c 210 servo.pulsewidth_us(theta_0);
gennnisi 23:a162feaea9ab 211 if(count != 6){
gennnisi 23:a162feaea9ab 212 motor_shoot.output(load_pwm);
gennnisi 23:a162feaea9ab 213 wait(0.15);
gennnisi 23:a162feaea9ab 214 motor_shoot.output(0);
gennnisi 23:a162feaea9ab 215 }
gennnisi 24:deda85e681a2 216 shoot_phase = 0;
gennnisi 19:fbb3b5331641 217 printf("finished init\n");
gennnisi 19:fbb3b5331641 218 count ++;
gennnisi 22:97328720703c 219 break;
gennnisi 19:fbb3b5331641 220
gennnisi 22:97328720703c 221 case 10://非常停止
gennnisi 19:fbb3b5331641 222 while(emergency == 1){
gennnisi 19:fbb3b5331641 223 motor_shoot.output(0);
gennnisi 19:fbb3b5331641 224 motor_revolver.output(0);
gennnisi 20:4db5f042a45c 225 printf("emergency\n");
gennnisi 19:fbb3b5331641 226 wait(0.1);
gennnisi 19:fbb3b5331641 227 }
gennnisi 24:deda85e681a2 228 break;
gennnisi 24:deda85e681a2 229
gennnisi 24:deda85e681a2 230 case 100://位置合わせ開始、装填
gennnisi 24:deda85e681a2 231 led[0].write(0);
gennnisi 24:deda85e681a2 232 led[1].write(0);
gennnisi 24:deda85e681a2 233 led[2].write(0);
gennnisi 24:deda85e681a2 234 led[3].write(0);
gennnisi 24:deda85e681a2 235 while(limit_2.read() == 0){
gennnisi 24:deda85e681a2 236 motor_shoot.output(back_pwm);
gennnisi 24:deda85e681a2 237 printf("loading\n");
gennnisi 24:deda85e681a2 238 wait(0.1);
gennnisi 24:deda85e681a2 239 }
gennnisi 24:deda85e681a2 240 motor_shoot.output(0);
gennnisi 24:deda85e681a2 241 shoot_phase = 0;
gennnisi 24:deda85e681a2 242 break;
gennnisi 22:97328720703c 243 }
gennnisi 9:6008a52fb6ef 244 }
gennnisi 12:aef2a6626ea5 245 }