Diff: moves/moves.cpp

Revision:

25:c740e6fd5dab

Parent:

23:ef274a44a867

Child:

26:5fb1aa9cb7f0

--- a/moves/moves.cpp	Fri May 03 10:00:20 2019 +0000
+++ b/moves/moves.cpp	Mon May 06 04:03:00 2019 +0000
@@ -37,6 +37,31 @@
     else if(degree0 - target_degree > tolerance_degree) curveRight();
     else straight();
 }
+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());
@@ -82,7 +107,21 @@
     while(1) {
         if(bus_in.read() == 1) break;
     }
-    step_num_r++;
+}
+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);
+    while(1) {
+        if(bus_in.read() == 1) break;
+    }
+    step_num_l--;
 }
 void turnRight()
 {
@@ -115,6 +154,20 @@
     }
     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);
+    while(1) {
+        if(bus_in.read() == 1) break;
+    }
+}
 void SetupMoves()
 {
     motor_lo_f.period_us(100);
@@ -138,15 +191,15 @@
 
 void reset()
 {
-    motor_lo.setDutyLimit(0.3);
-    motor_li.setDutyLimit(0.3);
+    motor_lo.setDutyLimit(0.2);
+    motor_li.setDutyLimit(0.2);
     while(switch_lo.read()) {
-        motor_lo.output(0.3);
+        motor_lo.output(0.2);
     }
     ec_lo.reset();
     motor_lo.output(0.0);
     while(switch_li.read()) {
-        motor_li.output(0.3);
+        motor_li.output(0.2);
     }
     ec_li.reset();
     motor_li.output(0.0);