Diff: moves/moves.cpp

Revision:

29:81d159ff339a

Parent:

27:d392a95f4799

Child:

30:231e6584afe9

--- a/moves/moves.cpp	Wed May 08 07:48:01 2019 +0000
+++ b/moves/moves.cpp	Wed May 08 08:44:12 2019 +0000
@@ -20,7 +20,27 @@
     STOP,
     FORWARD,
 };
-int step_num_l, step_num_r;
+int step_num_l;
+void WaitAnotherLeg();
+
+//収束していなければLED1をON. param:: d_step_l:左足を動かす歩数。負なら後ろに進む
+void Run(WalkMode mode, int d_step_l)
+{
+    int target_step = step_num_l + d_step_l;
+    can_send(mode, motor_lo.getDutyLimit());
+    leg_lo.setTargetPose(180 + target_step*180);
+    leg_li.setTargetPose(target_step*180);
+    //収束前
+    led1 = 1;
+    robot.run();
+    led1 = 0;
+    step_num_l = target_step;
+    motor_lo_f.write(0);
+    motor_lo_b.write(0);
+    motor_li_f.write(0);
+    motor_li_b.write(0);
+    WaitAnotherLeg();
+}
 void WaitAnotherLeg()
 {
     FileWrite();
@@ -48,119 +68,32 @@
 void climbAndInfinity(float target_degree, float tolerance_degree)
 {
     if(target_degree - degree0 > tolerance_degree) {//左に曲がる
-
         curveLeft();
     } else if(degree0 - target_degree > tolerance_degree) {//右に曲がる
-
         curveRight();
     } else {
-
         straight();
     }
 }
-void onestraight()
-{
-    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);
-    step_num_l++;
-    step_num_r++;
-}
 void straight()
 {
-    can_send(FORWARD, motor_lo.getDutyLimit());
-    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);
-    WaitAnotherLeg();
-    step_num_l++;
-    step_num_r++;
+    Run(FORWARD,1);
 }
 void turnLeft()
 {
-    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();
-    motor_lo_f.write(0);
-    motor_lo_b.write(0);
-    motor_li_f.write(0);
-    motor_li_b.write(0);
-    WaitAnotherLeg();
-    step_num_r++;
-    step_num_l--;
+    Run(FORWARD,-1);
 }
 void curveLeft()
 {
-    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();
-    motor_lo_f.write(0);
-    motor_lo_b.write(0);
-    motor_li_f.write(0);
-    motor_li_b.write(0);
-    WaitAnotherLeg();
-}
-void backLeft()
-{
-    can_send(STOP, motor_lo.getDutyLimit());
-    leg_lo.setTargetPose(360+(step_num_l-2)*180);
-    leg_li.setTargetPose(180+(step_num_l-2)*180);
-    robot.run();
-    motor_lo_f.write(0);
-    motor_lo_b.write(0);
-    motor_li_f.write(0);
-    motor_li_b.write(0);
-    WaitAnotherLeg();
-    step_num_l--;
+    Run(FORWARD,0);
 }
 void turnRight()
 {
-    can_send(BACK, motor_lo.getDutyLimit());
-    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);
-    WaitAnotherLeg();
-    step_num_r--;
-    step_num_l++;
+    Run(BACK,1);
 }
 void curveRight()
 {
-    can_send(STOP, motor_lo.getDutyLimit());
-    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);
-    WaitAnotherLeg();
-    step_num_l++;
-}
-void backRight()
-{
-    can_send(BACK, motor_lo.getDutyLimit());
-    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);
-    WaitAnotherLeg();
+    Run(STOP, 1);
 }
 void SetupMoves()
 {
@@ -201,8 +134,4 @@
 void HandMove()
 {
     can_send(STOP,0);
-}
-
-
-
-
+}
\ No newline at end of file