Diff: main.cpp

Revision:

12:aef2a6626ea5

Parent:

11:62c85df03747

Child:

13:66e37e0aa786

--- a/main.cpp	Wed Jan 19 14:34:32 2022 +0000
+++ b/main.cpp	Thu Jan 20 13:06:53 2022 +0000
@@ -5,13 +5,14 @@
 #include "Motor.hpp"
 
 Motor motor_revolver(PB_4,PB_5);//リボルバーモーター
-Motor motor_shoot(PB_13,PB_14);//発射機構モーター
+Motor motor_shoot(PB_1,PA_11);//発射機構モーター
 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
+ScrpSlave slave(PC_12,PD_2,PH_1,SERIAL_TX,SERIAL_RX,0x0807f800);
 
-RotaryInc v(PA_14,PA_15,1,1024,4);//リボルバーロリコン
+//RotaryInc v(PA_14,PA_15,1,1024,4);//リボルバーロリコン
+RotaryInc v(PA_14,PA_15,1,1024,2);
 
 DigitalIn limit_1(PA_9);//リミットスイッチ１
 DigitalIn limit_2(PA_8);//リミットスイッチ２
@@ -19,12 +20,14 @@
 int shoot_phase;//発射フェーズ 0:待機　1:装填動作　2:リボルバー回転　3:モーター復帰　4:発射　
 bool turn_able;//回転可否　0:待機　1:回転
 
-double revolver_pwm = 0.1;//リボルバー回転速度
+double revolver_pwm = -0.03;//リボルバー回転速度
 double load_pwm = -0.3;//装填モーター速度
 double back_pwm = 0.3;//復帰モーター速度
 
-int theta_0 = 1450;//0度
-int theta_90 = 1875;//45度
+double turn = 0.0;
+
+int theta_0 = 1290;//0度
+int theta_45 = 1875;//45度
 int roricon = 0;//ロリコン
 
 bool interrupt(int rx_data,int &tx_data){
@@ -36,25 +39,27 @@
 
 void revolver(int count){
     if(turn_able == 1){
-        while(roricon < 171 * count){
+        while(roricon < 341 * count){
+        //while(1){
             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;//何発目か
+    shoot_phase = 0;
     
     servo.period_us(20000);
     limit_1.mode(PullUp);
     limit_2.mode(PullUp);
     
     slave.addCMD(2,interrupt);
+    
     while(1){
         switch (shoot_phase){
             
@@ -73,62 +78,47 @@
                 printf("loading\n");
                 wait(0.1);
             } 
-            motor_shoot.output(0.0);
-            wait(1);
-            //shoot_phase = 2;
-            shoot_phase = 3;
-            printf("finished loading\n"); 
+            motor_shoot.output(0);
+            shoot_phase = 2;
+            printf("finished loading\n");
+            wait(5); 
             break;
-            /*
+            
             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");  
             break;
-            */
+            
             case 3://モーター復帰
             while(limit_2.read() == 0){
                 motor_shoot.output(back_pwm);
                 printf("backing\n");
                 wait(0.1); 
             }
-            motor_shoot.output(0.0);
+            
+            motor_shoot.output(0);
             shoot_phase = 4;
             printf("finished backing\n"); 
             break;
             
             case 4://発射動作
-            servo.pulsewidth_us(theta_90);
+            wait(10);
+            servo.pulsewidth_us(theta_45);
             printf("finished shooting %d shot\n",count);  
             count ++;
-            wait(1);
+            wait(5);
             servo.pulsewidth_us(theta_0);
             shoot_phase = 0;
             printf("finished init\n");  
-            break;
-            
+            break;            
         }
     }
-}
-
-//リボルバーP制御関数
-//double k_p = 0.001;
-//double target = 171.0;
-/*void revolver(int count){
-    if(turn_able == 1){
-        roricon =  v.get() - (171 * (count - 1));
-            
-        while(roricon != 171){
-            pwm = k_p * (target - roricon);
-            motor_revolver.output(pwm);
-            printf("roricon = %lf\n",roricon);
-            printf("pwm = %lf\n",pwm);
-            wait(interval);
-        }      
-        motor_revolver.output(0.0);
-        turn_able = 0;  
-    }
-}*/
\ No newline at end of file
+}
\ No newline at end of file