Diff: Hardwares/Motor.cpp

Revision:

46:a5eb9bd3bb55

Parent:

44:15de535c4005

Child:

47:a682be9908b9

--- a/Hardwares/Motor.cpp	Thu Mar 30 03:50:23 2017 +0000
+++ b/Hardwares/Motor.cpp	Thu Mar 30 22:34:20 2017 +0000
@@ -5,83 +5,64 @@
 #include "SWUSBServer.h"
 
 #include "PinAssignment.h"
+#include "GlobalVariable.h"
 
-#define MOTOR_PERIOD  0.002
+#define MOTOR_PERIOD  0.020
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
 
-Motor::Motor(SW::Core& core) :
-    m_core(core),
-    m_dirL(DigitalOut(PIN_MC_DIR_L, MDIR_Forward)),
-    m_dirR(DigitalOut(PIN_MC_DIR_R, MDIR_Forward)),
-    m_pwmL(PwmOut(PIN_MC_SPEED_L)),
-    m_pwmR(PwmOut(PIN_MC_SPEED_R))
+static DigitalOut motor_dir_l(PIN_MC_DIR_L, MDIR_Forward);
+static DigitalOut motor_dir_r(PIN_MC_DIR_R, MDIR_Forward);
+
+static PwmOut motor_pwm_l(PIN_MC_SPEED_L);
+static PwmOut motor_pwm_r(PIN_MC_SPEED_R);
     
+void motor_init()
 {
-    m_pwmL.period(MOTOR_PERIOD);
-    m_pwmR.period(MOTOR_PERIOD);
-    m_pwmL = 0.0f;
-    m_pwmR = 0.0f;
+    motor_pwm_l.period(MOTOR_PERIOD);
+    motor_pwm_r.period(MOTOR_PERIOD);
+    motor_dir_l = 0.0f;
+    motor_dir_r = 0.0f;
 }
 
-
-void Motor::Update(float deltaTime)
-{
-    
-}
-
-void Motor::setLeftSpeed(const float speed)
+void motor_set_left_speed(const float speed)
 {
     if(speed < -1.0f || speed > 1.0f)
         return;
     
-    //m_pwmL.period_ms(MOTOR_PERIOD);
+    motor_set_left_direction(speed < 0 ? MDIR_Backward : MDIR_Forward);
+    
+    motor_pwm_l.pulsewidth((speed < 0 ? -speed : speed) * MOTOR_PERIOD);
     
-    setLeftDirection(speed < 0 ? MDIR_Backward : MDIR_Forward);
-    m_pwmL.pulsewidth((speed < 0 ? -speed : speed) * MOTOR_PERIOD);
-    //m_pwmL = (speed < 0 ? -speed : speed);
-
-    char buf[20];
-    sprintf(buf, "L %f-%f", speed, (float)m_pwmL);
-    m_core.GetUSBServer().PushUnreliableMsg('D', buf);
-    //m_pwmL = speed;
+    //char buf[20];
+    //sprintf(buf, "Motor %f", (float)motor_pwm_l);
+    //g_core.GetUSBServer().PushUnreliableMsg('D', buf);
 }
 
-void Motor::setRightSpeed(const float speed)
+void motor_set_right_speed(const float speed)
 {
     if(speed < -1.0f || speed > 1.0f)
         return;
     
-    //m_pwmR.period_ms(MOTOR_PERIOD);
-    
-    setRightDirection(speed < 0 ? MDIR_Backward : MDIR_Forward);
-    m_pwmR.pulsewidth((speed < 0 ? -speed : speed) * MOTOR_PERIOD);
-    //m_pwmR = (speed < 0 ? -speed : speed);
+    motor_set_right_direction(speed < 0 ? MDIR_Backward : MDIR_Forward);
     
-    char buf[20];
-    sprintf(buf, "R %f-%f", speed, (float)m_pwmR);
-    m_core.GetUSBServer().PushUnreliableMsg('D', buf);
+    motor_pwm_r.pulsewidth((speed < 0 ? -speed : speed) * MOTOR_PERIOD);
+}
 
-    //m_pwmR = speed;
-}    
-
-void Motor::setSpeeds(const float leftSpeed, const float rightSpeed)
+void motor_set_left_direction(MotorDir dir)
 {
-    setLeftSpeed(leftSpeed);
-    setRightSpeed(rightSpeed);    
+    motor_dir_l = static_cast<int>(dir);
 }
 
-    
-void Motor::setLeftDirection(MotorDir dir)
-{
-    m_dirL = static_cast<int>(dir);
-}
-    
-void Motor::setRightDirection(MotorDir dir)
+void motor_set_right_direction(MotorDir dir)
 {
-    m_dirR = static_cast<int>(dir);
+    motor_dir_r = static_cast<int>(dir);
 }
-    
-void Motor::setDirections(MotorDir dirL, MotorDir dirR)
-{
-    setLeftDirection(dirL);
-    setRightDirection(dirR);
+
+
+#ifdef __cplusplus
 }
+#endif
\ No newline at end of file