Diff: main.cpp

Revision:

0:7a9ff37e6cc1

Child:

1:fddf1a6051b2

diff -r 000000000000 -r 7a9ff37e6cc1 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Mar 31 09:46:48 2022 +0000
@@ -0,0 +1,379 @@
+#include "mbed.h"
+#include "rotary_inc.hpp"
+#include "scrp_slave.hpp"
+#include "Motor.hpp"
+#include "mp3.hpp"
+
+//-----インスタンス--------------------------
+
+ScrpSlave slave(PC_12,PD_2,PH_1,SERIAL_TX,SERIAL_RX,5);//ID=5
+
+Playmp3 mp3(PA_0,PA_1);//mp3
+
+DigitalOut led[4] = {
+    DigitalOut(PA_10),
+    DigitalOut(PB_15),
+    DigitalOut(PB_2),
+    DigitalOut(PC_6)
+};
+
+//---右----------
+
+Motor R_motor_x(PB_6,PB_7);//水平軸モーター
+Motor R_motor_y(PB_9,PB_8);//垂直軸モーター
+
+RotaryInc R_v_distance(PA_7,PA_6);//水平軸ロリコン resolution:256
+RotaryInc R_v_height(PC_4,PA_13);//垂直軸ロリコン resolution:256
+
+DigitalIn R_limit(PB_4);//リミットスイッチ
+
+//---左----------
+
+Motor L_motor_x(PB_1,PA_11);//水平軸モーター
+Motor L_motor_y(PB_13,PB_14);//垂直軸モーター
+
+RotaryInc L_v_distance(PC_1,PC_0);//水平軸ロリコン resolution:256
+RotaryInc L_v_height(PC_5,PA_12);//垂直軸ロリコン resolution:256
+
+DigitalIn L_limit(PB_5);//リミットスイッチ
+
+//-----変数---------------------------------
+
+int get_phase = 10;//1:展開　2:下降　3:復帰（スロープ前） 4:スロープ　10:停止
+
+bool play = 0;
+
+//-----定数---------------------------------
+
+const double x_push_pwm = 0.35;//展開速度（水平軸）
+const double y_down_pwm = 0.2;//展開速度（垂直軸）
+
+const double x_back_pwm = -0.35;//復帰速度（水平軸）
+const double y_up_pwm = -0.2;//復帰速度（垂直軸）
+
+const int push_target = -380;//展開目標値
+const int down_target = -100;//下降目標値
+const int back_target = -50;//復帰目標値
+const int initX_target = 0;
+const int slope_x_target = -45;//スロープ目標値（水平軸）
+const int slope_y_target = -75;//スロープ目標値（垂直軸）
+
+//-----関数---------------------------------
+
+//通信受信
+bool get_serial(int rx_data,int &tx_data){
+    if(get_phase == 10){
+        get_phase = 0;
+    }
+    return true;
+}
+
+/*bool get_get(int rx_data,int &tx_data){
+    if(rx_data == 1){
+        if(get_phase == 0){
+            get_phase = 100;    
+        }   
+    }
+    return true;
+} */
+
+//音声再生、回収動作受信
+bool get_mp3(int rx_data,int &tx_data){
+    switch(rx_data){
+        
+        case 1://ビーンバッグ回収
+            mp3.set_number_of_tracks(3);
+            if(get_phase == 0){
+                get_phase = 100;    
+            }
+            
+        break;
+        
+        /*case 1://農作物回収
+            mp3.set_number_of_tracks(1);
+        break;*/
+        
+        case 2://農作物設置
+            mp3.set_number_of_tracks(2);
+        break;   
+        
+        case 3://農作物回収
+             mp3.set_number_of_tracks(1);
+        break;
+        
+        case 4://自動運転モード移行
+            if(play == 0){
+                mp3.set_number_of_tracks(4);
+                play = 1;
+            }
+        break;
+        
+        case 5://ビーンバッグ発射
+            mp3.set_number_of_tracks(5);
+        break;
+    }
+    return true;
+}
+
+//LED
+void turn_led(bool status){
+    if(status == 0){
+        led[0].write(0);
+        led[1].write(0);
+        led[2].write(0);
+        led[3].write(0);
+    }else if(status == 1){
+        led[0].write(1);
+        led[1].write(1);
+        led[2].write(1);
+        led[3].write(1);
+    }
+}
+
+//------------------------------------------
+
+int main(){
+    
+    mp3.set_volume(255);
+    mp3.quiet_mode(false);
+    
+    R_limit.mode(PullUp);
+    L_limit.mode(PullUp);
+    
+    //slave.addCMD(1,get_get);
+    slave.addCMD(1,get_mp3);
+    slave.addCMD(51,get_serial);
+    
+    int R_distance;
+    int R_height;
+    
+    int L_distance;
+    int L_height;
+    
+    int count = 0;//回収動作受信回数
+    
+    while(1){
+        
+//----------回収------------------------------
+        switch (get_phase){
+            case 0://待機
+                turn_led(1);
+                
+                while(get_phase == 0){
+                    wait(0.1);
+                    printf("waiting\n");  
+                }
+                
+                printf("finished waiting\n"); 
+            break;
+                
+            case 1://展開動作
+                turn_led(0);
+                
+                while(R_distance >= push_target || L_distance <= -push_target){
+                    R_distance = R_v_distance.get();
+                    L_distance = L_v_distance.get();
+                    //右
+                    if(R_distance >= push_target){
+                        printf("R_roricon = %d\n",R_distance);
+                        R_motor_x.output(x_push_pwm);
+                    }else{
+                        R_motor_x.output(0);
+                    }
+                    wait(0.01);
+                    //左
+                    if(L_distance <= -push_target){
+                        printf("L_roricon = %d\n",L_distance);
+                        L_motor_x.output(-x_push_pwm);
+                    }else{
+                        L_motor_x.output(0);
+                    }
+                    wait(0.01);
+                }
+
+                R_motor_x.output(0);
+                L_motor_x.output(0);
+                printf("finished pushing\n");
+                get_phase = 2;
+                
+            break;
+                
+            case 2://下降動作
+                while(R_height >= down_target || L_height >= down_target){
+                    //右
+                    if(R_height >= down_target){
+                        R_height = R_v_height.get();
+                        printf("R_roricon = %d\n",R_height);
+                        R_motor_y.output(y_down_pwm);
+                    }else{
+                        R_motor_y.output(0);
+                    }
+                    wait(0.01);
+                    //左
+                    if(L_height >= down_target){
+                        L_height = L_v_height.get();
+                        printf("L_roricon = %d\n",L_height);
+                        L_motor_y.output(y_down_pwm);
+                    }else{
+                        L_motor_y.output(0);
+                    }
+                    wait(0.01);
+                }
+                
+                R_motor_y.output(0);
+                L_motor_y.output(0);
+                printf("finished downing\n");
+                get_phase = 0;
+                
+            break; 
+            
+            case 3://復帰動作（スロープ前）
+            
+                turn_led(0);
+                
+                while(R_distance <= back_target || L_distance >= -back_target){
+                    //右
+                    if(R_distance <= back_target){
+                        R_distance = R_v_distance.get();
+                        printf("R_roricon = %d\n",R_distance);
+                        R_motor_x.output(x_back_pwm);
+                    }else{
+                        R_motor_x.output(0);
+                    }
+                    //左
+                    if(L_distance >= -back_target){
+                        L_distance = L_v_distance.get();
+                        printf("L_roricon = %d\n",L_distance);
+                        L_motor_x.output(-x_back_pwm);
+                    }else{
+                        L_motor_x.output(0);
+                    }
+                    wait(0.01);
+                }
+                
+                //R_motor_x.output(0);
+                //L_motor_x.output(0);
+                printf("finished backing\n");
+                get_phase = 4;
+                
+            break; 
+            
+            case 4://復帰動作（スロープ）
+             
+                while(R_distance <= slope_x_target || R_height <= slope_y_target || L_distance >= -slope_x_target || L_height >= -slope_y_target){
+                    //右
+                    if(R_distance <= slope_x_target){
+                        R_distance = R_v_distance.get();
+                        R_motor_x.output(x_back_pwm);
+                        printf("backing\n");  
+                    }else{
+                        R_motor_x.output(0);
+                    } 
+                    if(R_height <= slope_y_target){
+                        R_height = R_v_height.get();
+                        R_motor_y.output(y_up_pwm);
+                    }else{
+                        R_motor_y.output(0);
+                    } 
+                    wait(0.01);
+                    //左
+                    if(L_distance >= -slope_x_target){
+                        L_distance = L_v_distance.get();
+                        L_motor_x.output(-x_back_pwm);
+                        printf("backing\n");  
+                    }else{
+                        L_motor_x.output(0);
+                    } 
+                    if(L_height <= slope_y_target){
+                        L_height = L_v_height.get();
+                        L_motor_y.output(y_up_pwm);
+                    }else{
+                        L_motor_y.output(0);
+                    } 
+                    wait(0.01);  
+                }
+                R_motor_x.output(0);
+                R_motor_y.output(0);
+                L_motor_x.output(0);
+                L_motor_y.output(0);
+                
+                get_phase = 0;
+             
+            break;
+            
+            case 5://初期位置復帰
+                turn_led(0);
+                while(R_distance <= initX_target || L_distance >= -initX_target){
+                    //右
+                    if(R_distance <= initX_target){
+                        R_distance = R_v_distance.get();
+                        printf("R_roricon = %d\n",R_distance);
+                        R_motor_x.output(x_back_pwm);
+                    }else{
+                        R_motor_x.output(0);
+                    }
+                    //左
+                    if(L_distance >= -initX_target){
+                        L_distance = L_v_distance.get();
+                        printf("L_roricon = %d\n",L_distance);
+                        L_motor_x.output(-x_back_pwm);
+                    }else{
+                        L_motor_x.output(0);
+                    }
+                    wait(0.01);
+                }
+                
+                R_motor_x.output(0);
+                L_motor_x.output(0);
+                printf("finished backing\n");
+                
+                get_phase = 6; 
+                
+            break;
+            
+            case 6:
+                while(R_limit.read() == 0 || L_limit.read() == 0){
+                    //右
+                    if(R_limit.read() == 0){
+                        R_motor_y.output(y_up_pwm);
+                        printf("upping slope\n");
+                    }else{
+                        printf("finished uping slope\n");
+                        R_motor_y.output(0);
+                    }
+                    wait(0.01);
+                    //左
+                    if(L_limit.read() == 0){
+                        L_motor_y.output(y_up_pwm);
+                        printf("upping slope\n");
+                    }else{
+                        printf("finished uping slope\n");
+                        L_motor_y.output(0);
+                    }
+                    
+                    wait(0.01);
+                    
+                }
+                L_motor_x.output(0);
+                L_motor_y.output(0);
+                get_phase = 0;
+                
+                printf("finished slope\n");
+            break;
+            
+            case 100://フェーズ判定
+                count ++;
+                if(count == 1){
+                    get_phase = 1;
+                }
+                if(count == 2){
+                    get_phase = 3;   
+                }
+                if(count == 3){
+                    get_phase = 5;
+                    count = 0;    
+                }
+            break;
+        }   
+    }
+}
\ No newline at end of file