File content as of revision 10:daf0e763bf35:

#include "mbed.h"
#include "EC.h"
#include "SpeedController.h"

SpeedControl::SpeedControl(PinName pwm_F,PinName pwm_B,int us,Ec &ec) :
    Kp_(0),Kd_(0),pre_diff(0),ptsc_(0),Cf_(1/45),Cb_(1/45),Df_(0),Db_(0),initial_Df_(0),initial_Db_(0),duty_limit_(0.95),pwm_F_(pwm_F),pwm_B_(pwm_B)
{
    ec_=&ec;
    pwm_F_.period_us(us);
    pwm_B_.period_us(us);
}
void SpeedControl::period(double s)
{
    pwm_F_.period(s);
    pwm_B_.period(s);
}
void SpeedControl::period_ms(int ms)
{
    pwm_F_.period_ms(ms);
    pwm_B_.period_ms(ms);
}
void SpeedControl::period_us(int us)
{
    pwm_F_.period_us(us);
    pwm_B_.period_us(us);
}


void SpeedControl::Sc(double target_omega)        //スカンジウムじゃないよ
{
    double t=ec_->timer_.read();
    ec_->calOmega();
    double omega=ec_->getOmega();
    double diff=target_omega-omega;
    double pid=Kp_*diff+Kd_*(diff-pre_diff)/(t-ptsc_);
    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;
    turn(duty);
    pre_diff=diff;
    ptsc_=t;
}

void SpeedControl::turn(double duty)
{
    if(duty>duty_limit_)duty=duty_limit_;
    else if(duty<-duty_limit_)duty=-duty_limit_;

    if(duty>=0) {
        pwm_F_=duty;
        pwm_B_=0;
    } else if(duty<0) {
        pwm_F_=0;
        pwm_B_=-duty;
    }
}
double SpeedControl::getDutyF(){
    return pwm_F_.read();
}

double SpeedControl::getDutyB(){
    return pwm_B_.read();
}

void SpeedControl::setPDparam(double kp,double kd)
{
    Kp_=kp;
    Kd_=kd;
}


void SpeedControl::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 SpeedControl::setDutyLimit(double duty_limit)
{
    if(0<=duty_limit && duty_limit<0.95f)duty_limit_=duty_limit;
    else duty_limit_=0.95f;
}

void SpeedControl::reset()
{
    ec_->reset();
    pre_diff=0;
    ptsc_=0;
    Df_=initial_Df_;
    Db_=initial_Db_;
}

void SpeedControl::stop()
{
    pwm_F_=0;
    pwm_B_=0;
}