Diff: main.cpp

Revision:

10:5e3c789e07dc

Parent:

9:6008a52fb6ef

Child:

11:62c85df03747

--- a/main.cpp	Tue Jan 18 14:04:54 2022 +0000
+++ b/main.cpp	Wed Jan 19 09:05:16 2022 +0000
@@ -16,20 +16,20 @@
 DigitalIn limit_1(PA_9);//リミットスイッチ１
 DigitalIn limit_2(PA_8);//リミットスイッチ２
 
-int shoot_able;//発射フェーズ 0:待機　1:装填動作　2:リボルバー回転　3:モーター復帰　4:発射　
-bool turn_able;//回転フェーズ　0:待機　1:回転
+int shoot_phase;//発射フェーズ 0:待機　1:装填動作　2:リボルバー回転　3:モーター復帰　4:発射　
+bool turn_able;//回転可否　0:待機　1:回転
 
 double revolver_pwm = 0.1;//リボルバー回転速度
 double load_pwm = -0.3;//装填モーター速度
 double back_pwm = 0.3;//復帰モーター速度
 
 int theta_0 = 1450;//0度
-int theta_90 = 2400;//90度
+int theta_90 = 1875;//45度
 int roricon = 0;//ロリコン
 
 bool interrupt(int rx_data,int &tx_data){
-    if(shoot_able == 0){//フェーズ0以外は、発射できないように
-        shoot_able = rx_data;//1を受け取って装填動作開始 
+    if(shoot_phase == 0){//フェーズ0以外は、発射できないように
+        shoot_phase = rx_data;//1を受け取って装填動作開始 
     }
     return true;  
 }
@@ -51,57 +51,64 @@
     int count = 1;//何発目か
     
     servo.period_us(20000);
+    limit_1.mode(PullUp);
+    limit_2.mode(PullUp);
     
     slave.addCMD(2,interrupt);
     while(1){
-        switch (shoot_able){
+        switch (shoot_phase){
             
             case 0://待機
-            while(shoot_able == 0){
+            while(shoot_phase == 0){
                 wait(0.1);
                 printf("waiting\n");  
             }
-            shoot_able = 1;
+            shoot_phase = 1;
             printf("finished waiting\n");  
             break;
             
             case 1://装填動作
             while(limit_1.read() == 0){
-                motor_shoot.output(shoot_pwm);
+                motor_shoot.output(load_pwm);
                 printf("loading\n"); 
+                if(limit_1.read() == 1){
+                    break;
+                }
             }
             motor_shoot.output(0.0);
-            shoot_able = 2;
+            wait(1);
+            //shoot_phase = 2;
+            shoot_phase = 3;
             printf("finished loading\n"); 
             break;
-            
+            /*
             case 2://リボルバー回転
             turn_able = 1;
             revolver(count);
             if(turn_able == 0){
-                shoot_able = 3;
+                shoot_phase = 3;
             }
             printf("finished turnning\n");  
             break;
-            
+            */
             case 3://モーター復帰
             while(limit_2.read() == 0){
                 motor_shoot.output(back_pwm);
                 printf("backing\n"); 
             }
             motor_shoot.output(0.0);
-            shoot_able = 4;
+            shoot_phase = 4;
             printf("finished backing\n"); 
             break;
             
             case 4://発射動作
             servo.pulsewidth_us(theta_90);
-            shoot_able = 0;
             printf("finished shooting %dst shot\n",count);  
             count ++;
             wait(1);
             servo.pulsewidth_us(theta_0);
-            printf("finished initialize %d\n");  
+            shoot_phase = 0;
+            printf("finished initialize\n");  
             break;
             
         }