Diff: motor_drive.cpp

Revision:

0:aefa71eaf32a

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motor_drive.cpp	Sun Sep 24 05:24:19 2017 +0000
@@ -0,0 +1,36 @@
+#include "motor_drive.h"
+
+MotorDrive::MotorDrive (PinName Direction,PinName Pwm) : direction(Direction), pwm(Pwm){
+    direction = 0;
+    pwm    = 0;
+}
+
+void MotorDrive::setup(float frequency, float acceleration, float timerCycle){
+    float period = 1/frequency;
+    pwm.period(period);
+    a = acceleration*timerCycle;
+}
+
+void MotorDrive::output(float targetDuty){
+    if (fabs(targetDuty-duty) <= a) {
+        duty = targetDuty;
+    } else if (duty < targetDuty) {
+        duty = duty + a;
+    } else if (duty > targetDuty) {
+        duty = duty - a;
+    }
+    
+    if (duty < 0) {
+        direction  = 1;
+        if (duty > DUTY_LIMIT) {
+            duty = DUTY_LIMIT;
+        }
+    } else {
+        direction  = 0;
+        if (duty < -DUTY_LIMIT) {
+            duty = -DUTY_LIMIT;
+        }
+    }
+
+    pwm = fabs(duty);
+}
\ No newline at end of file