Diff: main.cpp

Revision:

9:6008a52fb6ef

Parent:

8:38db82c3492a

Child:

10:5e3c789e07dc

diff -r 38db82c3492a -r 6008a52fb6ef main.cpp
--- a/main.cpp	Mon Jan 17 02:45:48 2022 +0000
+++ b/main.cpp	Tue Jan 18 14:04:54 2022 +0000
@@ -4,35 +4,110 @@
 #include "scrp_slave.hpp"
 #include "Motor.hpp"
 
+Motor motor_revolver(PB_4,PB_5);//リボルバーモーター
+Motor motor_shoot(PB_13,PB_14);//発射機構モーター
+PwmOut servo(PB_8);//ロック解除用サーボ
+
 //ScrpSlave slave(PA_9,PA_10,PA_12,SERIAL_TX,SERIAL_RX,0x0803f800);//l432kc
 ScrpSlave slave(PC_12,PD_2,PH_1,SERIAL_TX,SERIAL_RX,0x0807f800);//f446re
 
-RotaryInc v(PA_14,PA_15,1,1024,4);
-
-Motor motor_revolver(PB_13,PB_14);//リボルバーモーター
-Motor motor_shoot(PA_11,PB_1);//発射機構モーター
+RotaryInc v(PA_14,PA_15,1,1024,4);//リボルバーロリコン
 
-PwmOut servo1(PB_8); //サーボ
-PwmOut servo2(PB_9);
+DigitalIn limit_1(PA_9);//リミットスイッチ１
+DigitalIn limit_2(PA_8);//リミットスイッチ２
 
-DigitalIn limit(dp16);
-
-int shoot_able;//発射フェーズ 0:待機　1:引き動作　2:リボルバー1/6回転　3:発射　
-int turn_able;//回転フェーズ　0:待機　1:1/6回転
+int shoot_able;//発射フェーズ 0:待機　1:装填動作　2:リボルバー回転　3:モーター復帰　4:発射　
+bool turn_able;//回転フェーズ　0:待機　1:回転
 
 double revolver_pwm = 0.1;//リボルバー回転速度
-double shoot_pwm = 0.1;//装填モーター速度
-int theta = 500;//サーボ回転角度
-double interval = 0.1;
-int roricon = 0;
+double load_pwm = -0.3;//装填モーター速度
+double back_pwm = 0.3;//復帰モーター速度
+
+int theta_0 = 1450;//0度
+int theta_90 = 2400;//90度
+int roricon = 0;//ロリコン
 
 bool interrupt(int rx_data,int &tx_data){
     if(shoot_able == 0){//フェーズ0以外は、発射できないように
-        shoot_able = rx_data;//1を受け取って引き動作開始 
+        shoot_able = rx_data;//1を受け取って装填動作開始 
     }
     return true;  
 }
 
+void revolver(int count){
+    if(turn_able == 1){
+        while(roricon < 171 * count){
+            roricon = v.get();
+            motor_revolver.output(revolver_pwm);
+            printf("roricon = %d\n",roricon);
+            wait(0.1);
+        }
+        motor_revolver.output(0.0);
+        turn_able = 0;
+    }
+}
+
+int main(){
+    int count = 1;//何発目か
+    
+    servo.period_us(20000);
+    
+    slave.addCMD(2,interrupt);
+    while(1){
+        switch (shoot_able){
+            
+            case 0://待機
+            while(shoot_able == 0){
+                wait(0.1);
+                printf("waiting\n");  
+            }
+            shoot_able = 1;
+            printf("finished waiting\n");  
+            break;
+            
+            case 1://装填動作
+            while(limit_1.read() == 0){
+                motor_shoot.output(shoot_pwm);
+                printf("loading\n"); 
+            }
+            motor_shoot.output(0.0);
+            shoot_able = 2;
+            printf("finished loading\n"); 
+            break;
+            
+            case 2://リボルバー回転
+            turn_able = 1;
+            revolver(count);
+            if(turn_able == 0){
+                shoot_able = 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;
+            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");  
+            break;
+            
+        } 
+    }
+}
+
 //リボルバーP制御関数
 //double k_p = 0.001;
 //double target = 171.0;
@@ -50,71 +125,4 @@
         motor_revolver.output(0.0);
         turn_able = 0;  
     }
-}*/
-
-void revolver(int count){
-    if(turn_able == 1){
-        while(roricon < 171 * count){
-            roricon = v.get();
-            motor_revolver.output(revolver_pwm);
-            printf("roricon = %d\n",roricon);
-            wait(interval);
-        }
-        motor_revolver.output(0.0);
-        turn_able = 0;
-    }
-}
-
-int main(){
-    int count = 1;//何発目か
-    
-    servo1.period_us(20000);
-    servo2.period_us(20000);
-    
-    slave.addCMD(2,interrupt);
-    while(1){
-        switch (shoot_able){
-            case 0:
-            //待機
-            while(shoot_able == 0){
-                wait(0.1);
-                printf("waiting\n");  
-            }
-            printf("finished waiting\n");  
-            break;
-            
-            case 1:
-            //装填動作コードをここに書く！
-            while(limit.read() == 0){
-                motor_shoot.output(shoot_pwm);
-            }
-            shoot_able = 2;
-            printf("finished pulling\n"); 
-            wait(5);
-            break;
-            
-            case 2:
-            wait(5);
-            //リボルバー回転
-            turn_able = 1;
-            revolver(count);
-            if(turn_able == 0){
-                shoot_able = 3;
-            }
-            printf("finished turnning\n");  
-            break;
-            
-            case 3:
-            wait(5);
-            //発射動作コードをここに書く！
-            servo1.pulsewidth_us(theta);
-            servo2.pulsewidth_us(theta);
-            shoot_able = 0;
-            printf("finished shooting %d\n",count);  
-            count ++;
-            servo1.pulsewidth_us(0);
-            servo2.pulsewidth_us(0);
-            break;
-        } 
-    }
-}
\ No newline at end of file
+}*/
\ No newline at end of file