Diff: main.cpp

Revision:

10:a335588b9ef0

Parent:

9:e248986c8423

Child:

11:a6fadff7cc78

--- a/main.cpp	Wed May 01 08:13:24 2019 +0000
+++ b/main.cpp	Wed May 01 11:53:58 2019 +0000
@@ -253,10 +253,17 @@
 ////////////関数
 void reset();
 void setup();
+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 turn(float turn_degree);//回転角, 収束許容誤差
+void straightAndInfinity(float target_degree, float tolerance_degree);//角度を補正するのと前進
 
 ////////////定数
 
@@ -273,63 +280,85 @@
 OneLeg leg_lo, leg_li;
 Robot robot;
 
-/////////////////////////////////////////////
+int step_num_l, step_num_r;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 int main()
 {
     setup();
-
-
-
     pid_lo.setTolerance(10);
     pid_li.setTolerance(10);
-
     motor_lo.setEncoder(&ec_lo);
     motor_lo.setResolution(1000);
     motor_li.setEncoder(&ec_li);
     motor_li.setResolution(600);
-
     leg_lo.setMotor(&motor_lo);
     leg_lo.setPIDcontroller(&pid_lo);
     leg_li.setMotor(&motor_li);
     leg_li.setPIDcontroller(&pid_li);
-
     robot.setLeg(&leg_lo, &leg_li);
     robot.setTickerTime(0.01); //モータ出力間隔 0.01
-
     motor_lo.setDutyLimit(0.5);
     motor_li.setDutyLimit(0.5);
-
-    /*
-    char str[255] = {};
-    printf("setup complete Input any key\n\r");
-    scanf("%s", str);
-    printf("start!");
-    */
-
-
-
-    
-
     reset();
     printf("bus standby\n\r");
     while(1) {
         if(bus_in.read() == 1) break;
     }
     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!");
+    set_gyro();
+    
     //Sample
-    static int step_num_l = 0, step_num_r = 0;
-
-    while(1)
+    //スタート直進
+    printf("dist:%.3f\r\n", get_dist_forward());
+    while(/*get_dist_back() < 300*/1)
     {
-        //straight(step_num_l, step_num_r);
-        turnLeft(step_num_l, step_num_r);
-        //printf("machine degree:%.3f\r\n", degree0);
+        //straightAndInfinity(0, 5);
         wait(0.01);
+        printf("angle: %.3f\r\n", degree0);
     }
+    printf("back dist:%.3f\r\n", get_dist_back());
+    //段差前旋回
+    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);
+    //段差後旋回
+    turn(45.0);
+    //前進
+    while(get_dist_forward() > 50) straight(step_num_l, step_num_r);
+    //ロープ前旋回
+    turn(-90);
+    //ロープ
+    while(1) straight(step_num_l, step_num_r);
     
-    //turnLeft(step_num_l, step_num_r);
-    //turnRight(step_num_l, step_num_r);
+}
+void turn(float turn_degree)//turn_degreeを超えるまで旋回し続ける関数
+{
+    float target_degree = degree0 + turn_degree;
+    if(target_degree - degree0 > 0)
+    {
+        while(target_degree - degree0 > 0)
+        turnLeft(step_num_l, step_num_r);   
+    }
+    else
+    {
+        while(target_degree - degree0 < 0)
+        turnRight(step_num_l, step_num_r);
+    }
+}
+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);
 }
 void straight(int &step_num_l, int &step_num_r)
 {
@@ -363,6 +392,21 @@
     step_num_r++;
     step_num_l--;
 }
+void curveLeft(int &step_num_l, int &step_num_r)
+{
+    can_send(step_num_r,step_num_r);
+    leg_lo.setTargetPose(360+(step_num_l-1)*180);
+    leg_li.setTargetPose(180+(step_num_l-1)*180);
+    robot.run();
+    motor_lo_f.write(0);
+    motor_lo_b.write(0);
+    motor_li_f.write(0);
+    motor_li_b.write(0);
+    while(1) {
+        if(bus_in.read() == 1) break;
+    }
+    step_num_r++;
+}
 void turnRight(int &step_num_l, int &step_num_r)
 {
     can_send(step_num_r-2,step_num_r-2);
@@ -379,6 +423,21 @@
     step_num_r--;
     step_num_l++;
 }
+void curveRight(int &step_num_l, int &step_num_r)
+{
+    can_send(step_num_r-1,step_num_r-1);
+    leg_lo.setTargetPose(360+step_num_l*180);
+    leg_li.setTargetPose(180+step_num_l*180);
+    robot.run();
+    motor_lo_f.write(0);
+    motor_lo_b.write(0);
+    motor_li_f.write(0);
+    motor_li_b.write(0);
+    while(1) {
+        if(bus_in.read() == 1) break;
+    }
+    step_num_l++;
+}
 
 
 void setup()
@@ -394,7 +453,10 @@
     switch_li.mode(PullUp);
     switch4.mode(PullUp);
 
+}
 
+void set_gyro()
+{
     device.baud(115200);
     device.format(8,Serial::None,1);
     device.attach(dev_rx, Serial::RxIrq);
@@ -431,4 +493,16 @@
     ec_li.reset();
     motor_li.output(0.0);
 }
-
+double get_dist_back()
+{
+    sensor_back.start();
+    //wait_ms(50);           //sensor.start()で信号を送ったあと反射波が帰ってきてから距離データが更新されるため、少し待つ必要がある。
+    //何ループも回す場合は不要。また、時間は適当だから調整が必要。
+    return sensor_back.get_dist_cm();
+}
+double get_dist_forward()
+{
+    //sensor_forward.start();
+    //return sensor_forward.get_dist_cm();
+    return 0.0;
+}