Diff: moves/moves.cpp

Revision:

42:b00cdc97f492

Parent:

41:645266b004a2

Child:

43:1c180b4516bf

--- a/moves/moves.cpp	Thu May 16 09:20:59 2019 +0000
+++ b/moves/moves.cpp	Fri May 17 02:17:40 2019 +0000
@@ -5,6 +5,8 @@
 #include "debug.h"
 
 const float kPidSpan_s = 0.02;
+const float kGroundDuty = 0.3;
+const float kRelativeDeg = 150;
 const int kToleranceDeg= 10;
 PwmOut motor_lo_f(pin_pwm[0][0]); //モータ正転
 PwmOut motor_lo_b(pin_pwm[0][1]); //モータ逆転
@@ -56,7 +58,6 @@
 
 void WaitAnotherLeg()
 {
-
     motor_lo_f.write(0);
     motor_lo_b.write(0);
     motor_li_f.write(0);
@@ -146,12 +147,12 @@
     leg_li.setPIDcontroller(&pid_li);
     robot.setLeg(&leg_lo, &leg_li);
     robot.setTickerTime(kPidSpan_s);
-    
+
     motor_lo.SetSlowDuty(0.1);
     motor_li.SetSlowDuty(0.1);
-    motor_lo.SetSlowRange(150, 190);
-    motor_li.SetSlowRange(150, 190);
-    robot.SetGroundRange(180,360);
+    motor_lo.SetSlowRange(325, 360);
+    motor_li.SetSlowRange(325, 360);
+    robot.SetGroundRange(0,180);
 }
 
 void reset()
@@ -183,17 +184,20 @@
     can_send(mode, motor_lo.getDutyLimit());
     int target_step = step_num_l + d_step_l;
     int target_deg = 180 + target_step*180;
+    //dutyの計算。角度PIDにしたいが未実装
+    float ground_duty = kGroundDuty;
     //1歩分進む
-    while (fabs(leg_lo.GetPosi_deg() - target_deg) >kToleranceDeg) { //loが収束していない時
+    while (fabs(leg_lo.GetPosi_deg() - target_deg) >kToleranceDeg) { //loが収束していない時.liは無視
         robot.StartTime();
-        float ground_omega_rad_s = 10;
-        float relative_deg = 120;
-        robot.RunUnSync(ground_omega_rad_s,  relative_deg);
+        robot.RunUnSync(ground_duty,  kRelativeDeg);
         FileWrite();
         robot.WaitRestTime();
     }
+    motor_lo_f.write(0);
+    motor_lo_b.write(0);
+    motor_li_f.write(0);
+    motor_li_b.write(0);
     step_num_l = target_step;
-    
 }
 void stop()
 {