2022_gorobo_Ateam
/
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.
Diff: main.cpp
- Revision:
- 24:deda85e681a2
- Parent:
- 23:a162feaea9ab
- Child:
- 25:430aa10d6f13
--- a/main.cpp Fri Mar 11 04:19:33 2022 +0000
+++ b/main.cpp Thu Mar 31 09:47:36 2022 +0000
@@ -5,35 +5,56 @@
//-----インスタンス--------------------------
-Motor motor_revolver(PA_8,PA_7);//リボルバーモーター
+ScrpSlave slave(PA_9,PA_10,PA_12,SERIAL_TX,SERIAL_RX,3);//ID=3
+
+Motor motor_revolver(PA_7,PA_8);//リボルバーモーター
Motor motor_shoot(PA_1,PA_3);//発射機構モーター
PwmOut servo(PB_6);//ロック解除用サーボ
-ScrpSlave slave(PA_9,PA_10,PA_12,SERIAL_TX,SERIAL_RX,3);//id=3
-
RotaryInc v(PA_0,PA_4,1,1024,2);//リボルバーロリコン
DigitalIn limit_1(PB_0);//リミットスイッチ１
DigitalIn limit_2(PB_1);//リミットスイッチ２
+DigitalOut led[4] = {
+ DigitalOut(PB_7),
+ DigitalOut(PB_5),
+ DigitalOut(PB_4),
+ DigitalOut(PB_3)
+};
+
//-----変数---------------------------------
-int shoot_phase = 0;//発射フェーズ 0:待機 1:装填動作 2:リボルバー回転、モーター復帰（同時） 3:発射
+int shoot_phase = 10;//発射フェーズ 0:待機 1:装填動作 2:リボルバー回転、モーター復帰（同時） 3:発射 10:停止 100:位置合わせ開始
bool turn_able = 0;//回転可否
bool back_able = 0;//復帰可否
bool shoot_start = 0;//発射開始 0:待機 1:開始済み
bool emergency = 0;//0:停止 1:停止
-double revolver_pwm = -0.1;//リボルバー回転速度
-double load_pwm = -0.5;//装填モーター速度
-double back_pwm = 0.5;//復帰モーター速度
+bool back = 0;
-int theta_0 = 1290;//0度
-int theta_45 = 1875;//45度
int roricon = 0;//ロリコン
+//-----定数---------------------------------
+const double revolver_pwm = -0.1;//リボルバー回転速度
+const double load_pwm = -0.5;//装填モーター速度
+const double back_pwm = 0.5;//復帰モーター速度
+
+const int theta_0 = 1290;//0度
+const int theta_45 = 1875;//45度
+
//-----関数---------------------------------
+//位置合わせ開始を受信
+bool get_move(int rx_data,int &tx_data){
+ if(shoot_phase == 0){//フェーズ0以外は、発射できないように
+ if(rx_data == 1){
+ shoot_phase = 100;
+ }
+ }
+ return true;
+}
+
//発射開始を受信
bool get_shoot(int rx_data,int &tx_data){
if(shoot_phase == 0){//フェーズ0以外は、発射できないように
@@ -46,21 +67,43 @@
//非常停止を受信
bool get_stop(int rx_data,int &tx_data){
- /*if(emergency == 0){
- emergency = 1;//停止
- shoot_phase = 10;
- }*/
+ if(rx_data == 0){
+ if(shoot_phase == 10){
+ shoot_phase = 0;
+ }
+ }
+ if(rx_data == 1){
+ while(rx_data == 1){
+ emergency = 1;
+ shoot_phase = 10;
+ }
+ }
return true;
}
+//初期化を受信
+bool init(int rx_data,int &tx_data){
+ if(rx_data == 1){
+ while(limit_1.read() == 0){
+ motor_shoot.output(load_pwm);
+ printf("loading\n");
+ wait(0.1);
+ }
+ motor_shoot.output(0);
+ return true;
+ }
+}
+
//リボルバー、ラック復帰
void revolver_back(int count){
while(turn_able == 1 || back_able == 1){
//リボルバー
+ //一射目
if(count == 1){
motor_revolver.output(0);
turn_able = 0;
- printf("no turn\n");
+ back_able = 0;
+ printf("no turn and back\n");
}
if(count > 1){
if(roricon < 341 * (count - 1)){
@@ -76,16 +119,18 @@
}
}
//復帰
- if(limit_2.read() == 0){
- if(back_able == 1){
- motor_shoot.output(back_pwm);
- printf("backing\n");
+ if(count > 1){
+ if(limit_2.read() == 0){
+ if(back_able == 1){
+ motor_shoot.output(back_pwm);
+ printf("backing\n");
+ }
}
- }
- if(limit_2.read() == 1){
- motor_shoot.output(0);
- back_able = 0;
- printf("finished backing\n");
+ if(limit_2.read() == 1){
+ motor_shoot.output(0);
+ back_able = 0;
+ printf("finished backing\n");
+ }
}
//ループ抜け出し
if(turn_able == 0 && back_able == 0){
@@ -105,12 +150,19 @@
limit_2.mode(PullUp);
slave.addCMD(2,get_shoot);
+ slave.addCMD(6,get_move);
slave.addCMD(51,get_stop);
+ slave.addCMD(10,init);
+
while(1){
//----------自動発射------------------------------
switch (shoot_phase){
case 0://待機
+ led[0].write(1);
+ led[1].write(1);
+ led[2].write(1);
+ led[3].write(1);
while(shoot_phase == 0){
wait(0.1);
printf("waiting\n");
@@ -119,8 +171,15 @@
break;
case 1://装填動作
+ led[0].write(0);
+ led[1].write(0);
+ led[2].write(0);
+ led[3].write(0);
if(count < 7){
while(limit_1.read() == 0){
+ if(count == 1){
+ break;
+ }
motor_shoot.output(load_pwm);
printf("loading\n");
wait(0.1);
@@ -141,7 +200,6 @@
revolver_back(count);
shoot_phase = 3;
printf("finished backing\n");
- wait_ms(2);
wait(0.5);
break;
@@ -155,7 +213,7 @@
wait(0.15);
motor_shoot.output(0);
}
- shoot_phase = 1;
+ shoot_phase = 0;
printf("finished init\n");
count ++;
break;
@@ -167,7 +225,21 @@
printf("emergency\n");
wait(0.1);
}
- break;
+ break;
+
+ case 100://位置合わせ開始、装填
+ led[0].write(0);
+ led[1].write(0);
+ led[2].write(0);
+ led[3].write(0);
+ while(limit_2.read() == 0){
+ motor_shoot.output(back_pwm);
+ printf("loading\n");
+ wait(0.1);
+ }
+ motor_shoot.output(0);
+ shoot_phase = 0;
+ break;
}
}
}
\ No newline at end of file