Diff: Hardwares/Motor.cpp

Revision:

44:15de535c4005

Parent:

13:7dcb1642ef99

Child:

46:a5eb9bd3bb55

--- a/Hardwares/Motor.cpp	Mon Mar 27 22:09:22 2017 +0000
+++ b/Hardwares/Motor.cpp	Wed Mar 29 21:19:27 2017 +0000
@@ -2,18 +2,24 @@
 #include "mbed.h"
 
 #include "Core.h"
+#include "SWUSBServer.h"
 
 #include "PinAssignment.h"
 
+#define MOTOR_PERIOD  0.002
+
 Motor::Motor(SW::Core& core) :
     m_core(core),
-    m_dirL(DigitalOut(PIN_MC_DIR_L, 1)),
-    m_dirR(DigitalOut(PIN_MC_DIR_R, 1)),
+    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))
     
 {
-    
+    m_pwmL.period(MOTOR_PERIOD);
+    m_pwmR.period(MOTOR_PERIOD);
+    m_pwmL = 0.0f;
+    m_pwmR = 0.0f;
 }
 
 
@@ -22,33 +28,42 @@
     
 }
 
-void Motor::setLeftSpeed(float speed)
+void Motor::setLeftSpeed(const float speed)
 {
     if(speed < -1.0f || speed > 1.0f)
         return;
     
-    m_pwmL.period_us(60);
+    //m_pwmL.period_ms(MOTOR_PERIOD);
     
     setLeftDirection(speed < 0 ? MDIR_Backward : MDIR_Forward);
-    speed = speed < 0 ? -speed : speed;
+    m_pwmL.pulsewidth((speed < 0 ? -speed : speed) * MOTOR_PERIOD);
+    //m_pwmL = (speed < 0 ? -speed : speed);
 
-    m_pwmL = speed;
+    char buf[20];
+    sprintf(buf, "L %f-%f", speed, (float)m_pwmL);
+    m_core.GetUSBServer().PushUnreliableMsg('D', buf);
+    //m_pwmL = speed;
 }
 
-void Motor::setRightSpeed(float speed)
+void Motor::setRightSpeed(const float speed)
 {
     if(speed < -1.0f || speed > 1.0f)
         return;
     
-    m_pwmR.period_us(60);
+    //m_pwmR.period_ms(MOTOR_PERIOD);
     
     setRightDirection(speed < 0 ? MDIR_Backward : MDIR_Forward);
-    speed = speed < 0 ? -speed : speed;
+    m_pwmR.pulsewidth((speed < 0 ? -speed : speed) * MOTOR_PERIOD);
+    //m_pwmR = (speed < 0 ? -speed : speed);
+    
+    char buf[20];
+    sprintf(buf, "R %f-%f", speed, (float)m_pwmR);
+    m_core.GetUSBServer().PushUnreliableMsg('D', buf);
 
-    m_pwmR = speed;
+    //m_pwmR = speed;
 }    
 
-void Motor::setSpeeds(float leftSpeed, float rightSpeed)
+void Motor::setSpeeds(const float leftSpeed, const float rightSpeed)
 {
     setLeftSpeed(leftSpeed);
     setRightSpeed(rightSpeed);