Diff: IMC_motorDrive.cpp

Revision:

0:17acb05d7f3e

diff -r 000000000000 -r 17acb05d7f3e IMC_motorDrive.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMC_motorDrive.cpp	Thu Feb 24 07:22:30 2022 +0000
@@ -0,0 +1,45 @@
+#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);
+    out_pwm = 0;
+    process_model_val = 0;
+}
+
+void IMC_motor::IMCdrive(double target){
+    drive((int)imc_out(target)/200);
+}
+
+double IMC_motor::imc_out(double target)
+{
+    out_pwm = imc_controler(target-(motor_enc.getSpeed()-process_model(process_model_val,out_pwm)));
+    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;    
+}
+
+void IMC_motor::drive(int pwm){
+    pwm = constrain(pwm,-100,100);
+    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(-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(pwm);
+    }
+}
\ No newline at end of file