Diff: Output/Motor/Motor.cpp

Revision:

0:2e7a61458dc3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Output/Motor/Motor.cpp	Mon Jan 28 08:34:36 2019 +0000
@@ -0,0 +1,88 @@
+#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),
+        };
+        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 = 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;
+        }
+    }
+}
+        
+        
\ No newline at end of file