yotaro morizumi
/
zoomy_customLibrary
my new gear...
actuator/IMC_motorDrive.cpp@8:1ca5a2052b0c, 2022-06-10 (annotated)
- Committer:
- yootee
- Date:
- Fri Jun 10 08:09:09 2022 +0000
- Revision:
- 8:1ca5a2052b0c
- Parent:
- 2:e7b09385d197
- Child:
- 10:b64f586efb2d
unt6i
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 | 8:1ca5a2052b0c | 3 | 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){ |
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 | 8:1ca5a2052b0c | 19 | if(cw_flag){ |
yootee | 8:1ca5a2052b0c | 20 | pwm = -pwm; |
yootee | 8:1ca5a2052b0c | 21 | } |
yootee | 0:1456b6f84c75 | 22 | if(!pwm) { |
yootee | 0:1456b6f84c75 | 23 | DigitalOut Moter1(motor.pin[0],0); |
yootee | 0:1456b6f84c75 | 24 | DigitalOut Moter2(motor.pin[1],0); |
yootee | 0:1456b6f84c75 | 25 | } else if(pwm < 0) { |
yootee | 0:1456b6f84c75 | 26 | PwmOut Moter1(motor.pin[0]); |
yootee | 0:1456b6f84c75 | 27 | Moter1.period_us(256); |
yootee | 0:1456b6f84c75 | 28 | Moter1.write(-output_pwm); |
yootee | 0:1456b6f84c75 | 29 | DigitalOut Moter2(motor.pin[1],0); |
yootee | 0:1456b6f84c75 | 30 | } else { |
yootee | 0:1456b6f84c75 | 31 | DigitalOut Moter1(motor.pin[0],0); |
yootee | 0:1456b6f84c75 | 32 | PwmOut Moter2(motor.pin[1]); |
yootee | 0:1456b6f84c75 | 33 | Moter2.period_us(256); |
yootee | 0:1456b6f84c75 | 34 | Moter2.write(output_pwm); |
yootee | 0:1456b6f84c75 | 35 | } |
yootee | 0:1456b6f84c75 | 36 | } |
yootee | 0:1456b6f84c75 | 37 | |
yootee | 0:1456b6f84c75 | 38 | double IMC_motor::imc_out(double target) |
yootee | 0:1456b6f84c75 | 39 | { |
yootee | 0:1456b6f84c75 | 40 | out_pwm = imc_controler(target-(motor_enc.getSpeed()-process_model(process_model_val,out_pwm))); |
yootee | 0:1456b6f84c75 | 41 | if(target==0){ |
yootee | 0:1456b6f84c75 | 42 | out_pwm = 0.0; |
yootee | 0:1456b6f84c75 | 43 | } |
yootee | 0:1456b6f84c75 | 44 | return out_pwm; |
yootee | 0:1456b6f84c75 | 45 | } |
yootee | 0:1456b6f84c75 | 46 | |
yootee | 0:1456b6f84c75 | 47 | double IMC_motor::imc_controler(double y){ |
yootee | 0:1456b6f84c75 | 48 | return y/K; |
yootee | 0:1456b6f84c75 | 49 | } |
yootee | 0:1456b6f84c75 | 50 | |
yootee | 0:1456b6f84c75 | 51 | double IMC_motor::process_model(double x,double mv) |
yootee | 0:1456b6f84c75 | 52 | { |
yootee | 0:1456b6f84c75 | 53 | process_model_val = x + (-x+K*mv)/(Tau*dt); |
yootee | 0:1456b6f84c75 | 54 | return process_model_val; |
yootee | 0:1456b6f84c75 | 55 | } |