Diff: Output/Motor/Motor.cpp

Revision:

0:562021ed1ba9

Child:

1:e73cf2469f83

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Output/Motor/Motor.cpp	Sat Jan 13 13:33:09 2018 +0000
@@ -0,0 +1,82 @@
+#include "Motor.h"
+
+#include <stdint.h>
+#include "mbed.h"
+
+namespace MOTOR
+{
+    namespace
+    {
+        MotorStatus motor[MOUNTING_MOTOR_NUM];
+        DigitalOut directions[] = {
+            DigitalOut(MOTOR0_D1_PIN),
+            DigitalOut(MOTOR0_D2_PIN),
+            DigitalOut(MOTOR1_D1_PIN),
+            DigitalOut(MOTOR1_D2_PIN),
+            DigitalOut(MOTOR2_D1_PIN),
+            DigitalOut(MOTOR2_D2_PIN),
+            DigitalOut(MOTOR3_D1_PIN),
+            DigitalOut(MOTOR3_D2_PIN),
+            DigitalOut(MOTOR4_D1_PIN),
+            DigitalOut(MOTOR4_D2_PIN),
+            DigitalOut(MOTOR5_D1_PIN),
+            DigitalOut(MOTOR5_D2_PIN),
+        };
+        PwmOut pwms[] = {
+            PwmOut(MOTOR0_PWM_PIN),
+            PwmOut(MOTOR1_PWM_PIN),
+            PwmOut(MOTOR2_PWM_PIN),
+            PwmOut(MOTOR3_PWM_PIN),
+            PwmOut(MOTOR4_PWM_PIN),
+            PwmOut(MOTOR5_PWM_PIN),
+        };
+    }
+
+    float percentage_to_ratio(float percentage);
+
+    void Motor::Initialize(void) {
+        //Port Initialize
+        for(uint8_t i = 0; i < MOUNTING_MOTOR_NUM * 2; i++) {
+            directions[i] = 0;
+        }
+
+        //Pwm Initialize
+        for(uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) {
+            pwms[i].period_us(50);  //20kHz
+            pwms[i] = 0.0;
+        }
+
+        SetDefault();
+    }
+
+    void Motor::SetDefault(void) {
+        for(uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) {
+            motor[i].dir = FREE;
+            motor[i].pwm = 0;
+        }
+    }
+
+    void Motor::Update(MotorStatus *status) {
+        for(uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) motor[i] = status[i];
+
+        //PWM Update
+        for(uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) pwms[i] = percentage_to_ratio(motor[i].pwm);
+
+        //Port Update
+        for(uint8_t i = 0; i < MOUNTING_MOTOR_NUM; i++) {
+            directions[i * 2]       = motor[i].d1;
+            directions[i * 2 + 1]   = motor[i].d2;
+        }
+    }
+
+    float percentage_to_ratio(float percentage) {
+        return percentage / 100.0;
+    }
+
+    int Motor::SetStatus(float pwm) {
+        if(pwm > 0.0)       return FOR;
+        else if(pwm < 0.0)  return BACK;
+        else                return BRAKE;
+    }
+}
+