Diff: main.cpp

Revision:

16:04f42eeb9561

Parent:

15:7c26b01dcefd

Child:

17:9ee4c69d2d8b

--- a/main.cpp	Tue Jan 25 23:56:51 2022 +0000
+++ b/main.cpp	Wed Jan 26 16:16:41 2022 +0000
@@ -19,6 +19,7 @@
 
 int shoot_phase;//発射フェーズ 0:待機　1:装填動作　2:リボルバー回転、モーター復帰（同時）　3:発射　
 bool turn_able;//回転可否　0:待機　1:回転
+bool back_able;
 
 double revolver_pwm = -0.03;//リボルバー回転速度
 double load_pwm = -0.3;//装填モーター速度
@@ -38,27 +39,43 @@
 }
 
 void revolver_back(int count){
-    if(turn_able == 1){
-        while(roricon < 341 * (count - 1) || limit_2.read() == 0){
-            if(count != 1){
-                if(roricon < 341 * (count - 1)){
+    while(turn_able == 1 || back_able == 1){
+        //リボルバー
+        if(count == 1){
+            motor_revolver.output(0);
+            turn_able = 0;
+            printf("finished turnning\n");  
+        }
+        if(count != 1){
+            if(roricon < 341 * (count - 1)){
+                if(turn_able == 1){
                     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");  
+                    printf("roricon = %d\n",roricon); 
                 }    
             }
-            
-            if(limit_2.read() == 0){
+            else{
+                motor_revolver.output(0);
+                turn_able = 0;   
+            } 
+        }     
+        //復帰   
+        if(limit_2.read() == 0){
+            if(back_able == 1){
                 motor_shoot.output(back_pwm);
-                printf("backing\n");
+                printf("backing\n");    
             }
-            
-            wait(0.01);
+        }
+        if(limit_2.read() == 1){
+            motor_shoot.output(0);
+            back_able = 0;
+            printf("finished backing\n");
         }
+        //ループ抜け出し   
+        if(turn_able == 0 && turn_able == 0){
+            break;
+        }
+        wait(0.01);
     }
 }
 
@@ -96,25 +113,26 @@
             motor_shoot.output(0);
             shoot_phase = 2;
             printf("finished loading\n");
-            wait(5); 
+            wait(3); 
             break;
             
             case 2://リボルバー回転,モーター復帰
             turn_able = 1;
+            back_able = 1;
             revolver_back(count);
             printf("finished backing\n"); 
             shoot_phase = 3;
             break;
             
             case 3://発射動作
-            wait(5);
+            wait(3);
             servo.pulsewidth_us(theta_45);
             printf("finished shooting %d shot\n",count);  
             count ++;
-            wait(5);
+            wait(3);
             servo.pulsewidth_us(theta_0);
             motor_shoot.output(load_pwm);
-            wait(0.1);
+            wait(0.15);
             motor_shoot.output(0);
             shoot_phase = 0;
             printf("finished init\n");