File content as of revision 0:cca61e773cbb:

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

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


SpeedControl::SpeedControl(PinName signalA , PinName signalB , PinName signalZ , int s , double t , PinName pwm_F , PinName pwm_B) : Ec(signalA,signalB,signalZ,s,t) , pwm_F_(pwm_F),pwm_B_(pwm_B) {
    
    Kv_p=0;Kv_d=0;diff=0;diff_old=0;now_time_=0;old_time_=0;
    now_omega=0;now_RPM=0;
    out_duty=0;out_plus=0;out_minus=0;duty=0;sth_debug=0;
    pwm_F_.period_us(50);
    pwm_B_.period_us(50);
    Cf=45.0;
    Df=0;
    Cb=45.0;
    Db=0;
}

void SpeedControl::Sc(double target_omega){       //スカンジウムじゃないよ
    now_omega=omega;
    now_time_=timer.read();
    diff= target_omega-now_omega;
    out_duty=Kv_p*diff+Kv_d*(diff-diff_old)/(now_time_-old_time_);
    diff_old=diff;
    if(out_duty>0.1)out_duty=0.1;
    if(out_duty<-0.1)out_duty=-0.1;
    if(target_omega>0){
        if((duty>=-0.8)&&(duty<=0.8)) out_plus+=out_duty;
        duty=0.00002*out_plus+target_omega*Cf+Df;
        if(duty<0)duty=0;
    }else{
        if((duty>=-0.8)&&(duty<=0.8)) out_minus+=out_duty;
        duty=0.00002*out_minus+target_omega*Cb-Db;
        if(duty>0)duty=0;
    }
    if(duty<-0.8)duty=-0.8;
    else if(duty>0.8)duty=0.8;
    old_time_=now_time_;
    
    if(duty>=0){
        pwm_F_=duty;
        pwm_B_=0;
    } else {
        pwm_F_=0;
        pwm_B_=-duty;
    }
}

void SpeedControl::turnF(double duty){
    if(duty>0.95) {
        pwm_F_=0.95;
        pwm_B_=0;
    } else if(duty<0) {
        pwm_F_=0;
        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 if(duty<0) {
        pwm_F_=0;
        pwm_B_=0;
    } else {
        pwm_F_=0;
        pwm_B_=duty;
    }
}

//RC2017で一時期使っていたがあまり出来が良くないので消し飛ばしました。ただ完全に消去するのもあれなので一応コメントアウトという形で残したよ
/*void SpeedControl::Accelarate(double target_duty){
    double now_speed,old_speed;
    double duty;
    int start_point;
    int max_point=int(target_duty/0.05);
    int now_point=int((double)pwm_F_/0.05);
    if(now_point<3) start_point=3;
    else start_point=now_point;
    
    if(max_point>19) max_point=19;
    else if(max_point<0) max_point=0;
    if(max_point>now_point){
        for(int i=start_point;i<=max_point;i++){
            duty=(double)i*5.0/100.0;
            printf("duty = %f RPM = %f\r\n",(double)i*5.0/100.0,getRPM());
            turnF(duty);
            int count=0;
            while(1){
                old_speed=now_speed;
                wait(0.01);
                now_speed=getRPM();
                if(now_speed<old_speed) {
                    if(count>1) break;
                    else count++;
                }
            }
        }
    }
    else if(max_point<now_point){
        for(int i=now_point;i>=max_point;i--){
            duty=(double)i*5.0/100.0;
            printf("duty = %f RPM = %f\r\n",(double)i*5.0/100.0,getRPM());
            turnF(duty);
            int count=0;
            while(1){
                old_speed=now_speed;
                wait(0.01);
                now_speed=getRPM();
                if(now_speed>old_speed) {
                    if(count>1) break;
                    else count++;
                }
            }
        }
    }
}*/

/*void SpeedControl::ScZ(double target_RPM){
    now_RPM=getRPM();
    //if(fabs(now_RPM-target_RPM)>300) Accelarate(target_RPM/C);
    diff=target_RPM-now_RPM;
    integral+=diff;
    out_duty=0.01*(Kv_p*diff+Kv_d*(diff-diff_old));
    diff_old=diff;
    if(out_duty>0.04)out_duty=0.04;
    if(out_duty<-0.04)out_duty=-0.04;
    if((duty>=0)&&(duty<0.95)) out+=out_duty;
    duty=0.0001*out+target_RPM/C;
    if(duty>=0.95) {
        duty=0.94;
        out=0.94/0.0001;
    }
    turnF(duty);
}*/

/*void SpeedControl::ScZ2(double target_RPM){
    now_time_=timer.read();
    now_RPM=getRPM();
    diff=target_RPM-now_RPM;
    out_duty=(now_time_-old_time_)*Kv_p*diff+Kv_d*(diff-diff_old)/(now_time_-old_time_);
    old_time_=now_time_;
    diff_old=diff;
    if(out_duty>0.001)out_duty=0.001;
    if(out_duty<-0.001)out_duty=-0.001;
    if((duty>0)&&(duty<0.95)) out+=out_duty;
    duty=0.001*out;
    turnF(duty);
}*/
    

void SpeedControl::setPDparam(double p,double d){
    Kv_p=p;
    Kv_d=d;
}


void SpeedControl::setDOconstant(double cf,double df,double cb,double db){
    Cf=1/cf;
    Df=df;
    Cb=1/cb;
    Db=db;
}

void SpeedControl::reset(){
    S=0;stateA=0;stateB=0;count=0;pre_count=0.0,omega=0;
    rev=0;now_time=0;old_time=0;RPM=0;RPM_old=0;
    diff=0;diff_old=0;now_time_=0;old_time_=0;
    out_plus=0;out_minus=0;out_duty=0;duty=0;
    timer.stop();
    timer.reset();
    timer.start();
}

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