File content as of revision 21:72dcbbfeb5d8:

#include<IMC_motorDrive.hpp>

IMC_motor::IMC_motor(Port encoder,int resolution,double wheel_ensyu,Port motor_,double K_,double Tau_,double dt_,bool CW_flag):K(K_),Tau(Tau_),dt(dt_),motor(motor_),cw_flag(CW_flag){
    motor_enc.specinit(encoder.pin[0],encoder.pin[1],wheel_ensyu,resolution,4);
    IMCreset();
}

void IMC_motor::IMCdrive(double target){
    drive((int)imc_out(target));
}

void IMC_motor::IMCreset(){
    out_pwm = 0;
    process_model_val = 0;    
}

void IMC_motor::drive(int pwm)
{
    double output_pwm;
    if(pwm < -200) {
        output_pwm = -200;
    } else if(pwm > 200) {
        output_pwm = 200;
    } else {
        output_pwm = pwm;
    }
    if(cw_flag) {
        if(!pwm) {
            DigitalOut Moter1(motor.pin[0],0);
            DigitalOut Moter2(motor.pin[1],0);
        } else if(pwm < 0) {
            PwmOut Moter1(motor.pin[0]);
            Moter1.period_us(256);
            Moter1.write(-output_pwm/255);
            DigitalOut Moter2(motor.pin[1],0);
        } else {
            DigitalOut Moter1(motor.pin[0],0);
            PwmOut Moter2(motor.pin[1]);
            Moter2.period_us(256);
            Moter2.write(output_pwm/255);
        }
    }else{
        if(!pwm) {
            DigitalOut Moter1(motor.pin[0],0);
            DigitalOut Moter2(motor.pin[1],0);
        } else if(pwm < 0) {
            PwmOut Moter1(motor.pin[1]);
            Moter1.period_us(256);
            Moter1.write(-output_pwm/255);
            DigitalOut Moter2(motor.pin[0],0);
        } else {
            DigitalOut Moter1(motor.pin[1],0);
            PwmOut Moter2(motor.pin[0]);
            Moter2.period_us(256);
            Moter2.write(output_pwm/255);
        }    
    }
}

double IMC_motor::imc_out(double target)
{
    out_pwm = imc_controler(target-(motor_enc.getSpeed()-process_model(process_model_val,out_pwm)));
    if(target==0){
        out_pwm = 0.0;    
    }
    return out_pwm;
}

double IMC_motor::imc_controler(double y){
    return y/K;
}

double IMC_motor::process_model(double x,double mv)
{
    process_model_val = x + (-x+K*mv)/(Tau*dt);
    return process_model_val;    
}