File content as of revision 6:75cfa1a66382:

#include "mbed.h"
#include "EC.h"
#include "KondoServo.h"
#include "hcsr04.h"
#include "pin.h"
#include "microinfinity.h"

//#define DEBUG_ON

#ifdef DEBUG_ON
#define DEBUG(...) printf("" __VA_ARGS__);
#else
#define DEBUG(...)
#endif


int ecgear = 25;        //ここはいちいち設定したくないからグローバル
float accel_max = 0.01; //これグローバルにしたのはごめん。set関数多すぎてめんどくなった。

#define Pi 3.14159265359 //円周率π

class PIDcontroller //distanceをvalueに置き換えました
{
    float Kp_, Ki_, Kd_, tolerance_, time_delta_;
    float pile_, value_old_, target_;

  public:
    bool IsConvergence_;                                //収束したかどうか
    PIDcontroller(float Kp, float Ki, float Kd);        //初期設定で係数を入力
    void setCoefficients(float Kp, float Ki, float Kd); //係数を変更するときに使う
    void setTimeDelta(float delta);
    void setTarget(float target);       //目標位置の設定
    void setTolerance(float tolerance); //許容誤差の設定
    float calc(float nowVal);           //現在位置と目標を比較してPID補正
    bool knowConvergence();             //収束したかどうかを外部に伝える
};

class Motor //PIDコントローラ、エンコーダを含むモータのクラス
{
    PwmOut *pin_forward_, *pin_back_;
    Ec *ec_;                             //対応するエンコーダ
    float duty_, pre_duty_, duty_limit_; //dutyと現在のモータ位置
  public:
    Motor(PwmOut *forward, PwmOut *back); //ピンをポインタ渡し
    void setDutyLimit(float limit);
    float getPosi();                   //ポジをエンコーダから取得
    void calcDuty(PIDcontroller *pid); //Duty比を計算
    void setEncoder(Ec *ec);           //エンコーダを設定
    void output();                     //出力するだけ
};

class AirCylinder
{
    DigitalOut *air_;
    bool IsOpenState_;

  public:
    AirCylinder(DigitalOut *air); //ピンをポインタ渡し
    void open();                    //足上げ
    void close();                  //足下げ
    void changeState();           //上がってるとき下げる　その逆も
};

class OneLeg //足の挙動を制御する
{
    Motor *motor_;
    AirCylinder *air1_, *air2_;
    float target_pose_;

  public:
    PIDcontroller *pid_;
    OneLeg(){};
    void setMotor(Motor *motor);
    void setPIDcontroller(PIDcontroller *pid);
    void setAir(AirCylinder *air1, AirCylinder *air2);
    void setTargetPose(float target_pose);
    void actMotor();//モータ出力
    void actCylinder();//シリンダ出力
};


class Robot
{
    float ticker_time_, air_wait_time_;
    OneLeg *rightLeg_, *leftLeg_;
    Timer timer;

  public:
    Robot(){timer.reset(); timer.start();};
    void setLeg(OneLeg *rightLeg, OneLeg *leftLeg);
    void setTickerTime(float ticker_time);
    void setAirWaitTime(float air_wait_time);
    void run();//ここがメインで走る記述
};




PIDcontroller::PIDcontroller(float Kp, float Ki, float Kd)
{
    Kp_ = Kp, Ki_ = Ki, Kd_ = Kd;
    DEBUG("set Kp:%.3f  KI:%.3f  Kd:%.3f \n\r", Kp_, Ki_, Kd_);
    IsConvergence_=true;
}
void PIDcontroller::setCoefficients(float Kp, float Ki, float Kd)
{
    Kp_ = Kp, Ki_ = Ki, Kd_ = Kd;
}
void PIDcontroller::setTimeDelta(float delta)
{
    time_delta_ = delta;
    DEBUG("set TimeDelta: %.3f\n\r", time_delta_);
}
void PIDcontroller::setTarget(float target)
{
    if (IsConvergence_) //収束時のみ変更可能
    {
        target_ = target;
        DEBUG("set Target: %.3f\n\r", target_);
        IsConvergence_ = false;
    }
    else
    {
        DEBUG("error: setTarget permission denied!\n");
    }
}
void PIDcontroller::setTolerance(float tolerance)
{
    tolerance_ = tolerance;
    DEBUG("set Tolerance: %.3f\n\r", tolerance_);
}
float PIDcontroller::calc(float nowVal)
{
    float out = 0;
    //PID計算ここで行う
    float deviation = target_ - nowVal; //目標との差分
    pile_ += deviation;                 //積分用に和を取る
    out = deviation * Kp_ - (nowVal - value_old_) / time_delta_ * Kd_ + pile_ * Ki_ * time_delta_;
    value_old_ = nowVal; //今のデータを保存
    //
    if (fabs(deviation) < tolerance_) //収束した場合
    {
        DEBUG("complete !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\n\r");
        out = 0;
        pile_ = 0;
        value_old_ = 0;
        IsConvergence_ = true;
    }
    return out;
}
bool PIDcontroller::knowConvergence()
{
    return IsConvergence_;
}

Motor::Motor(PwmOut *forward, PwmOut *back)
{
    pin_forward_ = forward;
    pin_back_ = back;
}
float Motor::getPosi()
{
    float posi = 1.0 * ecgear * Pi * (ec_->getCount)() /resolution;
    DEBUG("posi is %.4f\n\r",posi);
    return posi;
}
void Motor::setDutyLimit(float limit)
{
    duty_limit_ = limit;
    DEBUG("set DutyLimit: %.3f\n\r", duty_limit_);
}
void Motor::calcDuty(PIDcontroller *pid)
{    
    duty_ = pid->calc(getPosi());
    DEBUG("duty is %.4f\n\r",duty_);
}
void Motor::setEncoder(Ec *ec)
{
    ec_ = ec;
}
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_;
}

AirCylinder::AirCylinder(DigitalOut *air)
{
    air_ = air;
}
void AirCylinder::open()
{
    *air_ = 1;
    IsOpenState_ = true;
}
void AirCylinder::close()
{
    *air_ = 0;
    IsOpenState_ = false;
}
void AirCylinder::changeState()
{
    if (IsOpenState_)
        close();
    else
        open();
}

void OneLeg::setMotor(Motor *motor)
{
    motor_ = motor;
}
void OneLeg::setPIDcontroller(PIDcontroller *pid)
{
    pid_ = pid;
}
void OneLeg::setAir(AirCylinder *air1, AirCylinder *air2)
{
    air1_ = air1, air2_ = air2;
}
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 OneLeg::actCylinder()
{
    //上がっている場合は下げ、下がっている時は上げる
    air1_->changeState();
    air2_->changeState();
}

void Robot::setLeg(OneLeg *rightLeg, OneLeg *leftLeg)
{
    rightLeg_ = rightLeg;
    leftLeg_ = leftLeg;
}
void Robot::setTickerTime(float ticker_time)
{
    ticker_time_ = ticker_time;
    rightLeg_->pid_->setTimeDelta(ticker_time_);
    leftLeg_->pid_->setTimeDelta(ticker_time_);
}
void Robot::setAirWaitTime(float air_wait_time)
{
    air_wait_time_ = air_wait_time;
    DEBUG("set AirWaitTime: %.3f\n\r", air_wait_time_);
}
void Robot::run()
{
    while (/*!rightLeg_->pid_->IsConvergence_ || */!leftLeg_->pid_->IsConvergence_) //片方が収束していない時*/
    {
        //ticker_time毎にモータに出力する
        float time_s = timer.read();
        rightLeg_->actMotor();
        leftLeg_->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")
    }

    rightLeg_->actCylinder();
    leftLeg_->actCylinder();
    wait(air_wait_time_);
}






void setup();
void wait_MR1();

//PIDcontroller, Motor, AirCylinderはOneLegのメンバクラスとして扱う
//しかし変更を多々行うためポインタ渡しにしてある
//文が長くなると困るため, PID係数の変更は直接PIDコントローラを介して行う
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;
Robot robot;


int main()
{
    DEBUG("start\n\r");
    setup();
    
    //PIDコントローラ生成　左右２つで、係数の変更はメンバ関数を用いて行う
    //許容誤差設定
    pid_r.setTolerance(20.0);
    pid_l.setTolerance(20.0);
    //モータ生成　PID、ECの代入を行うこと
    motorLeft.setEncoder(&ec_l);
    motorRight.setEncoder(&ec_r);

    AirCylinder air_leg[4] = {//名前は分けたほうがいいきがする？
                              AirCylinder(&air[0]),
                              AirCylinder(&air[1]),
                              AirCylinder(&air[2]),
                              AirCylinder(&air[3])};
    AirCylinder air_nonleg[2] = {
        AirCylinder(&air[4]),
        AirCylinder(&air[5])};

    //シリンダ初期設定
    air_leg[0].open();
    air_leg[1].close();//内側の二つが開く
    air_leg[2].close();
    air_leg[3].open();

    leftLeg.setMotor(&motorLeft);
    leftLeg.setPIDcontroller(&pid_l);
    leftLeg.setAir(&air_leg[0], &air_leg[1]);
    rightLeg.setMotor(&motorRight);
    rightLeg.setPIDcontroller(&pid_r);
    rightLeg.setAir(&air_leg[2], &air_leg[3]);

    robot.setLeg(&rightLeg, &leftLeg);
    robot.setTickerTime(0.01); //モータ出力間隔 0.01
    robot.setAirWaitTime(0.2); //シリンダを待つ時間
    
    motorLeft.setDutyLimit(0.2);
    motorRight.setDutyLimit(0.2);

    //初期位置は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(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++)
    {
        air[i] = 1;
    }
    air[4] = 0;
    air[5] = 0;

    servo.init();
    wait_us(1000);
    servo.set_degree(0, 105); //初期角度105

    device.baud(115200);
    device.format(8, Serial::None, 1);
    device.attach(dev_rx, Serial::RxIrq);

    wait(0.05);
    theta0 = degree0;
}
void wait_MR1()
{
    while (switch_x == 1)
    {
    }
    //while(switch_hand==0);
    //air_hand=1;
    wait(1);
}