IMC_motorDrive.cpp

00001 #include<IMC_motorDrive.hpp>
00002 
00003 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){
00004     motor_enc.specinit(encoder.pin[0],encoder.pin[1],wheel_ensyu,resolution,4);
00005     IMCreset();
00006 }
00007 
00008 void IMC_motor::IMCdrive(double target){
00009     drive((int)imc_out(target));
00010 }
00011 
00012 void IMC_motor::IMCreset(){
00013     out_pwm = 0;
00014     process_model_val = 0;    
00015 }
00016 
00017 void IMC_motor::drive(int pwm)
00018 {
00019     double output_pwm;
00020     if(pwm < -200) {
00021         output_pwm = -200;
00022     } else if(pwm > 200) {
00023         output_pwm = 200;
00024     } else {
00025         output_pwm = pwm;
00026     }
00027     if(cw_flag) {
00028         if(!pwm) {
00029             DigitalOut Moter1(motor.pin[0],0);
00030             DigitalOut Moter2(motor.pin[1],0);
00031         } else if(pwm < 0) {
00032             PwmOut Moter1(motor.pin[0]);
00033             Moter1.period_us(256);
00034             Moter1.write(-output_pwm/255);
00035             DigitalOut Moter2(motor.pin[1],0);
00036         } else {
00037             DigitalOut Moter1(motor.pin[0],0);
00038             PwmOut Moter2(motor.pin[1]);
00039             Moter2.period_us(256);
00040             Moter2.write(output_pwm/255);
00041         }
00042     }else{
00043         if(!pwm) {
00044             DigitalOut Moter1(motor.pin[0],0);
00045             DigitalOut Moter2(motor.pin[1],0);
00046         } else if(pwm < 0) {
00047             PwmOut Moter1(motor.pin[1]);
00048             Moter1.period_us(256);
00049             Moter1.write(-output_pwm/255);
00050             DigitalOut Moter2(motor.pin[0],0);
00051         } else {
00052             DigitalOut Moter1(motor.pin[1],0);
00053             PwmOut Moter2(motor.pin[0]);
00054             Moter2.period_us(256);
00055             Moter2.write(output_pwm/255);
00056         }    
00057     }
00058 }
00059 
00060 double IMC_motor::imc_out(double target)
00061 {
00062     out_pwm = imc_controler(target-(motor_enc.getSpeed()-process_model(process_model_val,out_pwm)));
00063     if(target==0){
00064         out_pwm = 0.0;    
00065     }
00066     return out_pwm;
00067 }
00068 
00069 double IMC_motor::imc_controler(double y){
00070     return y/K;
00071 }
00072 
00073 double IMC_motor::process_model(double x,double mv)
00074 {
00075     process_model_val = x + (-x+K*mv)/(Tau*dt);
00076     return process_model_val;    
00077 }