From 99bf909d20f80e78908b214c26b150f5f039e1bc Mon Sep 17 00:00:00 2001
From: Kyle Corry <kylecorry31@gmail.com>
Date: Sat, 2 Apr 2022 10:15:37 -0400
Subject: [PATCH] Fix GSpeedController ramping and add voltage control

---
 .../output/actuators/GSpeedController.java    | 53 +++++++++++++++++--
 1 file changed, 50 insertions(+), 3 deletions(-)

diff --git a/src/main/java/com/thegongoliers/output/actuators/GSpeedController.java b/src/main/java/com/thegongoliers/output/actuators/GSpeedController.java
index fe721b8..b0b8e71 100644
--- a/src/main/java/com/thegongoliers/output/actuators/GSpeedController.java
+++ b/src/main/java/com/thegongoliers/output/actuators/GSpeedController.java
@@ -23,6 +23,9 @@ public class GSpeedController implements MotorController {
     private Clock mClock;
     private double mLastTime;
     private double mScale = 1.0;
+    private boolean mUseVoltageControl = false;
+    private double mMaxVoltage = 12.0;
+    private double mResetDuration = Double.POSITIVE_INFINITY;
 
     /**
      * A speed controller with added functionality
@@ -134,23 +137,51 @@ public GSpeedController(MotorController speedController, DistanceSensor distance
         mDistancePID = distancePID;
         mVelocityPID = velocityPID;
         mClock = clock;
-        mLastTime = mClock.getTime();
+        mLastTime = 0.0;
     }
 
     @Override
     public void set(double speed) {
         if (mSecondsToFullSpeed <= 0){
-            mSpeedController.set(speed * mScale);
+            setHelper(speed);
         } else {
+            if (mLastTime == 0.0){
+                mLastTime = mClock.getTime();
+            }
             var time = mClock.getTime();
             var deltaTime = time - mLastTime;
+            if (deltaTime > mResetDuration || deltaTime == 0.0){
+                deltaTime = 0.02;
+            }
             mLastTime = time;
             double maximumRate = getMaxRate(deltaTime);
             var newSpeed = GMath.rateLimit(maximumRate, speed, get());
-            mSpeedController.set(newSpeed * mScale);
+            setHelper(newSpeed);
         }
     }
 
+    private void setHelper(double speed){
+        var newSpeed = speed * mScale;
+        if (mUseVoltageControl){
+            mSpeedController.setVoltage(newSpeed * mMaxVoltage);
+        } else {
+            mSpeedController.set(newSpeed);
+        }
+    }
+
+    public void useVoltageControl(double maxVoltage){
+        mUseVoltageControl = true;
+        mMaxVoltage = maxVoltage;
+    }
+
+    public void disableVoltageControl(){
+        mUseVoltageControl = false;
+    }
+
+    public void setResetDuration(double seconds){
+        mResetDuration = seconds;
+    }
+
     /**
      * Controls the ramping of the motor. Set to 0 to disable.
      * @param secondsToFullSpeed the ramping time in seconds from 0 to full speed
@@ -173,6 +204,22 @@ public double getVelocity(){
         return mVelocitySensor.getVelocity();
     }
 
+    public boolean atDistanceSetpoint(){
+        return mDistancePID.atSetpoint();
+    }
+
+    public boolean atVelocitySetpoint(){
+        return mVelocityPID.atSetpoint();
+    }
+
+    public void resetDistancePID(){
+        mDistancePID.reset();
+    }
+
+    public void resetVelocityPID(){
+        mVelocityPID.reset();
+    }
+
     /**
      * @param distance the distance of the motor in encoder units
      */