From 67772cdbd3c86b99b471a5f7a1605706390b4a1e Mon Sep 17 00:00:00 2001 From: icanotc <44480394+icanotc@users.noreply.github.com> Date: Thu, 20 Apr 2023 02:19:28 -0500 Subject: [PATCH 1/4] omg susssy!!!! --- .../evlib/hardware/control/NotLinearLift.kt | 64 +++++++++++++++++++ 1 file changed, 64 insertions(+) create mode 100644 Evlib/src/main/java/ftc/evlib/hardware/control/NotLinearLift.kt diff --git a/Evlib/src/main/java/ftc/evlib/hardware/control/NotLinearLift.kt b/Evlib/src/main/java/ftc/evlib/hardware/control/NotLinearLift.kt new file mode 100644 index 000000000000..174b70c87e41 --- /dev/null +++ b/Evlib/src/main/java/ftc/evlib/hardware/control/NotLinearLift.kt @@ -0,0 +1,64 @@ +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 private constructor( + private 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: Int = 0, + private val maxExtensionPower: Double = 1.0, + scaling: ControlFn, + potentiometer: AnalogSensor +) { + + init { + assert(maxExtensionPosition > minExtensionPosition) { "maxExtensionPosition must be greater than minExtensionPosition" } + assert(maxExtensionPower > 0 && maxExtensionPower < 1.0) { "maxExtensionPower must be greater than 0" } + assert(tolerance >= 0) { "tolerance must be greater than or equal to 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 + + val extensionCurrentPos: Double = 0.0 + val lowerLimit = AnalogInputEdgeDetector(potentiometer, 0.0, 0.0, false) + var extensionSetPoint = 0.0 + + // !!! Change this to -10000 when limit switch is reinstalled !!! + var extensionPower = 0.0 + val controlExtensionScale: ControlFn = scaling + + fun pre_act() { + lowerLimit.update() + } + + fun act() { + extensionSetPoint = extensionSetPoint.coerceIn(minExtensionPosition, maxExtensionPosition) + extensionPower = extensionPID.computeCorrection(extensionSetPoint, extensionCurrentPos) + + motor.setSpeed(extensionPower * maxExtensionPower) + motor.update() + } + + fun controlExtension(x: Double) { + extensionSetPoint += controlExtensionScale.rescale(x, extensionCurrentPos, extensionPower) + } + + fun stopExtension() { + extensionSetPoint = extensionCurrentPos + } + + val isDone: Boolean + get() = extensionSetPoint - extensionCurrentPos <= tolerance && extensionSetPoint - extensionCurrentPos >= -tolerance + +} \ No newline at end of file From 89148e2c30bd73c378017e9d890471d38ad1da97 Mon Sep 17 00:00:00 2001 From: icanotc <44480394+icanotc@users.noreply.github.com> Date: Thu, 20 Apr 2023 03:42:12 -0500 Subject: [PATCH 2/4] wrote code, gonna sleep --- .../evlib/hardware/control/NotLinearLift.kt | 32 ++++---- .../ftc/teamcode/PowerPlay/Scorer.kt | 79 ++++++++++++------- 2 files changed, 67 insertions(+), 44 deletions(-) diff --git a/Evlib/src/main/java/ftc/evlib/hardware/control/NotLinearLift.kt b/Evlib/src/main/java/ftc/evlib/hardware/control/NotLinearLift.kt index 174b70c87e41..fe75bc830ff2 100644 --- a/Evlib/src/main/java/ftc/evlib/hardware/control/NotLinearLift.kt +++ b/Evlib/src/main/java/ftc/evlib/hardware/control/NotLinearLift.kt @@ -9,20 +9,20 @@ import ftc.evlib.hardware.sensors.AnalogSensor //OR ELSE DEAD BODY WILL BE FOUND //AND EMERGENCY MEETING WILL BE CALLED -class NotLinearLift private constructor( - private val motor: MotorEncEx, +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: Int = 0, - private val maxExtensionPower: Double = 1.0, - scaling: ControlFn, - potentiometer: AnalogSensor + 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(maxExtensionPower > 0 && maxExtensionPower < 1.0) { "maxExtensionPower must be greater than 0" } + assert(maxCorrectionPower > 0 && maxCorrectionPower < 1.0) { "maxExtensionPower must be between 0 and 1"} assert(tolerance >= 0) { "tolerance must be greater than or equal to 0" } } @@ -31,22 +31,21 @@ class NotLinearLift private constructor( //record the power that are sent to the motor val extensionCurrentPos: Double = 0.0 - val lowerLimit = AnalogInputEdgeDetector(potentiometer, 0.0, 0.0, false) var extensionSetPoint = 0.0 - - // !!! Change this to -10000 when limit switch is reinstalled !!! var extensionPower = 0.0 - val controlExtensionScale: ControlFn = scaling fun pre_act() { - lowerLimit.update() + } fun act() { extensionSetPoint = extensionSetPoint.coerceIn(minExtensionPosition, maxExtensionPosition) - extensionPower = extensionPID.computeCorrection(extensionSetPoint, extensionCurrentPos) + extensionPower = extensionPID.computeCorrection(extensionSetPoint, potentiometer.value) + + if (extensionCurrentPos <= minExtensionPosition) //wow dont go too fast if ur below the min + extensionPower *= 0.1 - motor.setSpeed(extensionPower * maxExtensionPower) + motor.setSpeed(extensionPower * maxCorrectionPower) //scale the actual power with maxExtensionPower motor.update() } @@ -59,6 +58,9 @@ class NotLinearLift private constructor( } val isDone: Boolean - get() = extensionSetPoint - extensionCurrentPos <= tolerance && extensionSetPoint - extensionCurrentPos >= -tolerance + get() { + val difference = extensionSetPoint - extensionCurrentPos + return difference in (-tolerance..tolerance) + } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PowerPlay/Scorer.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PowerPlay/Scorer.kt index 53bde29c8847..bf2750821dcd 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PowerPlay/Scorer.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PowerPlay/Scorer.kt @@ -4,22 +4,24 @@ 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_MAX_EXTENSION = 3.02 +const val LIFT_MIN_EXTENSION = 0.992 const val LIFT_SENSITIVITY = 16.0 const val FETCHER_SENSITIVITY = 44.0 const val FETCHER_MAX_EXTENSION = 900 -const val LINEAR_SLIDE_TOLERANCE = 5 +const val LINEAR_SLIDE_TOLERANCE = 5.0 const val FETCHER_MAX_ENC_FOR_SAFE_LIFT_HOMING = 30.0 @@ -31,6 +33,15 @@ enum class FetcherPresets(val height: Double) { FETCHER_HOME(-50.0) } +object LiftPIDCoefficients { + + const val kP = 20.0 + const val kI = 0.3 + const val kD = 6.0 + const val kF = 0.0 + +} + object FetcherAutoPIDCoefficients { const val kP = 50.0 const val kI = 0.2 @@ -52,7 +63,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 +79,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,16 +89,16 @@ 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.kI, LiftPIDCoefficients.kI, LiftPIDCoefficients.kD, LiftPIDCoefficients.kF), LIFT_MAX_EXTENSION, + LIFT_MIN_EXTENSION, LINEAR_SLIDE_TOLERANCE, - liftLimitSwitch, - true, + 1.0, { control, _, _ -> LIFT_SENSITIVITY * Functions.eBased(1.5).f(-control) }, - false), + liftPotentiometer), fetcher = LinearSlide(fetcher, - null, + with (FetcherTelePIDCoefficients){PIDController(kP, kI, kD, kF)}, FETCHER_MAX_EXTENSION, 25, fetchLimitSwitch, @@ -100,9 +112,9 @@ data class ScorerHardware( ) init { - lift.maxCorrectionPower = 1.0 - lift.setDownPower(0.11) - fetcher.maxCorrectionPower = 1.00 +// lift.maxCorrectionPower = 1.0 now in the constructor +// lift.setDownPower(0.11) slow descent no longer needed +// fetcher.maxCorrectionPower = 1.00 now in the constructor } fun pre_update() { @@ -152,10 +164,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 +221,21 @@ 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 encoder value: ", hardware.lift.extensionEncoder) + telemetry.addData("Lift encoder value: ", hardware.lift.motor.encoderPosition) 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 +393,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 +425,11 @@ 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 = + if (value > LIFT_MAX_EXTENSION) LIFT_MAX_EXTENSION + else value + } fun runDropper(power: DropperPresets) { From 900de3d8a1387c8258bc841313a0cffcf164be69 Mon Sep 17 00:00:00 2001 From: icanotc <44480394+icanotc@users.noreply.github.com> Date: Thu, 20 Apr 2023 14:05:32 -0500 Subject: [PATCH 3/4] changes from PR comments --- .../evlib/hardware/control/NotLinearLift.kt | 17 ++++----- .../ftc/teamcode/PowerPlay/Scorer.kt | 36 +++++++++---------- 2 files changed, 25 insertions(+), 28 deletions(-) diff --git a/Evlib/src/main/java/ftc/evlib/hardware/control/NotLinearLift.kt b/Evlib/src/main/java/ftc/evlib/hardware/control/NotLinearLift.kt index fe75bc830ff2..00d42e0bb443 100644 --- a/Evlib/src/main/java/ftc/evlib/hardware/control/NotLinearLift.kt +++ b/Evlib/src/main/java/ftc/evlib/hardware/control/NotLinearLift.kt @@ -22,15 +22,15 @@ class NotLinearLift( 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 or equal to 0" } + 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 - val extensionCurrentPos: Double = 0.0 + var extensionCurrentPos: Double = potentiometer.value var extensionSetPoint = 0.0 var extensionPower = 0.0 @@ -39,13 +39,14 @@ class NotLinearLift( } fun act() { + extensionCurrentPos = potentiometer.value extensionSetPoint = extensionSetPoint.coerceIn(minExtensionPosition, maxExtensionPosition) - extensionPower = extensionPID.computeCorrection(extensionSetPoint, potentiometer.value) + extensionPower = extensionPID.computeCorrection(extensionSetPoint, extensionCurrentPos) - if (extensionCurrentPos <= minExtensionPosition) //wow dont go too fast if ur below the min - extensionPower *= 0.1 + if (extensionCurrentPos <= minExtensionPosition + 0.1) //wow, don't go too fast in the stop zone + extensionPower = extensionPower.coerceIn(-0.1, maxCorrectionPower) - motor.setSpeed(extensionPower * maxCorrectionPower) //scale the actual power with maxExtensionPower + motor.power = extensionPower.coerceIn(-maxCorrectionPower, maxCorrectionPower) //scale the actual power with maxExtensionPower motor.update() } @@ -60,7 +61,7 @@ class NotLinearLift( val isDone: Boolean get() { val difference = extensionSetPoint - extensionCurrentPos - return difference in (-tolerance..tolerance) + return difference in (-tolerance..tolerance) //if difference is between tolerance } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PowerPlay/Scorer.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PowerPlay/Scorer.kt index bf2750821dcd..56f3792e722f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PowerPlay/Scorer.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PowerPlay/Scorer.kt @@ -18,10 +18,11 @@ import kotlin.math.cos const val LIFT_MAX_EXTENSION = 3.02 const val LIFT_MIN_EXTENSION = 0.992 -const val LIFT_SENSITIVITY = 16.0 +const val LIFT_SENSITIVITY = 0.05 //used to be 16 with encoders +const val LIFT_TOLERANCE = 0.01 //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.0 const val FETCHER_MAX_ENC_FOR_SAFE_LIFT_HOMING = 30.0 @@ -35,7 +36,7 @@ enum class FetcherPresets(val height: Double) { object LiftPIDCoefficients { - const val kP = 20.0 + const val kP = 40.0 //uh lets just double the gain becuz the comment said so const val kI = 0.3 const val kD = 6.0 const val kF = 0.0 @@ -93,14 +94,14 @@ data class ScorerHardware( PIDController(LiftPIDCoefficients.kI, LiftPIDCoefficients.kI, LiftPIDCoefficients.kD, LiftPIDCoefficients.kF), LIFT_MAX_EXTENSION, LIFT_MIN_EXTENSION, - LINEAR_SLIDE_TOLERANCE, + LIFT_TOLERANCE, 1.0, { control, _, _ -> LIFT_SENSITIVITY * Functions.eBased(1.5).f(-control) }, liftPotentiometer), fetcher = LinearSlide(fetcher, - with (FetcherTelePIDCoefficients){PIDController(kP, kI, kD, kF)}, + null, FETCHER_MAX_EXTENSION, - 25, + FETCHER_TOLERANCE, fetchLimitSwitch, true, { control, _, _ -> FETCHER_SENSITIVITY * Functions.eBased(1.5).f(-control) }, @@ -111,11 +112,6 @@ data class ScorerHardware( liftPotentiometer = liftPotentiometer2 ) - init { -// lift.maxCorrectionPower = 1.0 now in the constructor -// lift.setDownPower(0.11) slow descent no longer needed -// fetcher.maxCorrectionPower = 1.00 now in the constructor - } fun pre_update() { dropperLimitSwitch.update() @@ -164,10 +160,10 @@ class Scorer(val hardware: ScorerHardware, val telemetry: Telemetry) { init { // hardware.lift.extension.setVelocityPIDFCoefficients(20.0, 2.0, 5.0, 0.0) - with(LiftPIDCoefficients){ - hardware.lift.motor.setVelocityPIDFCoefficients(kP, kI, kD, kF) - hardware.lift.motor.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 why do we need this??? hardware.lift.maxCorrectionPower = LIFT_UP_SPEED.toDouble() @@ -221,9 +217,6 @@ 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 encoder value: ", hardware.lift.motor.encoderPosition) - 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) @@ -427,8 +420,11 @@ class Scorer(val hardware: ScorerHardware, val telemetry: Telemetry) { liftGoingToPreset = true hardware.lift.extensionSetPoint = - if (value > LIFT_MAX_EXTENSION) LIFT_MAX_EXTENSION - else value + when { + value > LIFT_MAX_EXTENSION -> LIFT_MAX_EXTENSION + value < LIFT_MIN_EXTENSION -> LIFT_MIN_EXTENSION + else -> value + } } From 6ccc94267becacdf7ac3586e1eb03b55de41dbb7 Mon Sep 17 00:00:00 2001 From: icanotc <44480394+icanotc@users.noreply.github.com> Date: Thu, 20 Apr 2023 14:17:59 -0500 Subject: [PATCH 4/4] new values for scorer --- .../firstinspires/ftc/teamcode/PowerPlay/Scorer.kt | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PowerPlay/Scorer.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PowerPlay/Scorer.kt index 56f3792e722f..af2b4aa38f8f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PowerPlay/Scorer.kt +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PowerPlay/Scorer.kt @@ -18,8 +18,8 @@ import kotlin.math.cos const val LIFT_MAX_EXTENSION = 3.02 const val LIFT_MIN_EXTENSION = 0.992 -const val LIFT_SENSITIVITY = 0.05 //used to be 16 with encoders -const val LIFT_TOLERANCE = 0.01 //used to be 5 +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 @@ -36,9 +36,9 @@ enum class FetcherPresets(val height: Double) { object LiftPIDCoefficients { - const val kP = 40.0 //uh lets just double the gain becuz the comment said so - const val kI = 0.3 - const val kD = 6.0 + 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 } @@ -91,7 +91,7 @@ data class ScorerHardware( ) : this( grabber = grabber, lever = lever, placer = placer, dropper = dropper, lift = NotLinearLift(lift, - PIDController(LiftPIDCoefficients.kI, LiftPIDCoefficients.kI, LiftPIDCoefficients.kD, LiftPIDCoefficients.kF), + PIDController(LiftPIDCoefficients.kP, LiftPIDCoefficients.kI, LiftPIDCoefficients.kD, LiftPIDCoefficients.kF), LIFT_MAX_EXTENSION, LIFT_MIN_EXTENSION, LIFT_TOLERANCE,