Diff: src/MotorControl.cpp

Revision:

0:441289ea4e29

Child:

1:7c355adbc977

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/MotorControl.cpp	Thu May 27 18:36:23 2021 +0000
@@ -0,0 +1,78 @@
+/* Karbot motor control class
+ * Written by Simon Krogedal
+ * 27/05/21
+ * Team 9 4th Year project
+ * 
+ * for NUCLEO-F401RE
+ * 
+ */
+ 
+ #include "mbed.h"
+ #include "motor.h"
+ #include "MotorControl.h"
+ 
+ 
+double MotorControl::getError(void) {return (r_clicks - getClicks());}
+
+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;
+    if(output > 1.0) {                                          //limit checks and error flag set
+        output = 1.0;
+        max_out = 1;                                            //this flag says not to increase speed more
+    }
+    else if(output < -1.0) {
+        output = -1.0;
+        max_out = 1;
+    }
+    else
+        max_out = 0;
+    mot->setOut(output);                                        //set new output
+//          pc.printf("output set to %2.2f\n", output);
+    acc_err += 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, i, r, c, p, ec), mot(m), max_speed(ms), Kp(kp), Ki(ki) {
+    output = 0.1; dir = 1;                          //default to avoid bugs
+    max_out = 0;                                    //flag set
+    acc_err = 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) {
+    mot->setOut(output); //pc.printf("initial output set to %2.2f\n", output);
+    mot->drive(); //pc.printf("motor driving\n");
+    //pc.printf("setting sampler at period %2.2f\n", period);
+    sampler.attach(callback(this, &MotorControl::algorithm), period); //pc.printf("sampler set\n");
+}
+
+void MotorControl::stop(void) {
+    mot->stop();
+    sampler.detach();
+    acc_err = 0;
+}
+
+void MotorControl::driveManual(void) {
+    mot->setOut(output); //pc.printf("initial output set to %2.2f\n", output);
+    mot->drive(); //pc.printf("motor driving\n");
+}
+
+void MotorControl::samplecall(void) {algorithm();}
+
+void MotorControl::setK(double k) {Kp = k;}
+
+void MotorControl::start(void) {} //this function is overridden to avoid bugs
+//      double MotorControl::getError(void) {return (speed - getSpeed());}
+double MotorControl::getOutput(void) {return output;}
+bool MotorControl::checkFlag(void) {return max_out;}
\ No newline at end of file