Diff: main.cpp

Revision:

12:9a5de6adae9c

Parent:

11:a6fadff7cc78

Child:

13:29e71a2fd11e

--- a/main.cpp	Thu May 02 00:51:47 2019 +0000
+++ b/main.cpp	Thu May 02 04:52:47 2019 +0000
@@ -12,6 +12,18 @@
 
 #define Pi 3.14159265359 //円周率π
 
+enum WalkMode
+{
+    BACK,
+    STOP,
+    FORWARD,
+};
+enum EVENT
+{
+    NORMAL,
+    GEREGE,
+    GOAL,    
+};
 float accel_max = 0.01; //これグローバルにしたのはごめん。set関数多すぎてめんどくなった。
 
 class PIDcontroller //distanceをvalueに置き換えました
@@ -54,6 +66,7 @@
     {
         duty_limit_ = limit;
     };
+    float getDutyLimit(){return duty_limit_;};
     float getPosi();                   //ポジをエンコーダから取得
     void calcDuty(PIDcontroller *pid); //Duty比を計算
     void setEncoder(Ec *ec)
@@ -165,14 +178,14 @@
     //DEBUG("dutyOut %.3f\n\r",duty_);
     //加速度が一定値を超えたら変更加える
     if (duty_ > 0) {
-        if (duty_ - pre_duty_ > accel_max) duty_ = pre_duty_ + accel_max;
+        //if (duty_ - pre_duty_ > accel_max) duty_ = pre_duty_ + accel_max;
         double output_duty=min(fabs(duty_), duty_limit_);
         pin_forward_->write(output_duty);
         pin_back_->write(0);
         DEBUG("forward %.3f\n\r",pin_forward_->read());
     } else {
-        if (pre_duty_ - duty_ > accel_max)
-            duty_ = pre_duty_ - accel_max;
+        //if (pre_duty_ - duty_ > accel_max)
+        //    duty_ = pre_duty_ - accel_max;
         double output_duty=min(fabs(duty_), duty_limit_);
         pin_forward_->write(0);
         pin_back_->write(output_duty);
@@ -256,19 +269,21 @@
 void set_gyro();
 double get_dist_forward();
 double get_dist_back();
-void can_send(float target_ro, float target_ri);
-void straight(int &step_num_l, int &step_num_r);
-void turnLeft(int &step_num_l, int &step_num_r);
-void curveLeft(int &step_num_l, int &step_num_r);
-void turnRight(int &step_num_l, int &step_num_r);
-void curveRight(int &step_num_l, int &step_num_r);
+void can_send(int mode, float duty);
+void straight();
+void turnLeft();
+void curveLeft();
+void turnRight();
+void curveRight();
 void turn(float turn_degree);//回転角, 収束許容誤差
 void straightAndInfinity(float target_degree, float tolerance_degree);//角度を補正するのと前進
 
+void wait_gerege();
+
 ////////////定数
 
 ////////////変数
-bool hand_mode=0;
+bool hand_mode=NORMAL;
 
 //PIDcontroller, Motor, AirCylinderはOneLegのメンバクラスとして扱う
 //しかし変更を多々行うためポインタ渡しにしてある
@@ -302,6 +317,7 @@
     robot.setTickerTime(0.01); //モータ出力間隔 0.01
     motor_lo.setDutyLimit(0.5);//0.5
     motor_li.setDutyLimit(0.5);
+    printf("reset standby\n\r");
     reset();
     printf("bus standby\n\r");
     while(1) {
@@ -309,12 +325,15 @@
     }
     printf("bus is %d\n\r", bus_in.read());
     
-    char str[255] = {};
-    printf("setup complete Input any key\n\r");
-    scanf("%s", str);
-    printf("start!");
+    wait(0.5);
+    
+    straight();
+    
+    wait_gerege();
+    
+    hand_mode = GEREGE;
+    
     set_gyro();
-    
     //Sample
     //スタート直進
     printf("dist:%.3f\r\n", get_dist_forward());
@@ -322,26 +341,42 @@
     {
         straightAndInfinity(0, 5);
         //wait(0.01);
-        //printf("angle: %.3f\r\n", degree0);
+        printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
     }
-    printf("back dist:%.3f\r\n", get_dist_back());
+    //printf("back dist:%.3f\r\n", get_dist_back());
     //段差前旋回
+    motor_lo.setDutyLimit(0.4);//0.5
+    motor_li.setDutyLimit(0.4);
     turn(45.0);
     //段差乗り越え
-    for(int i= 0;i<5;++i) straight(step_num_l, step_num_r);
-    while(get_dist_back() > 40) straight(step_num_l, step_num_r);
+    for(int i= 0;i<5;++i) straight();
+    while(get_dist_back() > 40) straight();
     //段差後旋回
+    printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
     turn(45.0);
+    printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
     //前進
-    while(get_dist_forward() > 50) straight(step_num_l, step_num_r);
+    motor_lo.setDutyLimit(0.5);//0.5
+    motor_li.setDutyLimit(0.5);
+    while(get_dist_forward() > 60) 
+    {
+        straightAndInfinity(90.0, 5.0);
+        printf("angle:%.3f backdist:%.2f\r\n", degree0, get_dist_back());
+    }
     //ロープ前旋回
     turn(-90);
     //ロープ
-    while(1)
+    while(get_dist_forward() > 60)
     {
      straightAndInfinity(0, 5);
-     printf("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
+     //printf("forward:%.3f back:%.3f\r\n", get_dist_forward(),get_dist_back());
     }
+    
+    turn(-90);
+    
+    hand_mode = GOAL;
+    
+    straight();
 }
 void turn(float turn_degree)//turn_degreeを超えるまで旋回し続ける関数
 {
@@ -349,23 +384,23 @@
     if(target_degree - degree0 > 0)
     {
         while(target_degree - degree0 > 0)
-        turnLeft(step_num_l, step_num_r);   
+        turnLeft();   
     }
     else
     {
         while(target_degree - degree0 < 0)
-        turnRight(step_num_l, step_num_r);
+        turnRight();
     }
 }
 void straightAndInfinity(float target_degree, float tolerance_degree)
 {
-    if(target_degree - degree0 > tolerance_degree) curveLeft(step_num_l, step_num_r);
-    else if(degree0 - target_degree > tolerance_degree) curveRight(step_num_l, step_num_r);
-    else straight(step_num_l, step_num_r);
+    if(target_degree - degree0 > tolerance_degree) curveLeft();
+    else if(degree0 - target_degree > tolerance_degree) curveRight();
+    else straight();
 }
-void straight(int &step_num_l, int &step_num_r)
+void straight()
 {
-    can_send((float)step_num_r,(float)step_num_r);
+    can_send(FORWARD, motor_lo.getDutyLimit());
     leg_lo.setTargetPose(360+step_num_l*180);
     leg_li.setTargetPose(180+step_num_l*180);
     robot.run();
@@ -379,9 +414,9 @@
     step_num_l++;
     step_num_r++;
 }
-void turnLeft(int &step_num_l, int &step_num_r)
+void turnLeft()
 {
-    can_send(step_num_r,step_num_r);
+    can_send(FORWARD, motor_lo.getDutyLimit());
     leg_lo.setTargetPose(360+(step_num_l-2)*180);
     leg_li.setTargetPose(180+(step_num_l-2)*180);
     robot.run();
@@ -395,9 +430,9 @@
     step_num_r++;
     step_num_l--;
 }
-void curveLeft(int &step_num_l, int &step_num_r)
+void curveLeft()
 {
-    can_send(step_num_r,step_num_r);
+    can_send(FORWARD, motor_lo.getDutyLimit());
     leg_lo.setTargetPose(360+(step_num_l-1)*180);
     leg_li.setTargetPose(180+(step_num_l-1)*180);
     robot.run();
@@ -410,9 +445,9 @@
     }
     step_num_r++;
 }
-void turnRight(int &step_num_l, int &step_num_r)
+void turnRight()
 {
-    can_send(step_num_r-2,step_num_r-2);
+    can_send(BACK, motor_lo.getDutyLimit());
     leg_lo.setTargetPose(360+step_num_l*180);
     leg_li.setTargetPose(180+step_num_l*180);
     robot.run();
@@ -426,9 +461,9 @@
     step_num_r--;
     step_num_l++;
 }
-void curveRight(int &step_num_l, int &step_num_r)
+void curveRight()
 {
-    can_send(step_num_r-1,step_num_r-1);
+    can_send(STOP, motor_lo.getDutyLimit());
     leg_lo.setTargetPose(360+step_num_l*180);
     leg_li.setTargetPose(180+step_num_l*180);
     robot.run();
@@ -470,17 +505,15 @@
 
 
 //////////////////////////////////////can
-void can_send(float target_ro, float target_ri)
+void can_send(int mode, float duty)
 {
-    char data[4]= {0};
-    int target_ro_send=target_ro+360;
-    int target_ri_send=target_ri+360;
-    data[0]=target_ro_send & 0b11111111;
-    data[1]=target_ri_send & 0b11111111;
-    data[2]=(target_ro_send>>8) | ((target_ri_send>>4) & 0b11110000);
-    data[3]=hand_mode;
+    char data[2]= {0};
+    int send=mode * 10 + (int)(duty * 10.0);
+    //printf("duty is %.3f\n\r",duty);
+    data[0]=send & 0b11111111;
+    data[1]=hand_mode;
 
-    if(can1.write(CANMessage(0,data,4)))led4=1;
+    if(can1.write(CANMessage(0,data,2)))led4=1;
     else led4=0;
 }
 void reset()
@@ -508,3 +541,12 @@
     sensor_forward.start();
     return sensor_forward.get_dist_cm();
 }
+
+void wait_gerege()
+{
+    int i = 0;
+    while(i!=100)
+    {
+        if(hand.read()==0)i++;    
+    }    
+}
\ No newline at end of file