Diff: actuator/IMC_motorDrive.cpp

Revision:

2:e7b09385d197

Parent:

0:1456b6f84c75

Child:

8:1ca5a2052b0c

diff -r 744e1f66d5dc -r e7b09385d197 actuator/IMC_motorDrive.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/actuator/IMC_motorDrive.cpp	Sun Mar 27 04:39:16 2022 +0000
@@ -0,0 +1,52 @@
+#include<IMC_motorDrive.hpp>
+
+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_){
+    motor_enc.specinit(encoder.pin[0],encoder.pin[1],wheel_ensyu,resolution,4);
+    IMCreset();
+}
+
+void IMC_motor::IMCdrive(double target){
+    drive((int)imc_out(target));
+}
+
+void IMC_motor::IMCreset(){
+    out_pwm = 0;
+    process_model_val = 0;    
+}
+
+void IMC_motor::drive(int pwm){
+    float output_pwm = constrain(pwm,-200,200)/255;
+    if(!pwm) {
+        DigitalOut Moter1(motor.pin[0],0);
+        DigitalOut Moter2(motor.pin[1],0);
+    } else if(pwm < 0) {
+        PwmOut Moter1(motor.pin[0]);
+        Moter1.period_us(256);
+        Moter1.write(-output_pwm);
+        DigitalOut Moter2(motor.pin[1],0);
+    } else {
+        DigitalOut Moter1(motor.pin[0],0);
+        PwmOut Moter2(motor.pin[1]);
+        Moter2.period_us(256);
+        Moter2.write(output_pwm);
+    }
+}
+
+double IMC_motor::imc_out(double target)
+{
+    out_pwm = imc_controler(target-(motor_enc.getSpeed()-process_model(process_model_val,out_pwm)));
+    if(target==0){
+        out_pwm = 0.0;    
+    }
+    return out_pwm;
+}
+
+double IMC_motor::imc_controler(double y){
+    return y/K;
+}
+
+double IMC_motor::process_model(double x,double mv)
+{
+    process_model_val = x + (-x+K*mv)/(Tau*dt);
+    return process_model_val;    
+}
\ No newline at end of file