From 10693a4c9b22644fc0b6d7a11e4e0619db933381 Mon Sep 17 00:00:00 2001
From: Elia <124895905+Nataso08@users.noreply.github.com>
Date: Fri, 5 Jun 2026 10:16:11 +0200
Subject: [PATCH] Corrected velocity measurement in CascadeControl

---
 .../controller/CascadeController.java         | 22 ++++++++++---------
 1 file changed, 12 insertions(+), 10 deletions(-)

diff --git a/core/src/main/java/com/seattlesolvers/solverslib/controller/CascadeController.java b/core/src/main/java/com/seattlesolvers/solverslib/controller/CascadeController.java
index 85745d4d..150c91e2 100644
--- a/core/src/main/java/com/seattlesolvers/solverslib/controller/CascadeController.java
+++ b/core/src/main/java/com/seattlesolvers/solverslib/controller/CascadeController.java
@@ -4,6 +4,7 @@ public class CascadeController extends Controller {
     private final Controller primary;
     private final Controller secondary;
     private double velMeasuredValue;
+    private double prevMeasuredValue;
     private double velSetPoint;
 
     public CascadeController(Controller primary, Controller secondary) {
@@ -14,24 +15,21 @@ public CascadeController(Controller primary, Controller secondary) {
     @Override
     protected double calculateOutput(double pv) {
         prevErrorVal = errorVal_p;
+        prevMeasuredValue = measuredValue;
 
         double currentTimeStamp = (double) System.nanoTime() / 1E9;
         if (lastTimeStamp == 0) lastTimeStamp = currentTimeStamp;
         period = currentTimeStamp - lastTimeStamp;
-        lastTimeStamp = currentTimeStamp;
 
-        if (measuredValue == pv) {
-            errorVal_p = setPoint - measuredValue;
-        } else {
-            errorVal_p = setPoint - pv;
+        if (measuredValue != pv) {
             measuredValue = pv;
         }
+        errorVal_p = setPoint - measuredValue;
 
         if (Math.abs(period) > 1E-6) {
-            velMeasuredValue = (errorVal_p - prevErrorVal) / period;
-        } else {
-            velMeasuredValue = 0;
-        }
+            velMeasuredValue = (measuredValue - prevMeasuredValue) / period;
+            lastTimeStamp = currentTimeStamp;
+        } 
 
         errorVal_v = velSetPoint - velMeasuredValue;
 
@@ -50,7 +48,11 @@ public void setSetPoints(double psp, double vsp) {
         setPoint = psp;
         velSetPoint = vsp;
         errorVal_p = setPoint - measuredValue;
-        velMeasuredValue = (errorVal_p - prevErrorVal) / period;
+        velMeasuredValue = (measuredValue - prevMeasuredValue) / period;
         errorVal_v = velSetPoint - velMeasuredValue;
     }
+
+    public double getMeasuredVel() {
+        return velMeasuredValue;
+    }
 }