yotaro morizumi
/
zoomy_customLibrary
my new gear...
actuator/IMC_motorDrive.cpp@21:72dcbbfeb5d8, 2022-09-30 (annotated)
- Committer:
- yootee
- Date:
- Fri Sep 30 00:07:30 2022 +0000
- Revision:
- 21:72dcbbfeb5d8
- Parent:
- 10:b64f586efb2d
okokok
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 | 21:72dcbbfeb5d8 | 17 | void IMC_motor::drive(int pwm) |
yootee | 21:72dcbbfeb5d8 | 18 | { |
yootee | 21:72dcbbfeb5d8 | 19 | double output_pwm; |
yootee | 21:72dcbbfeb5d8 | 20 | if(pwm < -200) { |
yootee | 21:72dcbbfeb5d8 | 21 | output_pwm = -200; |
yootee | 21:72dcbbfeb5d8 | 22 | } else if(pwm > 200) { |
yootee | 21:72dcbbfeb5d8 | 23 | output_pwm = 200; |
yootee | 0:1456b6f84c75 | 24 | } else { |
yootee | 21:72dcbbfeb5d8 | 25 | output_pwm = pwm; |
yootee | 21:72dcbbfeb5d8 | 26 | } |
yootee | 21:72dcbbfeb5d8 | 27 | if(cw_flag) { |
yootee | 21:72dcbbfeb5d8 | 28 | if(!pwm) { |
yootee | 21:72dcbbfeb5d8 | 29 | DigitalOut Moter1(motor.pin[0],0); |
yootee | 21:72dcbbfeb5d8 | 30 | DigitalOut Moter2(motor.pin[1],0); |
yootee | 21:72dcbbfeb5d8 | 31 | } else if(pwm < 0) { |
yootee | 21:72dcbbfeb5d8 | 32 | PwmOut Moter1(motor.pin[0]); |
yootee | 21:72dcbbfeb5d8 | 33 | Moter1.period_us(256); |
yootee | 21:72dcbbfeb5d8 | 34 | Moter1.write(-output_pwm/255); |
yootee | 21:72dcbbfeb5d8 | 35 | DigitalOut Moter2(motor.pin[1],0); |
yootee | 21:72dcbbfeb5d8 | 36 | } else { |
yootee | 21:72dcbbfeb5d8 | 37 | DigitalOut Moter1(motor.pin[0],0); |
yootee | 21:72dcbbfeb5d8 | 38 | PwmOut Moter2(motor.pin[1]); |
yootee | 21:72dcbbfeb5d8 | 39 | Moter2.period_us(256); |
yootee | 21:72dcbbfeb5d8 | 40 | Moter2.write(output_pwm/255); |
yootee | 21:72dcbbfeb5d8 | 41 | } |
yootee | 21:72dcbbfeb5d8 | 42 | }else{ |
yootee | 21:72dcbbfeb5d8 | 43 | if(!pwm) { |
yootee | 21:72dcbbfeb5d8 | 44 | DigitalOut Moter1(motor.pin[0],0); |
yootee | 21:72dcbbfeb5d8 | 45 | DigitalOut Moter2(motor.pin[1],0); |
yootee | 21:72dcbbfeb5d8 | 46 | } else if(pwm < 0) { |
yootee | 21:72dcbbfeb5d8 | 47 | PwmOut Moter1(motor.pin[1]); |
yootee | 21:72dcbbfeb5d8 | 48 | Moter1.period_us(256); |
yootee | 21:72dcbbfeb5d8 | 49 | Moter1.write(-output_pwm/255); |
yootee | 21:72dcbbfeb5d8 | 50 | DigitalOut Moter2(motor.pin[0],0); |
yootee | 21:72dcbbfeb5d8 | 51 | } else { |
yootee | 21:72dcbbfeb5d8 | 52 | DigitalOut Moter1(motor.pin[1],0); |
yootee | 21:72dcbbfeb5d8 | 53 | PwmOut Moter2(motor.pin[0]); |
yootee | 21:72dcbbfeb5d8 | 54 | Moter2.period_us(256); |
yootee | 21:72dcbbfeb5d8 | 55 | Moter2.write(output_pwm/255); |
yootee | 21:72dcbbfeb5d8 | 56 | } |
yootee | 0:1456b6f84c75 | 57 | } |
yootee | 0:1456b6f84c75 | 58 | } |
yootee | 0:1456b6f84c75 | 59 | |
yootee | 0:1456b6f84c75 | 60 | double IMC_motor::imc_out(double target) |
yootee | 0:1456b6f84c75 | 61 | { |
yootee | 0:1456b6f84c75 | 62 | out_pwm = imc_controler(target-(motor_enc.getSpeed()-process_model(process_model_val,out_pwm))); |
yootee | 0:1456b6f84c75 | 63 | if(target==0){ |
yootee | 0:1456b6f84c75 | 64 | out_pwm = 0.0; |
yootee | 0:1456b6f84c75 | 65 | } |
yootee | 0:1456b6f84c75 | 66 | return out_pwm; |
yootee | 0:1456b6f84c75 | 67 | } |
yootee | 0:1456b6f84c75 | 68 | |
yootee | 0:1456b6f84c75 | 69 | double IMC_motor::imc_controler(double y){ |
yootee | 0:1456b6f84c75 | 70 | return y/K; |
yootee | 0:1456b6f84c75 | 71 | } |
yootee | 0:1456b6f84c75 | 72 | |
yootee | 0:1456b6f84c75 | 73 | double IMC_motor::process_model(double x,double mv) |
yootee | 0:1456b6f84c75 | 74 | { |
yootee | 0:1456b6f84c75 | 75 | process_model_val = x + (-x+K*mv)/(Tau*dt); |
yootee | 0:1456b6f84c75 | 76 | return process_model_val; |
yootee | 0:1456b6f84c75 | 77 | } |