yotaro morizumi
/
zoomy_customLibrary
my new gear...
Diff: IMC_motorDrive.cpp
- Revision:
- 0:1456b6f84c75
diff -r 000000000000 -r 1456b6f84c75 IMC_motorDrive.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/IMC_motorDrive.cpp Fri Feb 25 05:20:11 2022 +0000 @@ -0,0 +1,52 @@ +#include<IMC_motorDrive.hpp> + +IMC_motor::IMC_motor(Port encoder,int resolution,double wheel_ensyu,Port motor_,double K_,double Tau_,double dt_):K(K_),Tau(Tau_),dt(dt_),motor(motor_){ + 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){ + float output_pwm = constrain(pwm,-200,200)/255; + 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); + 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); + } +} + +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; +} \ No newline at end of file