yotaro morizumi / Mbed 2 deprecated zoomy_customLibrary

my new gear...

Dependencies:   mbed

actuator/IMC_motorDrive.cpp@2:e7b09385d197, 2022-03-27 (annotated)

Committer:
yootee
Date:
Sun Mar 27 04:39:16 2022 +0000
Revision:
2:e7b09385d197
Parent:
IMC_motorDrive.cpp@0:1456b6f84c75
Child:
8:1ca5a2052b0c
arrc?

Who changed what in which revision?

UserRevisionLine numberNew 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 }