File content as of revision 4:1b333860dd41:

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

/***EC classの方を先に読んでおくこと***/


SpeedControl::SpeedControl(PinName signalA , PinName signalB , int s , double t , PinName pwm_F , PinName pwm_B) : Ec(signalA,signalB,s,t) , pwm_F_(pwm_F),pwm_B_(pwm_B) {
    Kv_p=0;Kv_d=0;Kv_i=0;diff=0;diff_old=0;integral=0;
    out_duty=0;out=0;duty=0;
    pwm_F_.period_us(100);
    pwm_B_.period_us(100);
    C=45.0;
}

void SpeedControl::Sc(double target_omega){       //スカンジウムじゃないよ
    double now_omega=omega;
    diff= target_omega-now_omega;
    integral+=diff;
    out_duty=Kv_p*diff+Kv_d*(diff-diff_old)+Kv_i*integral;
    diff_old=diff;
    if(out_duty>0.1)out_duty=0.1;
    if(out_duty<-0.1)out_duty=-0.1;
    out+=out_duty;
    duty=0.0001*out+target_omega/C;
    if(duty<-0.95)duty=-0.95;
    else if(duty>0.95)duty=0.95;
    
    if(duty>=0){
        pwm_F_=duty;
        pwm_B_=0;
    }
    else {
        pwm_F_=0;
        pwm_B_=-duty;
    }
}


void SpeedControl::setPIDparam(double p,double i,double d){
    Kv_p=p;
    Kv_d=d;
    Kv_i=i;
}


void SpeedControl::setDOconstant(double c){
    C=c;
}

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

void SpeedControl::turnF(double duty){
    if(duty>0.95) {
        pwm_F_=0.95;
        pwm_B_=0;
    } else {
        pwm_F_=duty;
        pwm_B_=0;
    }
}

void SpeedControl::turnB(double duty){
    if(duty>0.95) {
        pwm_F_=0;
        pwm_B_=0.95;
    } else {
        pwm_F_=0;
        pwm_B_=duty;
    }
}