Diff: motor.h

Revision:

0:caa8ee3bd882

Child:

2:df0c6af898ac

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motor.h	Sun Nov 12 00:14:05 2017 +0000
@@ -0,0 +1,78 @@
+#ifndef _MOTOR_H_
+#define _MOTOR_H_
+
+#include "FastPWM.h"
+#include "QEI.h"
+
+#define PWM_PERIOD_US 10
+
+class Motor
+{
+public:
+    Motor(PinName pwm, PinName dir, PinName enc_a, PinName enc_b,
+          int pulses_per_rev, bool invert_dir = false) :
+        encoder_(enc_a, enc_b, NC, NULL, QEI::X4_ENCODING),
+        dir_(dir),
+        speed_(pwm),
+        kPulsesPerRev_(pulses_per_rev),
+        kInvertDir_(invert_dir) 
+    {
+        Stop();
+        offset_ = 0;
+        speed_.period_us(PWM_PERIOD_US);
+    }
+
+    void Stop() {
+        power_ = false;
+        dir_=0;
+        speed_=0;
+    }
+
+    void Start() {
+        power_ = true;
+    }
+
+    bool has_power() {
+        return power_;
+    }
+
+    void set_pwm(double pwm) {
+        if (power_) {
+            if (pwm<0) {
+                dir_ = kInvertDir_;
+            } else {
+                dir_ = !kInvertDir_;
+            }
+            if (abs(pwm)>1) {
+                speed_ = 1;
+            } else {
+                speed_ = abs(pwm);
+            }
+        }
+    }
+
+    double get_angle() {
+        return offset_ + 360.0 * ((double)get_pulses()) / kPulsesPerRev_;
+    }
+
+    void set_angle(double angle) {
+        offset_ = angle - 360.0 * ((double)get_pulses()) / kPulsesPerRev_;
+    }
+
+private:
+
+    QEI encoder_;
+    DigitalOut dir_;
+    FastPWM speed_;
+    const int kPulsesPerRev_;
+    const bool kInvertDir_;
+    bool power_;
+    double offset_;
+
+    int get_pulses() {
+        return kInvertDir_?-encoder_.getPulses():encoder_.getPulses();
+    }
+
+};
+
+#endif
\ No newline at end of file