File content as of revision 5:2e5f40e761bd:

#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_2();
    ec_->calOmega();

    double duty=calSc(target_omega);

    turn(duty);
}
void MotorController::AcDuty(double target_angle)
{
    ec_->rewriteCount_2();
    //double angle = 62*3.14159265359f/180 - ec_->getRad()/3;
    double angle = ec_->getRad()*36/60 - 6.4*3.14159265359f/180;
    double devia = target_angle - angle;
    double duty_omega = ac_duty_pid->calP_D(devia,ec_->getOmega());
    printf("%f\t%f\t%f\r\n",duty_omega, target_angle, angle);
    turn(duty_omega);
}
void MotorController::AcOmega_1(double target_angle,double max_duty)
{
    ec_->rewriteCount_1();
    ec_->calOmega();
    double angle = ec_->getRad()/3 + 159.72*3.14159265359f/180;
    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;
    }
    //printf("duty_1:%f\t",duty);
    if(duty > max_duty) duty = max_duty;
    if(duty < -max_duty)duty = -max_duty;
    turn(duty);
}
void MotorController::AcOmega_2(double target_angle,double angleA,double max_duty)
{
    ec_->rewriteCount_2();
    ec_->calOmega();
    double angle;
    if(angleA >= 0){
            angle = ec_->getRad()*36/60 + (-115 - angleA/3 - angleA*16.63/90)*3.14159265359f/180;
        }else if(angleA < 0){
            angle = ec_->getRad()*36/60 + (-115 - angleA/3 + angleA*16.63/90)*3.14159265359f/180;
    }
    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;
    }
    //printf("duty_2:%f   ",duty);
    if(duty > max_duty) duty = max_duty;
    if(duty < -max_duty)duty = -max_duty;
    turn(duty);
}

void MotorController::AcOmega(double target_angle)
{
    ec_->rewriteCount_2();
    ec_->calOmega();
    double angle = ec_->getRad()*36/60 - 6.4*3.14159265359f/180;
    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;
    }
    //printf("%f\t%f\t%f\t\r\n",duty, target_angle, angle);
    if(duty > 0.2) duty = 0.2;
    if(duty < -0.2)duty = -0.2;
    turn(duty);
}

void MotorController::stop()
{
    motor_p = 0;
    motor_n = 0;
}