Diff: main.cpp

Revision:

1:e4fe1b3b7376

Parent:

0:fe9f19382f7a

Child:

2:cc875033357d

--- a/main.cpp	Tue Jul 31 22:29:03 2018 +0000
+++ b/main.cpp	Tue May 21 07:12:26 2019 +0000
@@ -16,6 +16,9 @@
     bool controller_enable;
     int des_enc_pulse_rate;  // per control period
     float Kp, Ki;  // gains
+    int error;  // error
+    int acc_error;  // accumulated error;
+
     EventQueue *queue;
 public:
     MotorWithEncoder(PinName _pinPWM, PinName _pinDir, float _gear_ratio, 
@@ -67,13 +70,24 @@
 }
     
 void MotorWithEncoder::updateInputVoltage() {
-    int error = 0;
-    static int acc_error = 0;
+    float temp_input_voltage;
     
     error = des_enc_pulse_rate - enc_pulse_rate;
     acc_error += error;
     
-    input_voltage =  (float)(Kp * error + Ki * acc_error);
+    temp_input_voltage =  (float)(Kp * error + Ki * acc_error);
+    
+    if (temp_input_voltage > 1.0) {
+        input_voltage = 1.0;
+        acc_error -= error;
+    }
+    else if (temp_input_voltage < - 1.0) {
+        input_voltage = - 1.0;
+        acc_error -= error;
+    }
+    else {
+        input_voltage = temp_input_voltage;
+    }
 }
 
 void MotorWithEncoder::applyInputVoltage() {
@@ -105,6 +119,9 @@
 }
 
 void MotorWithEncoder::switchControllerEnable(bool _switch) {
+    error = 0.0;
+    acc_error = 0.0;
+    input_voltage = 0.0;
     controller_enable = _switch;
 }