Smooth out Speed & Turn Servo Based on Encoder

SJSURoboticsTeam · Mar 9, 2020 · d764097 · d764097
1 parent 04977fb
commit d764097
Show file tree

Hide file tree

Showing 2 changed files with 36 additions and 6 deletions.
diff --git a/library/L3_Application/Robotics/Common/ChiHaiServo.hpp b/library/L3_Application/Robotics/Common/ChiHaiServo.hpp
@@ -10,6 +10,16 @@ namespace robotics
 {
 class ChiHaiServo
 {
+ private:
+  static constexpr float kResolution = 0.001f;
+  bool ApproximatelyEqual(units::angle::degree_t expected,
+                          units::angle::degree_t actual,
+                          float resolution)
+  {
+    return (-resolution < (actual.to<float>() - expected.to<float>())) &&
+           ((actual.to<float>() - expected.to<float>()) < resolution);
+  }
+
  public:
   explicit constexpr ChiHaiServo(
       sjsu::robotics::MagneticEncoder magnetic_encoder,
@@ -32,9 +42,11 @@ class ChiHaiServo
       // haha do math here to see if we should go forward or back
     }
     // can i use something else than a while loop here?
-    while (magnetic_encoder_.GetAngle() != angle)
+    while (!ApproximatelyEqual(current_degree, angle, kResolution))
     {
+      drv_.TurnBackward();
     }
+    drv_.Stop();
   }
   void Stop()
   {

diff --git a/library/L3_Application/Robotics/Drive/HubMotor.hpp b/library/L3_Application/Robotics/Drive/HubMotor.hpp
@@ -11,6 +11,8 @@ namespace robotics
 class HubMotor
 {
  public:
+  static constexpr float kPositiveSpeedChange = 0.05f;
+  static constexpr float kNegativeSpeedChange = -0.05f;
   enum Direction : uint8_t
   {
     kForward  = 0,
@@ -44,17 +46,33 @@ class HubMotor
   // used for testing purposes only. e.g. does the board turn the motor?
   void Start() const
   {
-    brake_.SetDutyCycle(1.0f);
+    for (float i = 0.0f; i <= 1.0f; i += kPositiveSpeedChange)
+    {
+      brake_.SetDutyCycle(i);
+    }
   }
 
   void Stop() const
   {
-    brake_.SetDutyCycle(0.0f);
+    for (float i = 0.0f; i <= 1.0f; i += kNegativeSpeedChange)
+    {
+      brake_.SetDutyCycle(i);
+    }
+  }
+
+  void SetSpeed(float duty_cycle) const
+  {
+    float current_duty_cycle = brake_.GetDutyCycle();
+    float speed_change = current_duty_cycle > duty_cycle ? kNegativeSpeedChange
+                                                         : kPositiveSpeedChange;
+    for (float i = 0.0f; i <= 1.0f; i += speed_change)
+    {
+      brake_.SetDutyCycle(i);
+    }
   }
 
   // Not sure about this function. Will need to test it out in with smruthi
-  void SetSpeedAndDirection(float duty_cycle,
-                            Direction direction) const
+  void SetSpeedAndDirection(float duty_cycle, Direction direction) const
   {
     if (direction == Direction::kForward)
     {
@@ -64,7 +82,7 @@ class HubMotor
     {
       direction_pin_.SetLow();
     }
-    brake_.SetDutyCycle(duty_cycle);
+    SetSpeed(duty_cycle);
   }
 
  private: