Diff: main.cpp

Revision:

19:fbb3b5331641

Parent:

18:c61ba21bbc67

Child:

20:4db5f042a45c

--- a/main.cpp	Sat Jan 29 15:43:32 2022 +0000
+++ b/main.cpp	Sun Jan 30 07:25:47 2022 +0000
@@ -1,9 +1,10 @@
-//発射、リボルバープログラム
 #include "mbed.h"
 #include "rotary_inc.hpp"
 #include "scrp_slave.hpp"
 #include "Motor.hpp"
 
+//-----インスタンス--------------------------
+
 Motor motor_revolver(PA_8,PA_7);//リボルバーモーター
 Motor motor_shoot(PB_6,PA_11);//発射機構モーター
 PwmOut servo(PA_1);//ロック解除用サーボ
@@ -15,27 +16,52 @@
 DigitalIn limit_1(PB_0);//リミットスイッチ１
 DigitalIn limit_2(PB_1);//リミットスイッチ２
 
+//-----変数---------------------------------
+
 int shoot_phase = 0;//発射フェーズ 0:待機　1:装填動作　2:リボルバー回転、モーター復帰（同時）　3:発射　
 bool turn_able = 0;//回転可否
 bool back_able = 0;//復帰可否　
 
+bool mode = 0;//0:手動　1:自動
+bool emergency = 0;//0:停止　1:停止
 double revolver_pwm = -0.03;//リボルバー回転速度
 double load_pwm = -0.3;//装填モーター速度
 double back_pwm = 0.3;//復帰モーター速度
 
-double turn = 0.0;
-
 int theta_0 = 1290;//0度
 int theta_45 = 1875;//45度
 int roricon = 0;//ロリコン
 
-bool interrupt(int rx_data,int &tx_data){
+//-----関数---------------------------------
+
+//発射開始を受信
+bool get_start(int rx_data,int &tx_data){
     if(shoot_phase == 0){//フェーズ0以外は、発射できないように
         shoot_phase = 1;
     }
     return true;  
 }
 
+//モード変更を受信
+bool get_changemode(int rx_data,int &tx_data){
+    if(mode == 0){
+        mode = 1;
+    }else{
+        mode = 0;
+    }
+}
+
+//非常停止を受信
+bool get_stop(int rx_data,int &tx_data){
+    if(emergency == 0){
+        emergency = 1;//停止
+        shootphase = 10;
+    }else{
+        emergency = 0;//解除
+    }
+}
+
+//リボルバー、ラック復帰
 void revolver_back(int count){
     while(turn_able == 1 || back_able == 1){
         //リボルバー
@@ -77,6 +103,8 @@
     }
 }
 
+//------------------------------------------
+
 int main(){
     int count = 1;//何発目か
     
@@ -84,56 +112,69 @@
     limit_1.mode(PullUp);
     limit_2.mode(PullUp);
     
-    slave.addCMD(2,interrupt);
+    slave.addCMD(2,get_start);
+    slave.addCMD(50,get_changemode);
+    slabe.addCMD(51,get_stop);
     
     while(1){
-        switch (shoot_phase){
-            
-            case 0://待機
-            while(shoot_phase == 0){
-                wait(0.1);
-                printf("waiting\n");  
-            }
-            shoot_phase = 1;
-            printf("finished waiting\n");  
-            break;
-            
-            case 1://装填動作
-            while(limit_1.read() == 0){
-                motor_shoot.output(load_pwm);
-                printf("loading\n");
-                if(limit_2.read() == 1){
-                    break;
+        if(mode == 1){
+            switch (shoot_phase){
+                case 0://待機
+                while(shoot_phase == 0){
+                    wait(0.1);
+                    printf("waiting\n");  
                 }
-                wait(0.1);
-            } 
-            motor_shoot.output(0);
-            shoot_phase = 2;
-            printf("finished loading\n");
-            wait(3); 
-            break;
-            
-            case 2://リボルバー回転,モーター復帰
-            turn_able = 1;
-            back_able = 1;
-            revolver_back(count);
-            shoot_phase = 3;
-            printf("finished backing\n"); 
-            wait(3);
-            break;
-            
-            case 3://発射動作
-            servo.pulsewidth_us(theta_45);
-            printf("%d shot\n",count);  
-            wait(3);
-            servo.pulsewidth_us(theta_0);
-            motor_shoot.output(load_pwm);
-            wait(0.15);
-            motor_shoot.output(0);
-            shoot_phase = 0;
-            printf("finished init\n");  
-            count ++;
-            break;            
+                shoot_phase = 1;
+                printf("finished waiting\n");  
+                break;
+                
+                case 1://装填動作
+                while(limit_1.read() == 0){
+                    motor_shoot.output(load_pwm);
+                    printf("loading\n");
+                    if(limit_2.read() == 1){
+                        break;
+                    }
+                    wait(0.1);
+                } 
+                motor_shoot.output(0);
+                shoot_phase = 2;
+                printf("finished loading\n");
+                wait(3); 
+                break;
+                
+                case 2://リボルバー回転,モーター復帰
+                turn_able = 1;
+                back_able = 1;
+                revolver_back(count);
+                shoot_phase = 3;
+                printf("finished backing\n"); 
+                wait(3);
+                break;
+                
+                case 3://発射動作
+                servo.pulsewidth_us(theta_45);
+                printf("%d shot\n",count);  
+                wait(3);
+                s
+                ervo.pulsewidth_us(theta_0);
+                motor_shoot.output(load_pwm);
+                wait(0.15);
+                motor_shoot.output(0);
+                shoot_phase = 0;
+                printf("finished init\n");  
+                count ++;
+                break;   
+                
+                //非常停止
+                case 10:
+                while(emergency == 1){
+                    motor_shoot.output(0);
+                    motor_revolver.output(0);
+                    wait(0.1);
+                }
+                break;         
+            }   
         }
     }
 }
\ No newline at end of file