Diff: main.cpp

Revision:

11:0522b336eb82

Parent:

10:2973cea54efd

Child:

12:e6fd3a3201f0

--- a/main.cpp	Wed May 08 08:21:08 2019 +0000
+++ b/main.cpp	Wed May 08 08:45:48 2019 +0000
@@ -33,6 +33,8 @@
       motor_ri(&motor_ri_f, &motor_ri_b); //forward,backのピンを代入
 OneLeg leg_ro, leg_ri;
 Robot robot;
+DigitalOut led[] = {LED1,LED2,LED3,LED4};
+
 
 int step_num_r = 0;
 int main()
@@ -74,7 +76,7 @@
         motor_ri.setDutyLimit(duty);
         if(hand_mode == GEREGE) {
             air_hand = 0;
-            
+
         } else if(hand_mode == GOAL) {
             for(int i=0; i<10; ++i)
                 servo.set_degree(0,(7200 - 3500) * 270.0/(11500 - 3500));
@@ -88,39 +90,34 @@
     }
 
 }
-
-void forward()
+void Run(int d_step_r)
 {
-    leg_ro.setTargetPose(360+step_num_r*180);
-    leg_ri.setTargetPose(180+step_num_r*180);
+    int target_step = step_num_r + d_step_r;
+    leg_ro.setTargetPose(180+target_step*180);
+    leg_ri.setTargetPose(target_step*180);
+    led[0] = 1;
     robot.run();
+    led[0] = 0;
+    step_num_r = target_step;
     motor_ro_f.write(0);
     motor_ro_b.write(0);
     motor_ri_f.write(0);
     motor_ri_b.write(0);
-    step_num_r++;
+    
+}
+
+void forward()
+{
+    Run(1);
 }
 void stop()
 {
-    leg_ro.setTargetPose(360+(step_num_r-1)*180);
-    leg_ri.setTargetPose(180+(step_num_r-1)*180);
-    robot.run();
-    motor_ro_f.write(0);
-    motor_ro_b.write(0);
-    motor_ri_f.write(0);
-    motor_ri_b.write(0);
+    Run(0);
 }
 
 void back()
 {
-    leg_ro.setTargetPose(360+(step_num_r-2)*180);
-    leg_ri.setTargetPose(180+(step_num_r-2)*180);
-    robot.run();
-    motor_ro_f.write(0);
-    motor_ro_b.write(0);
-    motor_ri_f.write(0);
-    motor_ri_b.write(0);
-    step_num_r--;
+    Run(-1);
 }
 //////////////////////////////////////////////
 void setup()