Diff: main.cpp

Revision:

1:3ae63be5592b

Parent:

0:111abd91b0cb

--- a/main.cpp	Fri Dec 14 10:49:59 2018 +0000
+++ b/main.cpp	Mon Dec 17 10:22:06 2018 +0000
@@ -1,3 +1,4 @@
+//2018/12/15 腰回転をシリンダに変更
 #include "mbed.h"
 #include "EC.h"
 #include "KondoServo.h"
@@ -7,15 +8,15 @@
 Timer timer;
 
 //パラメタ
-double dutylimit = 0.6; // duty出力の上限値
+double dutylimit = 0.4; // duty出力の上限値
 int resolution = 1000; //エンコーダの分解能×逓倍
 int ecgear = 35; //エンコーダのギアの歯数
 double Kp = 0.342000; //P値
 double Ki = 0.013062; //I値
 double Kd = 0.003266; //D値
-double leg_wait_time = 0.3; //脚の伸縮にかかる時間
-double turn_wait_time = 1.5; //旋回にかかる時間
-double target_max = 690; //直動機構の目標座標の上限（オーバーシュートを考慮して短めに設定）
+double leg_wait_time = 0.5; //脚の伸縮にかかる時間
+double turn_wait_time = 0.7; //旋回にかかる時間
+double target_max = 300; //直動機構の目標座標の上限（オーバーシュートを考慮して短めに設定）
 double target_min = 20; //直動機構の目標座標の下限（オーバーシュートを考慮して短めに設定）
 double rail_len = 710;
 double turn_error=10;   //旋回の際のY脚のずれの許容誤差
@@ -57,7 +58,7 @@
 DigitalOut air_x(p18); //X脚の伸縮
 DigitalOut air_y(p17); //Y脚の伸縮
 DigitalOut air_x_sub(p14); //X脚の先端の伸縮（今後実装予定）
-DigitalOut air_y_sub(p16); //Y脚の先端の伸縮（今後実装予定）
+DigitalOut air_turn(p16); //Y脚の先端の伸縮（今後実装予定）
 DigitalOut air_hand(p19); //ゲルゲ回収アームのハンド部開閉
 DigitalOut air_trig(p20); //ゲルゲ持ち上げ機構のロック解除
 Ec ec(p24,p23,NC,resolution,0.01); //直動機構のエンコーダ
@@ -82,46 +83,10 @@
 
 
 int main() {
-    setup();
-    while(switch_x==1){
-    }
-    
+   setup();
+   while(switch_x==1){}
     reset();
-    
-    /*while(1){
-        move(680,10);
-        pc.printf("%f , ",1.0*ecgear*Pi*ec.getCount()/resolution);
-        wait(0.5);
-        move(20,10);
-        pc.printf("%f\r\n",1.0*ecgear*Pi*ec.getCount()/resolution);
-        wait(0.5);
-    }*/ 
-    
-    //up_x();
-    /*while(1){
-    servo.set_degree(0,105);
-    wait(1);
-    servo.set_degree(0,135);
-    wait(1);
-    servo.set_degree(0,75);
-    wait(1);
-    }*/
-    //down_x();
-    
-    //reset();
-    
-    /*while(1){
-    out(0.2);
-    }*/
-    
-    //move_x(360,10);
-    //turn(90);
-    //turn(-45);
-    
-    
-    
-    
-    while(1) {
+   while(1) {
         switch((int)(target_plot[target_num][0])){
             case 0:
                 wait_MR1();
@@ -174,7 +139,7 @@
     air_x=0;
     air_y=0;
     air_x_sub=0;
-    air_y_sub=0;
+    air_turn=0;
     air_hand=0;
     air_trig=0;
     servo.setSpeed(0,127);
@@ -322,7 +287,7 @@
     if(fabs(1.0*ecgear*Pi*ec.getCount()/resolution-rail_len*0.5)>turn_error){
         move_y(rail_len*0.5,turn_error);
     }
-    bool turn_finish=0;
+   /* bool turn_finish=0;
     while(turn_finish==0){
         double degree_out=0;
         if(fabs(degree)<=45){
@@ -334,16 +299,63 @@
         }else{
             degree_out=-45;
             degree+=45;
+        }*///!!削除
+        if(degree == 45){
+          up_y();
+          air_turn=1;
+          wait(turn_wait_time);
+          down_y();
+          up_x();
+          air_turn=0;
+          wait(turn_wait_time);
+          down_x();
+        } else if(degree == 90){
+          up_y();
+          air_turn=1;
+          wait(turn_wait_time);
+          down_y();
+          up_x();
+          air_turn=0;
+          wait(turn_wait_time);
+          down_x();
+          
+          up_y();
+          air_turn=1;
+          wait(turn_wait_time);
+          down_y();
+          up_x();
+          air_turn=0;
+          wait(turn_wait_time);
+          down_x();
         }
+      if(degree == -45){
+        up_x();
+        air_turn=1;
+        wait(turn_wait_time);
+        down_x();
         up_y();
-        servo.set_degree(0,servo_iniposi+degree_out);
+        air_turn=0;
         wait(turn_wait_time);
         down_y();
+      }else if(degree == -90){
         up_x();
-        servo.set_degree(0,servo_iniposi);
+        air_turn=1;
         wait(turn_wait_time);
         down_x();
-    }
+        up_y();
+        air_turn=0;
+        wait(turn_wait_time);
+        down_y();
+        
+        up_x();
+        air_turn=1;
+        wait(turn_wait_time);
+        down_x();
+        up_y();
+        air_turn=0;
+        wait(turn_wait_time);
+        down_y();
+      }else{}
 }
 
 
@@ -374,10 +386,10 @@
 }
 
 void wait_MR1(){
-    /*while(switch_x==1){
+    while(switch_x==1){
         
-    }*/
-    while(switch_hand==0);
+    }
+    //while(switch_hand==0);
     air_hand=1;
     wait(1);