yotaro morizumi / Mbed 2 deprecated zoomy_customLibrary

my new gear...

Dependencies:   mbed

actuator/IMC_motorDrive.cpp@22:394337a4205a, 22 months ago (annotated)

Committer:
yootee
Date:
Sat Oct 15 07:52:25 2022 +0000
Revision:
22:394337a4205a
Parent:
21:72dcbbfeb5d8 
upgrade

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