yotaro morizumi / IMC_motordrive

wakaran

Dependencies:  

IMC_motorDrive.cpp@1:5b408b893c20, 2022-02-24 (annotated)

Committer:
yootee
Date:
Thu Feb 24 07:23:14 2022 +0000
Revision:
1:5b408b893c20
Parent:
0:17acb05d7f3e 
fuck

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yootee 0:17acb05d7f3e 1 #include<IMC_motorDrive.hpp>
yootee 0:17acb05d7f3e 2
yootee 0:17acb05d7f3e 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:17acb05d7f3e 4 motor_enc.specinit(encoder.pin[0],encoder.pin[1],wheel_ensyu,resolution,4);
yootee 0:17acb05d7f3e 5 out_pwm = 0;
yootee 0:17acb05d7f3e 6 process_model_val = 0;
yootee 0:17acb05d7f3e 7 }
yootee 0:17acb05d7f3e 8
yootee 0:17acb05d7f3e 9 void IMC_motor::IMCdrive(double target){
yootee 0:17acb05d7f3e 10 drive((int)imc_out(target)/200);
yootee 0:17acb05d7f3e 11 }
yootee 0:17acb05d7f3e 12
yootee 0:17acb05d7f3e 13 double IMC_motor::imc_out(double target)
yootee 0:17acb05d7f3e 14 {
yootee 0:17acb05d7f3e 15 out_pwm = imc_controler(target-(motor_enc.getSpeed()-process_model(process_model_val,out_pwm)));
yootee 0:17acb05d7f3e 16 return out_pwm;
yootee 0:17acb05d7f3e 17 }
yootee 0:17acb05d7f3e 18
yootee 0:17acb05d7f3e 19 double IMC_motor::imc_controler(double y){
yootee 0:17acb05d7f3e 20 return y/K;
yootee 0:17acb05d7f3e 21 }
yootee 0:17acb05d7f3e 22
yootee 0:17acb05d7f3e 23 double IMC_motor::process_model(double x,double mv)
yootee 0:17acb05d7f3e 24 {
yootee 0:17acb05d7f3e 25 process_model_val = x + (-x+K*mv)/(Tau*dt);
yootee 0:17acb05d7f3e 26 return process_model_val;
yootee 0:17acb05d7f3e 27 }
yootee 0:17acb05d7f3e 28
yootee 0:17acb05d7f3e 29 void IMC_motor::drive(int pwm){
yootee 0:17acb05d7f3e 30 pwm = constrain(pwm,-100,100);
yootee 0:17acb05d7f3e 31 if(!pwm) {
yootee 0:17acb05d7f3e 32 DigitalOut Moter1(motor.pin[0],0);
yootee 0:17acb05d7f3e 33 DigitalOut Moter2(motor.pin[1],0);
yootee 0:17acb05d7f3e 34 } else if(pwm < 0) {
yootee 0:17acb05d7f3e 35 PwmOut Moter1(motor.pin[0]);
yootee 0:17acb05d7f3e 36 Moter1.period_us(256);
yootee 0:17acb05d7f3e 37 Moter1.write(-pwm);
yootee 0:17acb05d7f3e 38 DigitalOut Moter2(motor.pin[1],0);
yootee 0:17acb05d7f3e 39 } else {
yootee 0:17acb05d7f3e 40 DigitalOut Moter1(motor.pin[0],0);
yootee 0:17acb05d7f3e 41 PwmOut Moter2(motor.pin[1]);
yootee 0:17acb05d7f3e 42 Moter2.period_us(256);
yootee 0:17acb05d7f3e 43 Moter2.write(pwm);
yootee 0:17acb05d7f3e 44 }
yootee 0:17acb05d7f3e 45 }