![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
my new gear...
actuator/IMC_motorDrive.cpp
- Committer:
- yootee
- Date:
- 23 months ago
- Revision:
- 21:72dcbbfeb5d8
- Parent:
- 10:b64f586efb2d
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; }