Diff: main.cpp

Revision:

6:75cfa1a66382

Parent:

5:1fa5aa097af5

--- a/main.cpp	Sat Apr 20 11:41:38 2019 +0000
+++ b/main.cpp	Sat Apr 27 11:22:00 2019 +0000
@@ -5,7 +5,7 @@
 #include "pin.h"
 #include "microinfinity.h"
 
-#define DEBUG_ON
+//#define DEBUG_ON
 
 #ifdef DEBUG_ON
 #define DEBUG(...) printf("" __VA_ARGS__);
@@ -86,7 +86,7 @@
     Timer timer;
 
   public:
-    Robot(){};
+    Robot(){timer.reset(); timer.start();};
     void setLeg(OneLeg *rightLeg, OneLeg *leftLeg);
     void setTickerTime(float ticker_time);
     void setAirWaitTime(float air_wait_time);
@@ -185,16 +185,18 @@
     if (duty_ > 0)
     {
         if (duty_ - pre_duty_ > accel_max) duty_ = pre_duty_ + accel_max;
-        *pin_forward_ = min(fabs(duty_), duty_limit_);
-        *pin_back_ = 0;
+        double output_duty=min(fabs(duty_), duty_limit_);
+        pin_forward_->write(output_duty);
+        pin_back_->write(0);
         DEBUG("forward %.3f\n\r",pin_forward_->read());
     }
     else
     {
         if (pre_duty_ - duty_ > accel_max)
             duty_ = pre_duty_ - accel_max;
-        *pin_forward_ = 0;
-        *pin_back_ = min(fabs(duty_), duty_limit_);
+        double output_duty=min(fabs(duty_), duty_limit_);
+        pin_forward_->write(0);
+        pin_back_->write(output_duty);
         DEBUG("back %.3f\n\r",pin_back_->read());
     }
     pre_duty_ = duty_;
@@ -270,7 +272,7 @@
 }
 void Robot::run()
 {
-    while (!rightLeg_->pid_->IsConvergence_ || !leftLeg_->pid_->IsConvergence_) //片方が収束していない時*/
+    while (/*!rightLeg_->pid_->IsConvergence_ || */!leftLeg_->pid_->IsConvergence_) //片方が収束していない時*/
     {
         //ticker_time毎にモータに出力する
         float time_s = timer.read();
@@ -279,9 +281,13 @@
         float rest_time_s = ticker_time_ - (timer.read() - time_s);
         //ticker_timeまで待機
         if (rest_time_s > 0)
+        {
             wait(rest_time_s);
+            DEBUG("start:%.3f last: %.3f restTime: %.3f\n\r",time_s, timer.read(),rest_time_s);
+        }
+            
         else //時間が足りない場合警告
-            DEBUG("error: restTime not enough\n\r");
+            printf("error: restTime not enough\n\r");
         DEBUG("loop end\n\r")
     }
 
@@ -301,8 +307,8 @@
 //PIDcontroller, Motor, AirCylinderはOneLegのメンバクラスとして扱う
 //しかし変更を多々行うためポインタ渡しにしてある
 //文が長くなると困るため, PID係数の変更は直接PIDコントローラを介して行う
-PIDcontroller pid_r(0.0015, 0.000, 0.000);
-PIDcontroller pid_l(0.0015, 0.000, 0.000);    //Kp.Ki,Kd
+PIDcontroller pid_r(0.002, 0.000, 0.000);
+PIDcontroller pid_l(0.002, 0.000, 0.000);    //Kp.Ki,Kd
 Motor motorLeft(&motor_l_f, &motor_l_b),
      motorRight(&motor_r_f, &motor_r_b); //forward,backのピンを代入
 OneLeg leftLeg, rightLeg;
@@ -313,15 +319,14 @@
 {
     DEBUG("start\n\r");
     setup();
+    
     //PIDコントローラ生成　左右２つで、係数の変更はメンバ関数を用いて行う
     //許容誤差設定
     pid_r.setTolerance(20.0);
     pid_l.setTolerance(20.0);
     //モータ生成　PID、ECの代入を行うこと
     motorLeft.setEncoder(&ec_l);
-    motorLeft.setDutyLimit(0.2);
     motorRight.setEncoder(&ec_r);
-    motorRight.setDutyLimit(0.2);
 
     AirCylinder air_leg[4] = {//名前は分けたほうがいいきがする？
                               AirCylinder(&air[0]),
@@ -348,26 +353,32 @@
     robot.setLeg(&rightLeg, &leftLeg);
     robot.setTickerTime(0.01); //モータ出力間隔 0.01
     robot.setAirWaitTime(0.2); //シリンダを待つ時間
+    
+    motorLeft.setDutyLimit(0.2);
+    motorRight.setDutyLimit(0.2);
 
     //初期位置は0これは内側が一番内側の時
-
-    //example これで１サイクル
-    leftLeg.setTargetPose(200.0);
-    rightLeg.setTargetPose(200.0);
+    char str[255] = {};
+    printf("setup complete Input any key\n\r");
+    scanf("%s", str);
+    printf("start!");
+    leftLeg.setTargetPose(2000.0);
+    rightLeg.setTargetPose(2000.0);
     robot.run();
     leftLeg.setTargetPose(80.0);
     rightLeg.setTargetPose(80.0);
     robot.run();
+    }
     //
     DEBUG("program end\n\n\r");
 }
 
 void setup()
 {
-    motor_r_f.period_us(100);
-    motor_r_b.period_us(100);
-    motor_l_f.period_us(100);
-    motor_l_b.period_us(100);
+    motor_r_f.period_us(50);
+    motor_r_b.period_us(50);
+    motor_l_f.period_us(50);
+    motor_l_b.period_us(50);
 
     for (int i = 0; i < 4; i++)
     {