Diff: main.cpp

Revision:

4:db1640bd0e89

Parent:

3:29999b02e940

Child:

5:28581157108b

--- a/main.cpp	Wed May 01 05:46:53 2019 +0000
+++ b/main.cpp	Wed May 01 06:42:07 2019 +0000
@@ -140,7 +140,7 @@
     //加速度が一定値を超えたら変更加える
     if (duty_ > 0)
     {
-        if (duty_ - pre_duty_ > accel_max) duty_ = pre_duty_ + accel_max;
+        //if (duty_ - pre_duty_ > accel_max) duty_ = pre_duty_ + accel_max;
         double output_duty=min(fabs(duty_), duty_limit_);
         pin_forward_->write(output_duty);
         pin_back_->write(0);
@@ -148,8 +148,8 @@
     }
     else
     {
-        if (pre_duty_ - duty_ > accel_max)
-            duty_ = pre_duty_ - accel_max;
+        //if (pre_duty_ - duty_ > accel_max)
+        //    duty_ = pre_duty_ - accel_max;
         double output_duty=min(fabs(duty_), duty_limit_);
         pin_forward_->write(0);
         pin_back_->write(output_duty);
@@ -234,6 +234,9 @@
 
 ////////////関数
 void setup();
+void straight(int &step_num_l, int &step_num_r);
+void turnLeft(int &step_num_l, int &step_num_r);
+void turnRight(int &step_num_l, int &step_num_r);
 void can_receive(float &tar_ro, float &tar_ri);
 void reset();
 
@@ -270,8 +273,8 @@
     robot.setLeg(&leg_ro, &leg_ri);
     robot.setTickerTime(0.01); //モータ出力間隔 0.01
     
-    motor_ro.setDutyLimit(0.5);
-    motor_ri.setDutyLimit(0.5);
+    motor_ro.setDutyLimit(0.3);
+    motor_ri.setDutyLimit(0.3);
     
     reset();
     bus_out = 1;
@@ -279,20 +282,20 @@
     
     
     
-    float target_ro, target_ri;
+    int target_ro, target_ri;
     while(1) 
     {
-        float target_ro_now, target_ri_now;
+        float target_ro_now = target_ro;
+        float target_ri_now = target_ri;
         can_receive(target_ro_now,target_ri_now);
-        printf("tar_pre:%.3f tar_now:%.3f\n\r",target_ro,target_ro_now);
-        if(target_ro_now != target_ro)
+        //printf("tar_pre:%.3f tar_now:%.3f\n\r",target_ro,target_ro_now);
+        if((int)target_ro_now != target_ro)
         {
-            target_ro = target_ro_now;
-            target_ri = target_ri_now;
+            target_ro = (int)target_ro_now;
+            target_ri = (int)target_ri_now;
             bus_out = 0;
-            leg_ro.setTargetPose(target_ro);
-            leg_ri.setTargetPose(target_ri);
-            robot.run();    
+            printf("target is %d\n\r",target_ro);
+            straight(target_ro, target_ri);
         }
         motor_ro_f.write(0);
         motor_ro_b.write(0);
@@ -303,6 +306,39 @@
     
 }
 
+void straight(int &step_num_l, int &step_num_r)
+{
+    leg_ro.setTargetPose(360+step_num_l*180);
+    leg_ri.setTargetPose(180+step_num_l*180);
+    robot.run();
+    motor_ro_f.write(0);
+    motor_ro_b.write(0);
+    motor_ri_f.write(0);
+    motor_ri_b.write(0);
+}
+void turnLeft(int &step_num_l, int &step_num_r)
+{
+    leg_ro.setTargetPose(360+(step_num_l-2)*180);
+    leg_ri.setTargetPose(180+(step_num_l-2)*180);
+    robot.run();
+    motor_ro_f.write(0);
+    motor_ro_b.write(0);
+    motor_ri_f.write(0);
+    motor_ri_b.write(0);
+}
+void turnRight(int &step_num_l, int &step_num_r)
+{
+    leg_ro.setTargetPose(360+(step_num_l-1)*180);
+    leg_ri.setTargetPose(180+(step_num_l-1)*180);
+    robot.run();
+    motor_ro_f.write(0);
+    motor_ro_b.write(0);
+    motor_ri_f.write(0);
+    motor_ri_b.write(0);
+    
+}
+
+
 //////////////////////////////////////////////
 void setup()
 {
@@ -318,6 +354,8 @@
     servo.init();
 }
 
+
+
 void reset()
 {
     while(switch_ro.read()) {
@@ -336,6 +374,8 @@
 }
 
 
+
+
 //////////////////////////////////////////can
 void can_receive(float &tar_ro, float &tar_ri)
 {