Diff: src/MotorControl.cpp

Revision:

3:4b6080e86761

Parent:

1:7c355adbc977

--- a/src/MotorControl.cpp	Thu May 27 19:26:21 2021 +0000
+++ b/src/MotorControl.cpp	Mon May 31 16:44:26 2021 +0000
@@ -17,9 +17,8 @@
 void MotorControl::algorithm(void) {
 //          pc.printf("speed adjusting\n");
     sample_func();
-    double error = getError();
-    double action = Kp * error + Ki * acc_err;                  //computes current error from encoder sample and the resulting action
-    output += action;
+    double error = getError();               
+    output += Kp * (error + Ti*error - prev_error);             //computes current error from encoder sample and the resulting action
     if(output > 1.0) {                                          //limit checks and error flag set
         output = 1.0;
         max_out = 1;                                            //this flag says not to increase speed more
@@ -32,22 +31,20 @@
         max_out = 0;
     mot->setOut(output);                                        //set new output
 //          pc.printf("output set to %2.2f\n", output);
-    acc_err += error;
+    prev_error = error;
 }
 
-MotorControl::MotorControl(PinName a, PinName b, PinName i, int r, bool c, double p, motor* m, double ms, double kp, double ki, double ec)
-            : encoder(a, b, r, c, p, ec), mot(m), max_speed(ms), Kp(kp), Ki(ki) {
+MotorControl::MotorControl(PinName a, PinName b, PinName i, int r, bool c, double p, motor* m, double ms, double kp, double ti, double ec)
+            : encoder(a, b, r, c, p, ec), mot(m), max_speed(ms), Kp(kp), Ti(ti) {
     output = 0.1; dir = 1;                          //default to avoid bugs
     max_out = 0;                                    //flag set
-    acc_err = 0;
+    prev_error = 0;
 }
 
 void MotorControl::setSpeed(double s) { //set speed in m/s. remember to consider the motor max speed
     r_speed = s;
     r_clicks = s / enc_const;
 //          pc.printf("r_clicks set to %2.2f\n", r_clicks);
-    output = s/max_speed;
-    mot->setOut(output);
 }
 
 void MotorControl::drive(void) {
@@ -60,7 +57,7 @@
 void MotorControl::stop(void) {
     mot->stop();
     sampler.detach();
-    acc_err = 0;
+    prev_error = 0;
 }
 
 void MotorControl::driveManual(void) {