Diff: actuator/IMC_motorDrive.cpp

Revision:

21:72dcbbfeb5d8

Parent:

10:b64f586efb2d

--- a/actuator/IMC_motorDrive.cpp	Fri Sep 30 00:04:11 2022 +0000
+++ b/actuator/IMC_motorDrive.cpp	Fri Sep 30 00:07:30 2022 +0000
@@ -6,9 +6,6 @@
 }
 
 void IMC_motor::IMCdrive(double target){
-    if(cw_flag){
-        target = -target;    
-    }
     drive((int)imc_out(target));
 }
 
@@ -17,21 +14,46 @@
     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);
+void IMC_motor::drive(int pwm)
+{
+    double output_pwm;
+    if(pwm < -200) {
+        output_pwm = -200;
+    } else if(pwm > 200) {
+        output_pwm = 200;
     } else {
-        DigitalOut Moter1(motor.pin[0],0);
-        PwmOut Moter2(motor.pin[1]);
-        Moter2.period_us(256);
-        Moter2.write(output_pwm);
+        output_pwm = pwm;
+    }
+    if(cw_flag) {
+        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/255);
+            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/255);
+        }
+    }else{
+        if(!pwm) {
+            DigitalOut Moter1(motor.pin[0],0);
+            DigitalOut Moter2(motor.pin[1],0);
+        } else if(pwm < 0) {
+            PwmOut Moter1(motor.pin[1]);
+            Moter1.period_us(256);
+            Moter1.write(-output_pwm/255);
+            DigitalOut Moter2(motor.pin[0],0);
+        } else {
+            DigitalOut Moter1(motor.pin[1],0);
+            PwmOut Moter2(motor.pin[0]);
+            Moter2.period_us(256);
+            Moter2.write(output_pwm/255);
+        }    
     }
 }