Diff: Motor/Motor.cpp

Revision:

2:32d2cd7d744b

Parent:

1:5b0303768126

--- a/Motor/Motor.cpp	Tue Jan 21 11:10:33 2020 +0000
+++ b/Motor/Motor.cpp	Thu Jan 23 09:16:38 2020 +0000
@@ -1,30 +1,86 @@
 #include "Motor.h"
 
+#include <stdint.h>
 #include "mbed.h"
 
-MOTOR::MOTOR(PinName D1, PinName D2, PinName pwm)
-    :D1_(D1), D2_(D2), pwm_(pwm)
-{
-    D1_.write(0);
-    D2_.write(0);
-    pwm_.period_us(50);  // 20kHz
-    pwm_.write(0);
-}
-
-void MOTOR::Dir(dire mode, uint8_t pwm)
+namespace MOTOR
 {
-    D1_.write((char)mode & 0x01);
-    D2_.write((char)mode & 0x02);
-    pwm_.write((float)(pwm / 255.0));
-}
+    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),
+        };
+        PwmOut pwms[] = {
+            PwmOut(MOTOR0_PWM_PIN),
+            PwmOut(MOTOR1_PWM_PIN),
+            PwmOut(MOTOR2_PWM_PIN),
+            PwmOut(MOTOR3_PWM_PIN),
+            PwmOut(MOTOR4_PWM_PIN),
+        };
+    }
+
+    float percentage_to_ratio(float percentage);
+
+    void Motor::Initialize(void) {
+        //Port Initialize
+        for(uint8_t i = MOTOR_START_NUM; i < MOUNTING_MOTOR_NUM * 2; i++) {
+            directions[i] = 0;
+        }
+
+        //Pwm Initialize
+        for(uint8_t i = MOTOR_START_NUM; i < MOUNTING_MOTOR_NUM; i++) {
+            pwms[i].period_us(50);  //20kHz
+            pwms[i] = 0.0;
+        }
 
-void MOTOR::Dir(dire mode)
-{
-    D1_.write((char)mode & 0x01);
-    D2_.write((char)mode & 0x02);
+        SetDefault();
+    }
+
+    void Motor::SetDefault(void) {
+        for(uint8_t i = MOTOR_START_NUM; i < MOUNTING_MOTOR_NUM; i++) {
+            motor[i].dir = FREE;
+            motor[i].pwm = 0;
+        }
+    }
+
+    void Motor::Update(MotorStatus *status) {
+        for(uint8_t i = MOTOR_START_NUM; i < MOUNTING_MOTOR_NUM; i++) motor[i] = status[i];
+
+        //PWM Update
+        for(uint8_t i = MOTOR_START_NUM; i < MOUNTING_MOTOR_NUM; i++) pwms[i] = percentage_to_ratio(motor[i].pwm);
+
+        //Port Update
+        for(uint8_t i = MOTOR_START_NUM; 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;
+    }
+    
+    float Motor::SetPWM(float pwm) {
+        if(pwm > 0.0)       return pwm;
+        else if(pwm < 0.0)  return -pwm;
+        else                return 80;
+    }
+    
+    
 }
-
-void MOTOR::PWM(uint8_t pwm)
-{
-    pwm_.write((float)(pwm / 255.0));
-}
\ No newline at end of file