Diff: main.cpp

Revision:

21:9f917ee199ff

Parent:

20:4db5f042a45c

--- a/main.cpp	Tue Feb 01 07:22:36 2022 +0000
+++ b/main.cpp	Tue Feb 01 12:21:48 2022 +0000
@@ -1,4 +1,4 @@
-d#include "mbed.h"
+#include "mbed.h"
 #include "rotary_inc.hpp"
 #include "scrp_slave.hpp"
 #include "Motor.hpp"
@@ -19,10 +19,13 @@
 //-----変数---------------------------------
 
 int shoot_phase = 0;//発射フェーズ 0:待機　1:装填動作　2:リボルバー回転、モーター復帰（同時）　3:発射　
+int shoot_phase_manual = 0;//発射フェーズ 0:待機　1:装填動作　2:リボルバー回転　3;モーター復帰　4:発射　
 bool turn_able = 0;//回転可否
 bool back_able = 0;//復帰可否　
 
+bool shoot_auto = 1;//手動発射か自動発射か　0:手動　1:自動
 bool mode = 0;//0:手動　1:自動
+
 bool emergency = 0;//0:停止　1:停止
 double revolver_pwm = -0.1;//リボルバー回転速度
 double load_pwm = -0.5;//装填モーター速度
@@ -34,14 +37,43 @@
 
 //-----関数---------------------------------
 
-//発射開始を受信
-bool get_start(int rx_data,int &tx_data){
+//自動発射開始を受信
+bool get_all(int rx_data,int &tx_data){
     if(shoot_phase == 0){//フェーズ0以外は、発射できないように
+        shoot_auto = 1;
         shoot_phase = 1;
     }
     return true;  
 }
 
+//装填動作受信
+bool get_load(int rx_data,int &tx_data){
+    shoot_auto = 0;
+    shoot_phase_manual = 1;
+    return true;  
+}
+
+//動作リボルバー受信
+bool get_revolver(int rx_data,int &tx_data){
+    shoot_auto = 0;
+    shoot_phase_manual = 2;
+    return true;  
+}
+
+//復帰動作受信
+bool get_back(int rx_data,int &tx_data){
+    shoot_auto = 0;
+    shoot_phase_manual = 3;
+    return true;  
+}
+
+//発射動作受信
+bool get_shoot(int rx_data,int &tx_data){
+    shoot_phase_manual = 4;
+    shoot_auto = 0;
+    return true;  
+}
+
 //モード変更を受信
 bool get_changemode(int rx_data,int &tx_data){
     if(mode == 0){
@@ -105,6 +137,20 @@
     }
 }
 
+//リボルバー回転のみ
+void revolver(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);
+            wait(0.01);
+        }
+        turn_able = 0;
+    }
+}
+
 //------------------------------------------
 
 int main(){
@@ -114,12 +160,18 @@
     limit_1.mode(PullUp);
     limit_2.mode(PullUp);
     
-    slave.addCMD(2,get_start);
+    slave.addCMD(2,get_all);
+    slave.addCMD(3,get_load);
+    slave.addCMD(4,get_revolver);
+    slave.addCMD(5,get_back);
+    slave.addCMD(6,get_shoot);
+    
     slave.addCMD(50,get_changemode);
     slave.addCMD(51,get_stop);
     
     while(1){
-        if(mode == 1){
+//----------自動発射------------------------------
+        if(mode == 1 && shoot_auto == 1){
             switch (shoot_phase){
                 case 0://待機
                 while(shoot_phase == 0){
@@ -177,5 +229,71 @@
                 break;         
             }   
         }
+        
+//----------手動発射------------------------------
+        if(mode == 1 && shoot_auto == 0){
+            switch (shoot_phase_manual){
+                case 0://待機
+                while(shoot_phase_manual == 0){
+                    wait(0.1);
+                    printf("waiting\n");  
+                }
+                printf("finished waiting\n");  
+                break;
+                
+                case 1://装填動作
+                while(limit_1.read() == 0){
+                    motor_shoot.output(load_pwm);
+                    printf("loading\n");
+                    wait(0.1);
+                    if(limit_2.read() == 1){
+                        break;
+                    }
+                } 
+                motor_shoot.output(0);
+                shoot_phase_manual = 0;
+                printf("finished loading\n");
+                wait(5); 
+                break;
+                
+                case 2://リボルバー回転
+                turn_able = 1;
+                revolver(count);
+                if(turn_able == 0){
+                    motor_revolver.output(0);
+                    shoot_phase_manual = 0;
+                }
+                else{
+                    printf("error\n");
+                }
+                count ++;
+                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);
+                shoot_phase_manual = 0;
+                printf("finished backing\n"); 
+                break;
+                
+                case 4://発射動作
+                servo.pulsewidth_us(theta_45);
+                printf("finished shooting %d shot\n",count);  
+                wait(5);
+                servo.pulsewidth_us(theta_0);
+                motor_shoot.output(load_pwm);
+                wait(0.15);
+                motor_shoot.output(0);
+                shoot_phase_manual = 0;
+                printf("finished init\n");  
+                break;      
+            }
+        }
     }
 }
\ No newline at end of file