yotaro morizumi / Mbed 2 deprecated zoomy_customLibrary

my new gear...

Dependencies:   mbed

actuator/IMC_motorDrive.cpp@10:b64f586efb2d, 2022-06-10 (annotated)

Committer:
yootee
Date:
Fri Jun 10 08:54:32 2022 +0000
Revision:
10:b64f586efb2d
Parent:
8:1ca5a2052b0c
Child:
21:72dcbbfeb5d8
unti

Who changed what in which revision?

UserRevisionLine numberNew 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 10:b64f586efb2d 9 if(cw_flag){
yootee 10:b64f586efb2d 10 target = -target;
yootee 10:b64f586efb2d 11 }
yootee 0:1456b6f84c75 12 drive((int)imc_out(target));
yootee 0:1456b6f84c75 13 }
yootee 0:1456b6f84c75 14
yootee 0:1456b6f84c75 15 void IMC_motor::IMCreset(){
yootee 0:1456b6f84c75 16 out_pwm = 0;
yootee 0:1456b6f84c75 17 process_model_val = 0;
yootee 0:1456b6f84c75 18 }
yootee 0:1456b6f84c75 19
yootee 0:1456b6f84c75 20 void IMC_motor::drive(int pwm){
yootee 0:1456b6f84c75 21 float output_pwm = constrain(pwm,-200,200)/255;
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 }