File content as of revision 24:8ac46a6d2ac4:

#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;Kv_i=0;diff=0;diff_old=0;integral=0;now_time_=0;old_time_=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){       //スカンジウムじゃないよ
    now_omega=omega;
    now_time_=timer.read();
    diff= target_omega-now_omega;
    integral+=diff;
    out_duty=Kv_p*diff+Kv_d*(diff-diff_old)/(now_time_-old_time_)+Kv_i*integral*(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((duty>0)&&(duty<0.95)) 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;
    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 {
        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;
    }
}

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)+Kv_i*integral);
    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;
    integral+=diff;
    out_duty=(now_time_-old_time_)*Kv_p*diff+Kv_d*(diff-diff_old)/(now_time_-old_time_)+Kv_i*integral;
    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::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::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;integral=0;now_time_=0;old_time_=0;
    out=0;out_duty=0;
}

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