Revision 6:ff7fd6556a81, committed 2018-04-01

Comitter:

kenken0721

Date:

Sun Apr 01 11:58:23 2018 +0000

Parent:

5:c5a2a5cf600d

Commit message:

Changed in this revision

diff -r c5a2a5cf600d -r ff7fd6556a81 config.h
--- a/config.h	Sun Apr 01 04:11:13 2018 +0000
+++ b/config.h	Sun Apr 01 11:58:23 2018 +0000
@@ -40,9 +40,3 @@
 #define STAND_BY_1      12
 #define STAND_BY_2      13
 #define THROWING        14
-#define MANUAL          15
-#define NOT_RECEIVE     16
-#define START_TEST      17
-#define STAND_BY_TEST   18
-#define THROWING_TEST   19
-#define NOTHING         20

diff -r c5a2a5cf600d -r ff7fd6556a81 main.cpp
--- a/main.cpp	Sun Apr 01 04:11:13 2018 +0000
+++ b/main.cpp	Sun Apr 01 11:58:23 2018 +0000
@@ -25,7 +25,7 @@
 Serialservo servo1(PC_10, PC_11);//アームの肘
 Serialservo servo2(PC_10, PC_11);//回転軸
 XQ_servo servo_hand(PB_5);
-DigitalOut dpin(PC_9);
+DigitalOut throwing_relay(PC_9);
 DigitalIn distance(PC_2);
 DigitalIn redIn(PC_4);
 DigitalIn blueIn(PB_2);
@@ -36,13 +36,6 @@
 DigitalOut motor_dir2(PA_9);
 PwmOut motor_pwm(PB_3);
 
-void linecheck(char *buff ,int data[2]);
-void LineCheck(int dmode);
-void syokikanomai(void);
-void syokikanomai2(void);
-void pre_touteki(void);
-void touteki(float armpower, int pos, bool thro_mode);
-
 PwmOut pwm_pins[] = {
     PwmOut( PC_8 ),
     PwmOut( PC_6 ),
@@ -64,6 +57,13 @@
     DigitalOut( PB_15)
 };
 
+
+void linecheck(char *buff ,int data[2]);
+void LineCheck(int dmode);
+void syokikanomai(void);
+void syokikanomai2(void);
+void touteki(float armpower, int pos, bool thro_mode);
+
 int crosscount = 0;
 bool mode = true;//trueでラジコン、falseでオート
 bool stopflag = true;//trueで機体停止
@@ -89,8 +89,6 @@
 int directmode = STRAIGHT;
 int state = START;
 int times = 0;
-bool throwing = false;
-int receive = NOT_RECEIVE;
 int redcount = 0;
 int bluecount = 0;
 int yellowcount = 0;
@@ -101,19 +99,16 @@
 int main() {
     timer.reset();
     timer.stop();
-    servo1.init(0);//アーム
-    servo2.init(1);//回転軸
-    dpin=0;
+    throwing_relay=0;
     lamp=1;
     air=1.0;
     buzzer = 0.0;
-    //motor_relay = 0.0;
     micro_switch.mode(PullUp);
-    
     motor_pwm=0;
-    motor_dir1=1;     //dir1,dir2はこの組み合わせじゃないと死ぬ
+    motor_dir1=1;
     motor_dir2=1;
-    
+    servo1.init(0);//アーム
+    servo2.init(1);//回転軸
     S_pid.init();
     R_pid.init();
     L_pid.init();
@@ -122,14 +117,15 @@
     pwm_pins[2].period(0.00005);
     pwm_pins[3].period(0.00005);
     motor_pwm.period(0.00005);
+//------通信スピード----------------------------------------------
     pc.baud(115200);
     kbtpc.baud(9600);
     master.frequency(100000);
-    lamp = 1.0;    
-//-------初期化----------------------------------------
+    lamp = 1.0;
+//-------初期化-------------------------------------------------
     syokikanomai();
     buzzer = 0;
-//-----------メインループ----------------------------------------め
+//-----------メインループ----------------------------------------
     while (true) {
         master.read(addr1,buff1,2);
         linecheck(buff1,linedata_1);
@@ -157,12 +153,8 @@
             directmode = RIGHT;
         }else if(kbtread == LEFT){
             directmode = LEFT;
-        }else if(kbtread == MANUAL){
-            state = MANUAL;
         }else if(kbtread == START){
             state = START;
-        }else if(kbtread == START_TEST){
-            state = START_TEST;
         }
 //--------------手動--------------------------------------
         if(mode == true){
@@ -186,82 +178,17 @@
             }else{
                 LineCheck(directmode);
                 switch(state){
-//------------------任意に進行方向を決めれる----------------------------------------
-                    case MANUAL:
-                    if(crosscount == 1){
-                        directmode = STOP;
-                        Stop(pwm_pins, dir_pins_1, dir_pins_2);
-                        crosscount = 0;
-                    }
-                    break;
-//------------------テスト用--------------------------------------------------------
-                    case START_TEST:
-                    if(crosscount == 0){
-                        directmode = RIGHT;
-                    }else if(crosscount == 1){
-                        directmode = STOP;
-                        state = STAND_BY_TEST;
-                        crosscount = 0;
-                        prelinedata = 0;
-                    }
-                    break;
-//------------------テスト用待機位置-----------------------------------------------
-                    case STAND_BY_TEST:
-                    directmode = STOP;
-                    if(distance == 1.0){
-                        led2 = 1.0;
-                        if(redIn == 1.0){
-                            buzzer = 1.0;
-                            redcount++;
-                        }else if(blueIn == 1.0){
-                            buzzer = 1.0;
-                            bluecount++;
-                        }else{
-                            buzzer = 0.0;
-                        }
-                        if(redcount >= 50){
-                            servo_hand.move(hand_tsukami_degree);
-                            //pre_touteki();
-                        }else if(bluecount >= 50){
-                            servo_hand.move(hand_tsukami_degree);
-                            //pre_touteki();
-                        }
-                    }else if(distance == 0.0 && redcount >= 50){
-                        servo2.move(koshi_nage_degree);
-                        state = THROWING_TEST;
-                        directmode = STOP;
-                        led2 = 0.0;
-                        redcount = 0;
-                        crosscount = 0;
-                    }else if(distance == 0.0 && bluecount >= 50){
-                        servo2.move(koshi_nage_degree);
-                        state = THROWING_TEST;
-                        directmode = STOP;
-                        bluecount = 0;
-                        led2 = 0.0;
-                        crosscount = 0;
-                    }
-                    break;
-//------------------------------------------------------------------------------
-                    case THROWING_TEST:
-                    if(crosscount == 0){
-                        directmode = LEFT;
-                    }else if(crosscount == 1){
-                        directmode = STOP;
-                        touteki(0.8, 190, 0);
-                        //syokikanomai();
-                        buzzer = 0.0;
-                    }
-                    break;
 //------------------初期位置から待機位置1------------------------------------------
                     case START:
                     if(crosscount == 0){
                         directmode = STRAIGHT;
-                    }else if(crosscount == 1 && directmode == STRAIGHT){
-                        directmode = LEFT;
-                        prelinedata = 0;
-                    }else if(crosscount == 1 && directmode == LEFT){
-                        directmode = LEFT;
+                    }else if(crosscount == 1){
+                        if(directmode == STRAIGHT){
+                            directmode = LEFT;
+                            prelinedata = 0;
+                        }else if(directmode == LEFT){
+                            directmode = LEFT;
+                        }
                     }else if(crosscount == 2){
                         directmode = STOP;
                         state = STAND_BY_1;
@@ -286,12 +213,15 @@
                         if(redcount >= 100 ){
                             servo_hand.move(hand_tsukami_degree);
                             servo1.move(hiji_tsukami_degree);
+                            servo1.move(hiji_tsukami_degree);
                         }else if(bluecount >= 100){
                             servo_hand.move(hand_tsukami_degree);
                             servo1.move(hiji_tsukami_degree);
+                            servo1.move(hiji_tsukami_degree);
                         }
                     }else if(distance == 0.0 && redcount >= 100){
                         servo2.move(koshi_nage_degree);
+                        servo2.move(koshi_nage_degree);
                         state = RED_RECEIVE;
                         directmode = STOP;
                         redcount = 0;
@@ -319,7 +249,6 @@
                         syokikanomai2();
                         if(set_flag == true){
                             state = STAND_BY_1;
-                            //throwing = false;
                             crosscount = 0;
                         }
                     }
@@ -356,11 +285,13 @@
                         if(bluecount >= 100){
                             servo_hand.move(hand_tsukami_degree);
                             servo1.move(hiji_tsukami_degree);
+                            servo1.move(hiji_tsukami_degree);
                         }else if(yellowcount >= 500){
                             air = 0.0;
                         }
                     }else if(distance == 0.0 && bluecount >= 100){
                         servo2.move(koshi_nage_degree);
+                        servo2.move(koshi_nage_degree);
                         state = BLUE_RECEIVE_2;
                         directmode = STOP;
                         bluecount = 0;
@@ -467,22 +398,18 @@
 }
 
 //---------------初期化---------------------------
-
-
 void syokikanomai(void){
-    //timer.start();
-    //int times = timer.read();
     syokika_flag = false;
     while (set_flag==false) {
         encoder_count= Enc_Arm.read_rotate(); 
         servo_hand.move(hand_uke_degree);
         servo1.move(hiji_nage_degree);//退避
         servo2.move(koshi_uke_degree);
-        //motor_relay = 0;
+        servo1.move(hiji_nage_degree);//退避
+        servo2.move(koshi_uke_degree);
         motor_dir1=0;
         motor_dir2=1;
         motor_pwm=0.15;
- 
         if(Enc_Arm.read_z() == 1) {
             syokika_flag=true;
             buzzer = 1.0;
@@ -497,34 +424,25 @@
             }
         }
     }
-    //if(times >= 1.0){
-    //wait(0.5);
-    //servo_hand.move(hand_uke_degree);
     servo1.move(hiji_uke_degree);
     servo1.move(hiji_uke_degree);
-    //servo2.move(koshi_uke_degree);
-    //}
 }
 
 void syokikanomai2(void){
-    //timer.start();
-    //int times = timer.read();
-    //syokika_flag = false;
     if(set_flag==false) {
         encoder_count= Enc_Arm.read_rotate(); 
         servo_hand.move(hand_uke_degree);
         servo1.move(hiji_nage_degree);//退避
         servo2.move(koshi_uke_degree);
-        //motor_relay = 0;
+        servo1.move(hiji_nage_degree);//退避
+        servo2.move(koshi_uke_degree);
         motor_dir1=0;
         motor_dir2=1;
         motor_pwm=0.15;
- 
         if(Enc_Arm.read_z() == 1) {
             syokika_flag = true;
             buzzer = 1.0;
         }
-        
         if (syokika_flag==true) {
             if(encoder_count == 170) {
                 motor_pwm=0;
@@ -532,54 +450,23 @@
                 motor_dir2=1;
                 servo1.move(hiji_uke_degree);
                 servo1.move(hiji_uke_degree);
-                //syokika_flag = false;
                 set_flag = true;  //投てき後，falseにする.
             }
         }
     }
-    //if(times >= 1.0){
-    //wait(0.5);
-    //servo_hand.move(hand_uke_degree);
-    //servo2.move(koshi_uke_degree);
-    //}
 }
 
-void pre_touteki(void){
-    /*
-    while(Enc_Arm.read_rotate()<pos) {
-        motor_dir1 = 0;
-        motor_dir2 = 1;
-        motor_pwm = 0.15;
-    }
-    motor_dir1=1;
-    motor_dir2=1;
-    motor_pwm=0;
-    */
-    //wait(1.0);
-    servo_hand.move(hand_tsukami_degree);
-    
-    //wait(1.0);
-    servo2.move(koshi_nage_degree);
-    //servo1.move(hiji_tsukami_degree);
-}
-
-void touteki(float armpower ,int pos, bool thro_mode){
+void touteki(float armpower ,int pos, bool throwing_mode){
     if(set_flag == true){
         timer.start();
     }
     int times = timer.read(); 
-    //wait(1.0);
     if(times >= 1.0 && times < 2.0){
     servo_hand.move(hand_uke_degree);
-    }
-    //wait(1.0);
-    //lamp = 0.0;
-    
+    }    
     if(times >= 2.0 && times < 3.0){
     servo1.move(hiji_nage_degree);
     }
-    
-    //wait(1.0);
     if(times >= 3.0 && times < 5.0){
         lamp = 0.0;
         while(Enc_Arm.read_rotate() < pos) {
@@ -591,22 +478,20 @@
     motor_dir1=1;
     motor_dir2=1;
     motor_pwm=0;
-    //wait(2.0);
 //----投てき-------------------------
-    if(thro_mode == 0.0){
+    if(throwing_mode == 0.0){
         if(times >= 5.0 && times < 6.0){
             motor_dir1 = 0;
             motor_dir2 = 1;
             motor_pwm = armpower;
         }
-    }else if(thro_mode == 1.0){
+    }else if(throwing_mode == 1.0){
         if(times >= 5.0 && times < 5.5){
-            dpin = 1.0;
+            throwing_relay = 1.0;
         }else{
-            dpin = 0.0;
+            throwing_relay = 0.0;
         }
     }
-    //wait(1.0);
     if(times >= 6.0 && times <7.0){
         motor_pwm = 0.0;
         motor_dir1 = 0;