yotaro morizumi
/
zoomy_customLibrary
my new gear...
IMC_motorDrive.cpp@0:1456b6f84c75, 2022-02-25 (annotated)
- Committer:
- yootee
- Date:
- Fri Feb 25 05:20:11 2022 +0000
- Revision:
- 0:1456b6f84c75
my_custom
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yootee | 0:1456b6f84c75 | 1 | #include<IMC_motorDrive.hpp> |
yootee | 0:1456b6f84c75 | 2 | |
yootee | 0:1456b6f84c75 | 3 | 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_){ |
yootee | 0:1456b6f84c75 | 4 | motor_enc.specinit(encoder.pin[0],encoder.pin[1],wheel_ensyu,resolution,4); |
yootee | 0:1456b6f84c75 | 5 | IMCreset(); |
yootee | 0:1456b6f84c75 | 6 | } |
yootee | 0:1456b6f84c75 | 7 | |
yootee | 0:1456b6f84c75 | 8 | void IMC_motor::IMCdrive(double target){ |
yootee | 0:1456b6f84c75 | 9 | drive((int)imc_out(target)); |
yootee | 0:1456b6f84c75 | 10 | } |
yootee | 0:1456b6f84c75 | 11 | |
yootee | 0:1456b6f84c75 | 12 | void IMC_motor::IMCreset(){ |
yootee | 0:1456b6f84c75 | 13 | out_pwm = 0; |
yootee | 0:1456b6f84c75 | 14 | process_model_val = 0; |
yootee | 0:1456b6f84c75 | 15 | } |
yootee | 0:1456b6f84c75 | 16 | |
yootee | 0:1456b6f84c75 | 17 | void IMC_motor::drive(int pwm){ |
yootee | 0:1456b6f84c75 | 18 | float output_pwm = constrain(pwm,-200,200)/255; |
yootee | 0:1456b6f84c75 | 19 | if(!pwm) { |
yootee | 0:1456b6f84c75 | 20 | DigitalOut Moter1(motor.pin[0],0); |
yootee | 0:1456b6f84c75 | 21 | DigitalOut Moter2(motor.pin[1],0); |
yootee | 0:1456b6f84c75 | 22 | } else if(pwm < 0) { |
yootee | 0:1456b6f84c75 | 23 | PwmOut Moter1(motor.pin[0]); |
yootee | 0:1456b6f84c75 | 24 | Moter1.period_us(256); |
yootee | 0:1456b6f84c75 | 25 | Moter1.write(-output_pwm); |
yootee | 0:1456b6f84c75 | 26 | DigitalOut Moter2(motor.pin[1],0); |
yootee | 0:1456b6f84c75 | 27 | } else { |
yootee | 0:1456b6f84c75 | 28 | DigitalOut Moter1(motor.pin[0],0); |
yootee | 0:1456b6f84c75 | 29 | PwmOut Moter2(motor.pin[1]); |
yootee | 0:1456b6f84c75 | 30 | Moter2.period_us(256); |
yootee | 0:1456b6f84c75 | 31 | Moter2.write(output_pwm); |
yootee | 0:1456b6f84c75 | 32 | } |
yootee | 0:1456b6f84c75 | 33 | } |
yootee | 0:1456b6f84c75 | 34 | |
yootee | 0:1456b6f84c75 | 35 | double IMC_motor::imc_out(double target) |
yootee | 0:1456b6f84c75 | 36 | { |
yootee | 0:1456b6f84c75 | 37 | out_pwm = imc_controler(target-(motor_enc.getSpeed()-process_model(process_model_val,out_pwm))); |
yootee | 0:1456b6f84c75 | 38 | if(target==0){ |
yootee | 0:1456b6f84c75 | 39 | out_pwm = 0.0; |
yootee | 0:1456b6f84c75 | 40 | } |
yootee | 0:1456b6f84c75 | 41 | return out_pwm; |
yootee | 0:1456b6f84c75 | 42 | } |
yootee | 0:1456b6f84c75 | 43 | |
yootee | 0:1456b6f84c75 | 44 | double IMC_motor::imc_controler(double y){ |
yootee | 0:1456b6f84c75 | 45 | return y/K; |
yootee | 0:1456b6f84c75 | 46 | } |
yootee | 0:1456b6f84c75 | 47 | |
yootee | 0:1456b6f84c75 | 48 | double IMC_motor::process_model(double x,double mv) |
yootee | 0:1456b6f84c75 | 49 | { |
yootee | 0:1456b6f84c75 | 50 | process_model_val = x + (-x+K*mv)/(Tau*dt); |
yootee | 0:1456b6f84c75 | 51 | return process_model_val; |
yootee | 0:1456b6f84c75 | 52 | } |