Diff: main.cpp

Revision:

17:2b3fa9b1a05b

Parent:

16:1e91753f0a01

Child:

18:d0a6771fa38d

--- a/main.cpp	Thu May 02 09:24:28 2019 +0000
+++ b/main.cpp	Thu May 02 09:34:57 2019 +0000
@@ -1,8 +1,8 @@
 #include "mbed.h"
+#include "debug.h"
 #include "pin.h"
 #include "microinfinity.h"
-#include "pidcontroller.h"
-#include "debug.h"
+#include "robot.h"
 /*
 //#define DEBUG_ON
 
@@ -26,184 +26,6 @@
 };
 float accel_max = 0.01; //これグローバルにしたのはごめん。set関数多すぎてめんどくなった。
 
-
-class Motor //PIDコントローラ、エンコーダを含むモータのクラス
-{
-    PwmOut *pin_forward_, *pin_back_;
-    Ec *ec_;                             //対応するエンコーダ
-    float duty_, pre_duty_, duty_limit_; //dutyと現在のモータ位置
-    int resolution_;
-public:
-    Motor(PwmOut *forward, PwmOut *back); //ピンをポインタ渡し
-    void setDutyLimit(float limit)
-    {
-        duty_limit_ = limit;
-    };
-    float getDutyLimit()
-    {
-        return duty_limit_;
-    };
-    float getPosi();                   //ポジをエンコーダから取得
-    void calcDuty(PIDcontroller *pid); //Duty比を計算
-    void setEncoder(Ec *ec)
-    {
-        ec_ = ec;
-    };           //エンコーダを設定
-    void setResolution(int reso)
-    {
-        resolution_ = reso;
-    };
-    void output();                     //出力するだけ
-    void output(float duty);
-};
-
-class OneLeg //足の挙動を制御する
-{
-    Motor *motor_;
-    float target_pose_;
-
-public:
-    PIDcontroller *pid_;
-    OneLeg() {};
-    void setMotor(Motor *motor)
-    {
-        motor_ = motor;
-    };
-    void setPIDcontroller(PIDcontroller *pid)
-    {
-        pid_ = pid;
-    };
-    void setTargetPose(float target_pose);
-    void actMotor();//モータ出力
-};
-
-class Robot
-{
-    float ticker_time_, air_wait_time_;
-    OneLeg *Leg1_, *Leg2_;
-    Timer timer;
-
-public:
-    Robot()
-    {
-        timer.reset();
-        timer.start();
-    };
-    void setLeg(OneLeg *Leg1_, OneLeg *Leg2_);
-    void setTickerTime(float ticker_time);
-    void run();//ここがメインで走る記述
-};
-
-
-
-Motor::Motor(PwmOut *forward, PwmOut *back)
-{
-    pin_forward_ = forward;
-    pin_back_ = back;
-}
-float Motor::getPosi()
-{
-    float posi = 2.0*180*((float)(ec_->getCount)()/(float)resolution_);
-
-    //DEBUG("value :%d  :%d\n\r", (ec_->getCount)(),resolution_);
-    DEBUG("posi is %.4f\n\r",posi);
-    return posi;
-}
-void Motor::calcDuty(PIDcontroller *pid)
-{
-    duty_ = pid->calc(getPosi());
-    DEBUG("duty is %.4f\n\r",duty_);
-}
-void Motor::output()
-{
-    //DEBUG("dutyOut %.3f\n\r",duty_);
-    //加速度が一定値を超えたら変更加える
-    if (duty_ > 0) {
-        //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);
-        DEBUG("forward %.3f\n\r",pin_forward_->read());
-    } else {
-        //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);
-        DEBUG("back %.3f\n\r",pin_back_->read());
-    }
-    pre_duty_ = duty_;
-}
-void Motor::output(float duty)
-{
-    duty_ = duty;
-    //DEBUG("dutyOut %.3f\n\r",duty_);
-    //加速度が一定値を超えたら変更加える
-    if (duty_ > 0) {
-        //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);
-        DEBUG("forward %.3f\n\r",pin_forward_->read());
-    } else {
-        //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);
-        DEBUG("back %.3f\n\r",pin_back_->read());
-    }
-    pre_duty_ = duty_;
-}
-
-void OneLeg::setTargetPose(float target_pose)
-{
-    target_pose_ = target_pose;
-    //PIDにtargetを送る
-    pid_->setTarget(target_pose_);
-}
-void OneLeg::actMotor()
-{
-    motor_->calcDuty(pid_);
-    motor_->output();
-}
-
-
-
-void Robot::setLeg(OneLeg *Leg1, OneLeg *Leg2)
-{
-    Leg1_ = Leg1;
-    Leg2_ = Leg2;
-}
-void Robot::setTickerTime(float ticker_time)
-{
-    ticker_time_ = ticker_time;
-    Leg1_->pid_->setTimeDelta(ticker_time_);
-    Leg2_->pid_->setTimeDelta(ticker_time_);
-}
-void Robot::run()
-{
-    while (!Leg1_->pid_->IsConvergence_ || !Leg2_->pid_->IsConvergence_) { //片方が収束していない時*/
-        //ticker_time毎にモータに出力する
-        float time_s = timer.read();
-        Leg1_->actMotor();
-        Leg2_->actMotor();
-        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 //時間が足りない場合警告
-            printf("error: restTime not enough\n\r");
-        DEBUG("loop end\n\r")
-    }
-
-}
-
-
-
 ////////////関数
 void reset();
 void setup();