Diff: P_motor.cpp

Revision:

0:29872002539b

Child:

1:d0b60b54e3bd

diff -r 000000000000 -r 29872002539b P_motor.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/P_motor.cpp	Fri Oct 20 09:56:12 2017 +0000
@@ -0,0 +1,97 @@
+#include "P_motor.h"
+
+#define R 16
+#define L 4
+#define C 0.04
+#define PI 3.141592654
+
+
+P_motor::P_motor(I2C *i2c_, char addr_)
+    : motor(i2c_, addr_), pc(USBTX, USBRX)
+{
+    pc.baud(115200);
+    RC = R * C;
+    LC = L * C;
+    timer_reset();
+    output_reset();
+}
+
+P_motor& P_motor::operator=(double target_)
+{
+    target = target_;
+    compute();
+    return *this;
+}
+
+void P_motor::run()
+{
+    motor = output_m;
+    pc.printf("%+3f\n\r", output_m);
+}
+
+void P_motor::compute()
+{
+    stat_old = stat;
+    double time_now = timer.read_ms();
+    double delta = (time_now - time_old) / 1000.0f;
+    if(target > 0.01)
+        stat = FORWARD;
+    else if(target < -0.01)
+        stat = BACKWARD;
+    else
+        free();
+    if(stat)
+    {
+        if(stat * stat_old < 0)
+        {
+            //output[1] = -target / 2;
+            free();
+        }
+        else
+        {
+            output[2] = (delta * delta * target
+                    + output[1] * (delta * RC + 2 * LC)
+                    - output[0] * LC)
+                    / (delta * RC + LC + delta * delta);
+            output[0] = output[1];
+            output[1] = output[2];
+            time_old = time_now;
+        }
+        if(target * output[2] < 1)
+            output_m = output[2];
+        else
+            output_m = target * 1;
+    }
+}
+
+void P_motor::free()
+{
+    motor.free();
+    timer_reset();
+    output_reset();
+    stat = NONE;
+}
+
+void P_motor::stop()
+{
+    motor.stop();
+    timer_reset();
+    output_reset();
+    stat = NONE;
+}
+
+void P_motor::timer_reset()
+{
+    timer.stop();
+    timer.reset();
+    time_old = 0;
+    timer.start();
+}
+
+void P_motor::output_reset(double value)
+{
+    output[0] = value;
+    output[1] = value;
+    output[2] = value;
+    output_m = value;
+}
\ No newline at end of file