Diff: main.cpp

Revision:

22:97328720703c

Parent:

21:9f917ee199ff

Child:

23:a162feaea9ab

--- a/main.cpp	Tue Feb 01 12:21:48 2022 +0000
+++ b/main.cpp	Sun Mar 06 06:34:20 2022 +0000
@@ -5,7 +5,7 @@
 
 //-----インスタンス--------------------------
 
-Motor motor_revolver(PA_7,PA_8);//リボルバーモーター
+Motor motor_revolver(PA_8,PA_7);//リボルバーモーター
 Motor motor_shoot(PA_1,PA_3);//発射機構モーター
 PwmOut servo(PB_6);//ロック解除用サーボ
 
@@ -19,13 +19,10 @@
 //-----変数---------------------------------
 
 int shoot_phase = 0;//発射フェーズ 0:待機　1:装填動作　2:リボルバー回転、モーター復帰（同時）　3:発射　
-int shoot_phase_manual = 0;//発射フェーズ 0:待機　1:装填動作　2:リボルバー回転　3;モーター復帰　4:発射　
 bool turn_able = 0;//回転可否
 bool back_able = 0;//復帰可否　
 
-bool shoot_auto = 1;//手動発射か自動発射か　0:手動　1:自動
-bool mode = 0;//0:手動　1:自動
-
+bool shoot_start = 0;//発射開始　0:待機　1:開始済み
 bool emergency = 0;//0:停止　1:停止
 double revolver_pwm = -0.1;//リボルバー回転速度
 double load_pwm = -0.5;//装填モーター速度
@@ -37,62 +34,22 @@
 
 //-----関数---------------------------------
 
-//自動発射開始を受信
-bool get_all(int rx_data,int &tx_data){
-    if(shoot_phase == 0){//フェーズ0以外は、発射できないように
-        shoot_auto = 1;
+//発射開始を受信
+bool get_shoot(int rx_data,int &tx_data){
+    if(shoot_phase == 0 && shoot_start == 0){//フェーズ0以外は、発射できないように
+        shoot_start = 1;
         shoot_phase = 1;
     }
     return true;  
 }
 
-//装填動作受信
-bool get_load(int rx_data,int &tx_data){
-    shoot_auto = 0;
-    shoot_phase_manual = 1;
-    return true;  
-}
-
-//動作リボルバー受信
-bool get_revolver(int rx_data,int &tx_data){
-    shoot_auto = 0;
-    shoot_phase_manual = 2;
-    return true;  
-}
-
-//復帰動作受信
-bool get_back(int rx_data,int &tx_data){
-    shoot_auto = 0;
-    shoot_phase_manual = 3;
-    return true;  
-}
-
-//発射動作受信
-bool get_shoot(int rx_data,int &tx_data){
-    shoot_phase_manual = 4;
-    shoot_auto = 0;
-    return true;  
-}
-
-//モード変更を受信
-bool get_changemode(int rx_data,int &tx_data){
-    if(mode == 0){
-        mode = 1;
-    }else{
-        mode = 0;
-    }
-    return 0;
-}
-
 //非常停止を受信
 bool get_stop(int rx_data,int &tx_data){
     if(emergency == 0){
         emergency = 1;//停止
         shoot_phase = 10;
-    }else{
-        emergency = 0;//解除
     }
-    return 0;
+    return true;
 }
 
 //リボルバー、ラック復帰
@@ -137,20 +94,6 @@
     }
 }
 
-//リボルバー回転のみ
-void revolver(int count){
-    if(turn_able == 1){
-        while(roricon < 341 * count){
-        //while(1){
-            roricon = v.get();
-            motor_revolver.output(revolver_pwm);
-            printf("roricon = %d\n",roricon);
-            wait(0.01);
-        }
-        turn_able = 0;
-    }
-}
-
 //------------------------------------------
 
 int main(){
@@ -160,29 +103,27 @@
     limit_1.mode(PullUp);
     limit_2.mode(PullUp);
     
-    slave.addCMD(2,get_all);
-    slave.addCMD(3,get_load);
-    slave.addCMD(4,get_revolver);
-    slave.addCMD(5,get_back);
-    slave.addCMD(6,get_shoot);
-    
-    slave.addCMD(50,get_changemode);
+    slave.addCMD(2,get_shoot);
     slave.addCMD(51,get_stop);
     
     while(1){
 //----------自動発射------------------------------
-        if(mode == 1 && shoot_auto == 1){
-            switch (shoot_phase){
-                case 0://待機
+        switch (shoot_phase){
+            case 0://待機
                 while(shoot_phase == 0){
                     wait(0.1);
                     printf("waiting\n");  
                 }
-                shoot_phase = 1;
+                if(count < 7){
+                    shoot_phase = 1;
+                }else{
+                    emergency = 1;
+                    shoot_phase = 10;
+                }
                 printf("finished waiting\n");  
-                break;
+            break;
                 
-                case 1://装填動作
+            case 1://装填動作
                 while(limit_1.read() == 0){
                     motor_shoot.output(load_pwm);
                     printf("loading\n");
@@ -194,22 +135,22 @@
                 motor_shoot.output(0);
                 shoot_phase = 2;
                 printf("finished loading\n");
-                wait(3); 
-                break;
+                wait(0.1); 
+            break;
                 
-                case 2://リボルバー回転,モーター復帰
+            case 2://リボルバー回転,モーター復帰
                 turn_able = 1;
                 back_able = 1;
                 revolver_back(count);
                 shoot_phase = 3;
                 printf("finished backing\n"); 
-                wait(3);
-                break;
+                wait(0.5);
+            break;
                 
-                case 3://発射動作
+            case 3://発射動作
                 servo.pulsewidth_us(theta_45);
                 printf("%d shot\n",count);  
-                wait(3);
+                wait(1);
                 servo.pulsewidth_us(theta_0);
                 motor_shoot.output(load_pwm);
                 wait(0.15);
@@ -217,83 +158,16 @@
                 shoot_phase = 0;
                 printf("finished init\n");  
                 count ++;
-                break;   
+            break;   
                 
-                case 10://非常停止
+            case 10://非常停止
                 while(emergency == 1){
                     motor_shoot.output(0);
                     motor_revolver.output(0);
                     printf("emergency\n"); 
                     wait(0.1);
                 }
-                break;         
-            }   
-        }
-        
-//----------手動発射------------------------------
-        if(mode == 1 && shoot_auto == 0){
-            switch (shoot_phase_manual){
-                case 0://待機
-                while(shoot_phase_manual == 0){
-                    wait(0.1);
-                    printf("waiting\n");  
-                }
-                printf("finished waiting\n");  
-                break;
-                
-                case 1://装填動作
-                while(limit_1.read() == 0){
-                    motor_shoot.output(load_pwm);
-                    printf("loading\n");
-                    wait(0.1);
-                    if(limit_2.read() == 1){
-                        break;
-                    }
-                } 
-                motor_shoot.output(0);
-                shoot_phase_manual = 0;
-                printf("finished loading\n");
-                wait(5); 
-                break;
-                
-                case 2://リボルバー回転
-                turn_able = 1;
-                revolver(count);
-                if(turn_able == 0){
-                    motor_revolver.output(0);
-                    shoot_phase_manual = 0;
-                }
-                else{
-                    printf("error\n");
-                }
-                count ++;
-                printf("finished turnning\n");  
-                break;
-                
-                case 3://モーター復帰
-                while(limit_2.read() == 0){
-                    motor_shoot.output(back_pwm);
-                    printf("backing\n");
-                    wait(0.1); 
-                }
-                
-                motor_shoot.output(0);
-                shoot_phase_manual = 0;
-                printf("finished backing\n"); 
-                break;
-                
-                case 4://発射動作
-                servo.pulsewidth_us(theta_45);
-                printf("finished shooting %d shot\n",count);  
-                wait(5);
-                servo.pulsewidth_us(theta_0);
-                motor_shoot.output(load_pwm);
-                wait(0.15);
-                motor_shoot.output(0);
-                shoot_phase_manual = 0;
-                printf("finished init\n");  
-                break;      
-            }
-        }
+            break;         
+        }   
     }
 }
\ No newline at end of file