File content as of revision 1:013870241971:

#include "mbed.h"
#include "AMT21.h"
#include "CalPID.h"
#include "MotorController.h"

MotorController::MotorController(PinName motor_p_, PinName motor_n_,double dt, Amt21 &ec, CalPID &sc_, CalPID &ac_duty, CalPID &ac_omega) : motor_p(motor_p_), motor_n(motor_n_)
{
    ec_ = &ec;
    sc_pid = &sc_;
    ac_duty_pid = &ac_duty;
    ac_omega_pid = &ac_omega;
    motor_p.period_us(50);
    motor_n.period_us(50);
    setDeltaTime(dt);

}
void MotorController::period(double s)
{
    motor_p.period(s);
    motor_n.period(s);
}
void MotorController::period_ms(int ms)
{
    motor_p.period_ms(ms);
    motor_n.period_ms(ms);
}
void MotorController::period_us(int us)
{
    motor_p.period_us(us);
    motor_n.period_us(us);
}
void MotorController::setDutyLimit(double max)
{
    sc_pid->setMaxValue(max);
    ac_duty_pid->setMaxValue(max);
}
void MotorController::setOmegaLimit(double max)
{
    ac_omega_pid->setMaxValue(max);
}
void MotorController::setPDParam_sc(double kp, double kd)
{
    sc_pid->setParameter(kp, 0, kd);
}
void MotorController::setPDParam_ac_duty(double kp, double kd)
{
    ac_duty_pid->setParameter(kp, 0, kd);
}
void MotorController::setPDParam_ac_omega(double kp, double kd)
{
    ac_omega_pid->setParameter(kp, 0, kd);
}
void MotorController::setDeltaTime(double dt)
{
    delta_t=dt;
    sc_pid->setDELTA_T(dt);
    ac_duty_pid->setDELTA_T(dt);
    ac_omega_pid->setDELTA_T(dt);

}
void MotorController::setEquation(double cf,double df,double cb,double db)
{
    Cf_=cf;
    if(cb>0)Cb_=cb;
    else Cb_=-cb;
    initial_Df_=df;
    initial_Db_=-db;
    Df_=initial_Df_;
    Db_=initial_Db_;
}
void MotorController::turn(double duty)
{
    if (duty >= 0) {
        motor_p = duty;
        motor_n = 0;
    } else if (duty < 0) {
        motor_p = 0;
        motor_n = -duty;
    }
}
double MotorController::calSc(double target_omega)
{
    double omega = ec_->getOmega();
    double devia = target_omega - omega;
    double pid = sc_pid->calPD(devia);

    double duty;
    if (target_omega > 0) {
        Df_ += Cf_ * pid;
        duty = Cf_ * target_omega + Df_;
    } else if (target_omega < 0) {
        Db_ += Cb_ * pid;
        duty = Cb_ * target_omega + Db_;
    } else
        duty = 0;
    return duty;
}
void MotorController::Sc(double target_omega)
{
    ec_->rewriteCount();
    ec_->calOmega();

    double duty=calSc(target_omega);

    turn(duty);
}
void MotorController::AcDuty(double target_angle)
{
    ec_->rewriteCount();
    double angle = ec_->getRad();
    double devia = target_angle - angle;
    double duty_omega = ac_duty_pid->calP_D(devia,ec_->getOmega());
    turn(duty_omega);
}
void MotorController::AcOmega(double target_angle)
{
    ec_->rewriteCount();
    ec_->calOmega();
    double angle = ec_->getRad();
    double devia = target_angle - angle;
    double omega = ac_omega_pid->calP_D(devia,ec_->getOmega());
    double duty=calSc(omega);
    if(fabs(devia)<ANGLE_ERROR) {//収束幅の設定
        omega=0;
    }
    turn(duty);
}
void MotorController::stop()
{
    motor_p = 0;
    motor_n = 0;
}