Season 2022 sussy play ppid omg

Evlib/src/main/java/ftc/evlib/hardware/control/NotLinearLift.kt

@@ -0,0 +1,67 @@
+package ftc.evlib.hardware.control
+
+import ftc.electronvolts.util.AnalogInputEdgeDetector
+import ftc.electronvolts.util.PIDController
+import ftc.evlib.hardware.motors.MotorEncEx
+import ftc.evlib.hardware.sensors.AnalogSensor
+
+//PLEASE ONLY USE THIS IN THE SCORER CLASS IF YOU KNOW WHAT YOU ARE DOING!!!!!
+//OR ELSE DEAD BODY WILL BE FOUND
+//AND EMERGENCY MEETING WILL BE CALLED
+
+class NotLinearLift(
+    val motor: MotorEncEx,
+    private val extensionPID: PIDController,
+    private var maxExtensionPosition: Double = 3.02, //these are now all in potentiometer units
+    private var minExtensionPosition: Double = 0.992,
+    private val tolerance: Double = 0.0,
+    var maxCorrectionPower: Double = 1.0,
+    val controlExtensionScale: ControlFn,
+    val potentiometer: AnalogSensor
+) {
+
+    init {
+        assert(maxExtensionPosition > minExtensionPosition) { "maxExtensionPosition must be greater than minExtensionPosition" }
+        assert(maxCorrectionPower > 0 && maxCorrectionPower <= 1.0) { "maxExtensionPower must be between 0 and 1"}
+        assert(tolerance > 0) { "tolerance must be greater than 0" }
+    }
+
+    //NOTES: if the lower limit is triggered, we can only put 10% power to the motor
+    //make sure there are enough stuff in the log to properly tune it.
+    //record the power that are sent to the motor
+
+    var extensionCurrentPos: Double = potentiometer.value
+    var extensionSetPoint = 0.0
+    var extensionPower = 0.0
+
+    fun pre_act() {
+
+    }
+
+    fun act() {
+        extensionCurrentPos = potentiometer.value
+        extensionSetPoint = extensionSetPoint.coerceIn(minExtensionPosition, maxExtensionPosition)
+        extensionPower = extensionPID.computeCorrection(extensionSetPoint, extensionCurrentPos)
+
+        if (extensionCurrentPos <= minExtensionPosition + 0.1) //wow, don't go too fast in the stop zone
+            extensionPower = extensionPower.coerceIn(-0.1, maxCorrectionPower)
+
+        motor.power = extensionPower.coerceIn(-maxCorrectionPower, maxCorrectionPower) //scale the actual power with maxExtensionPower
+        motor.update()
+    }
+
+    fun controlExtension(x: Double) {
+        extensionSetPoint += controlExtensionScale.rescale(x, extensionCurrentPos, extensionPower)
+    }
+
+    fun stopExtension() {
+       extensionSetPoint = extensionCurrentPos
+    }
+
+    val isDone: Boolean
+        get() {
+            val difference = extensionSetPoint - extensionCurrentPos
+            return difference in (-tolerance..tolerance) //if difference is between tolerance
+        }
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PowerPlay/Scorer.kt

@@ -4,22 +4,25 @@ import ftc.electronvolts.util.AnalogInputEdgeDetector
 import ftc.electronvolts.util.DigitalInputEdgeDetector
 import ftc.electronvolts.util.Functions
 import ftc.electronvolts.util.OptionsFile
+import ftc.electronvolts.util.PIDController
 import ftc.evlib.hardware.control.LinearSlide
+import ftc.evlib.hardware.control.NotLinearLift
 import ftc.evlib.hardware.motors.MotorEncEx
 import ftc.evlib.hardware.sensors.AnalogSensor
 import ftc.evlib.hardware.sensors.DigitalSensor
 import ftc.evlib.hardware.servos.ServoControl
 import ftc.evlib.util.FileUtil
 import org.firstinspires.ftc.robotcore.external.Telemetry
-import java.util.Comparator
 import java.util.PriorityQueue
 import kotlin.math.cos
 
-const val LIFT_MAX_EXTENSION = 900
-const val LIFT_SENSITIVITY = 16.0
+const val LIFT_MAX_EXTENSION = 3.02
+const val LIFT_MIN_EXTENSION = 0.992
+const val LIFT_SENSITIVITY = 0.03555 //used to be 16 with encoders
+const val LIFT_TOLERANCE = 0.01111 //used to be 5
+const val FETCHER_TOLERANCE = 25
 const val FETCHER_SENSITIVITY = 44.0
 const val FETCHER_MAX_EXTENSION = 900
-const val LINEAR_SLIDE_TOLERANCE = 5
 const val FETCHER_MAX_ENC_FOR_SAFE_LIFT_HOMING = 30.0
 
 
@@ -31,6 +34,15 @@ enum class FetcherPresets(val height: Double) {
     FETCHER_HOME(-50.0)
 }
 
+object LiftPIDCoefficients {
+
+    const val kP = 9000.0 //uh lets just double the gain becuz the comment said so
+    const val kI = 135.0
+    const val kD = 2700.0
+    const val kF = 0.0
+
+}
+
 object FetcherAutoPIDCoefficients {
     const val kP = 50.0
     const val kI = 0.2
@@ -52,7 +64,7 @@ data class ScorerHardware(
     val placer: ServoControl,
     val dropper: ServoControl,
 
-    val lift: LinearSlide,
+    val lift: NotLinearLift,
     val fetcher: LinearSlide,
     val dropperLimitSwitch: DigitalInputEdgeDetector,
     val liftEncoder: AnalogSensor,
@@ -68,7 +80,8 @@ data class ScorerHardware(
 
         lift: MotorEncEx,
         fetcher: MotorEncEx,
-        liftLimitSwitch: DigitalSensor,
+        liftLimitSwitch: DigitalSensor, //haha we dont need this anymore
+        //tom if you're reading this, lets add foam at where the limit switch is, for both sides ofc
         fetchLimitSwitch: DigitalSensor,
         dropperLimitSwitch: DigitalSensor,
         LiftEncoder: AnalogSensor,
@@ -77,18 +90,18 @@ data class ScorerHardware(
         liftLowThreshold: Double, liftHighThreshold: Double
     ) : this(
         grabber = grabber, lever = lever, placer = placer, dropper = dropper,
-        lift = LinearSlide(lift,
-            null,
+        lift = NotLinearLift(lift,
+            PIDController(LiftPIDCoefficients.kP, LiftPIDCoefficients.kI, LiftPIDCoefficients.kD, LiftPIDCoefficients.kF),
             LIFT_MAX_EXTENSION,
-            LINEAR_SLIDE_TOLERANCE,
-            liftLimitSwitch,
-            true,
+            LIFT_MIN_EXTENSION,
+            LIFT_TOLERANCE,
+            1.0,
             { control, _, _ -> LIFT_SENSITIVITY * Functions.eBased(1.5).f(-control) },
-            false),
+            liftPotentiometer),
         fetcher = LinearSlide(fetcher,
             null,
             FETCHER_MAX_EXTENSION,
-            25,
+            FETCHER_TOLERANCE,
             fetchLimitSwitch,
             true,
             { control, _, _ -> FETCHER_SENSITIVITY * Functions.eBased(1.5).f(-control) },
@@ -99,11 +112,6 @@ data class ScorerHardware(
         liftPotentiometer = liftPotentiometer2
     )
 
-    init {
-        lift.maxCorrectionPower = 1.0
-        lift.setDownPower(0.11)
-        fetcher.maxCorrectionPower = 1.00
-    }
 
     fun pre_update() {
         dropperLimitSwitch.update()
@@ -152,10 +160,12 @@ class Scorer(val hardware: ScorerHardware, val telemetry: Telemetry) {
 
     init {
 //        hardware.lift.extension.setVelocityPIDFCoefficients(20.0, 2.0, 5.0, 0.0)
-        hardware.lift.extension.setVelocityPIDFCoefficients(20.0, 0.3, 6.0, 0.0)
-        hardware.lift.extension.setPositionPIDCoefficients(15.0)
+//        with(LiftPIDCoefficients){
+//            hardware.lift.motor.setVelocityPIDFCoefficients(kP, kI, kD, kF)
+//            hardware.lift.motor.setPositionPIDCoefficients(15.0)
+//        }
         hardware.fetcher.extension.setVelocityPIDFCoefficients(45.0,0.2,25.0, 0.0)
-        hardware.lift.slowEncDelta = 0.5
+//        hardware.lift.slowEncDelta = 0.5 why do we need this???
         hardware.lift.maxCorrectionPower = LIFT_UP_SPEED.toDouble()
         hardware.lever.setDefaultSpeed(ScorerConstants.LEVER_SPEED) //so that it doesn't go all the way to the target value
         hardware.placer.setDefaultSpeed(ScorerConstants.PLACER_SPEED)
@@ -207,11 +217,18 @@ class Scorer(val hardware: ScorerHardware, val telemetry: Telemetry) {
         telemetry.addData("Fetcher encoder value: ", hardware.fetcher.extensionEncoder)
         telemetry.addData("Fetcher set point: ", hardware.fetcher.extensionSetPoint)
 //        telemetry.addData("Fetcher power", hardware.fetcher.extensionPower)
-        telemetry.addData("Lift encoder value: ", hardware.lift.extensionEncoder)
-        telemetry.addData("Lift set point: ", hardware.lift.extensionSetPoint)
         telemetry.addData("Grabber Servo Position: ", hardware.grabber.currentPosition)
         telemetry.addData("Grabber State: ", grabberState.name)
 //        telemetry.addData("Lift power", hardware.lift.extensionPower)
+
+        //new NotLinearLift logging
+        telemetry.addData("Lift power", hardware.lift.motor.power)
+        telemetry.addData("Lift velocity", hardware.lift.motor.velocity)
+        telemetry.addData("Lift position", hardware.lift.motor.encoderPosition)
+        telemetry.addData("Lift target position", hardware.lift.extensionSetPoint)
+        telemetry.addData("Lift is done", hardware.lift.isDone)
+        telemetry.addData("Lift potentiometer", hardware.lift.potentiometer)
+
     }
 
     fun getFetcherOffset(leverPosition: Double): Double {
@@ -369,13 +386,10 @@ class Scorer(val hardware: ScorerHardware, val telemetry: Telemetry) {
 
     fun liftToPreset(preset: LiftPresets) {
         liftGoingToPreset = true
-        hardware.lift.setExtension(
-            if (liftDynamicMode){
+        hardware.lift.extensionSetPoint =
+            if (liftDynamicMode)
                 GenericOptions.dynamicLiftPreset(optionsFile, preset)
-            } else {
-                preset.height
-            }
-        )
+            else preset.height
     }
 
     fun fetcherToPreset(preset: FetcherPresets) {
@@ -404,11 +418,14 @@ class Scorer(val hardware: ScorerHardware, val telemetry: Telemetry) {
 
     fun liftToValue(value: Double) {
         liftGoingToPreset = true
-        if(value > LIFT_MAX_EXTENSION) {
-            hardware.lift.setExtension(LIFT_MAX_EXTENSION.toDouble())
-        } else {
-            hardware.lift.setExtension(value)
-        }
+
+        hardware.lift.extensionSetPoint =
+            when {
+                value > LIFT_MAX_EXTENSION -> LIFT_MAX_EXTENSION
+                value < LIFT_MIN_EXTENSION -> LIFT_MIN_EXTENSION
+                else -> value
+            }
+
     }
 
     fun runDropper(power: DropperPresets) {