Diff: MotorControler.cpp

Branch:

SM_LAP_Switchng

Revision:

6:7955a025adee

Parent:

5:b46d1e179eb7

Child:

8:ad953e0e3c0f

--- a/MotorControler.cpp	Tue Feb 02 14:53:38 2021 +0000
+++ b/MotorControler.cpp	Tue Feb 02 15:19:09 2021 +0000
@@ -1,11 +1,12 @@
 #include "MotorControler.h"
 
-MotorControler::MotorControler(PinName DIR, PinName PWM, PinName nSLP, DriverType md_type) : DIR_(DIR), PWM_(PWM), nSLP_(nSLP)
+MotorControler::MotorControler(PinName DIR, PinName PWM, PinName nSLP, DriverType md_type, ControlType control_type) : DIR_(DIR), PWM_(PWM), nSLP_(nSLP)
 {
     disableDriver();
     reverse_direction_ = 0;
     current_speed_ = 0;
     md_type_ = md_type;
+    control_type_ = control_type;
 }
 
 float MotorControler::operator = (float speed) {
@@ -72,7 +73,6 @@
 void MotorControler::setSpeed(float speed)
 {
     uint8_t reverse = 0;
-    
     // 最大デューティ比で制限
     if(speed < -1) speed = -1;
     else if(speed > 1) speed = 1; 
@@ -83,18 +83,44 @@
         speed = -speed;  // Make speed a positive quantity
         reverse = 1;  // Preserve the direction
     }
-        
-    PWM_ = speed;
+    
+    int8_t dir = 1;
     if (reverse ^ FLIP_MOTOR_DIR ^ reverse_direction_)
     {
-        DIR_ = 1;
+        // CCW
+        dir = -1;
     }
     else
     {
-        DIR_ = 0;
+        // CW
+        dir = 1;
+    }
+    
+    if(control_type_ == SM)
+    {
+        setPinUsingSignMagnitude(dir, speed);
+    }
+    else if(control_type_ == LAP)
+    {
+        setPinusingLockedUntiPhase(dir, speed);
     }
 }
 
+void MotorControler::setPinUsingSignMagnitude(int8_t dir, float speed)
+{
+    if(dir > 0) DIR_ = 0;
+    else if(dir < 0) DIR_ = 1;
+    PWM_ = speed;
+}
+
+void MotorControler::setPinusingLockedUntiPhase(int8_t dir, float speed)
+{
+    PWM_ = 1.0;
+    float dir_input = 0.5 + 0.5 * speed * dir;
+    DIR_ = dir_input;
+}
+
+
 void MotorControler::setMotorDirection(MotorDirection dir)
 {
     reverse_direction_ = dir;