Diff: main.cpp

Revision:

13:66e37e0aa786

Parent:

12:aef2a6626ea5

Child:

14:81f09ab5ed23

--- a/main.cpp	Thu Jan 20 13:06:53 2022 +0000
+++ b/main.cpp	Thu Jan 20 15:31:50 2022 +0000
@@ -17,7 +17,7 @@
 DigitalIn limit_1(PA_9);//リミットスイッチ１
 DigitalIn limit_2(PA_8);//リミットスイッチ２
 
-int shoot_phase;//発射フェーズ 0:待機　1:装填動作　2:リボルバー回転　3:モーター復帰　4:発射　
+int shoot_phase;//発射フェーズ 0:待機　1:装填動作　2:リボルバー回転、モーター復帰（同時）　3:発射　
 bool turn_able;//回転可否　0:待機　1:回転
 
 double revolver_pwm = -0.03;//リボルバー回転速度
@@ -37,16 +37,26 @@
     return true;  
 }
 
-void revolver(int count){
+void revolver_back(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);
+        while(roricon < 341 * count || limit_2.read() == 0){
+            if(roricon < 341 * count){
+                roricon = v.get();
+                motor_revolver.output(revolver_pwm);
+                printf("roricon = %d\n",roricon);    
+            }else{
+                motor_revolver.output(0);
+                turn_able = 0;
+                printf("finished turnning\n");  
+            }
+            
+            if(limit_2.read() == 0){
+                motor_shoot.output(back_pwm);
+                printf("backing\n");
+            }
+            
             wait(0.1);
         }
-        turn_able = 0;
     }
 }
 
@@ -84,33 +94,15 @@
             wait(5); 
             break;
             
-            case 2://リボルバー回転
+            case 2://リボルバー回転,モーター復帰
             turn_able = 1;
-            revolver(count);
-            if(turn_able == 0){
-                motor_revolver.output(0);
-                shoot_phase = 3;
-            }
-            else{
-                printf("error\n");
-            }
-            printf("finished turnning\n");  
+            revolver_back(count);
+            printf("finished backing\n"); 
+            shoot_phase = 3;
             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 = 4;
-            printf("finished backing\n"); 
-            break;
-            
-            case 4://発射動作
-            wait(10);
+            case 3://発射動作
+            wait(5);
             servo.pulsewidth_us(theta_45);
             printf("finished shooting %d shot\n",count);  
             count ++;