From 1659d3fefd2ae7470c95fe8e7cc8f7ced3bd738d Mon Sep 17 00:00:00 2001 From: Sachet Korada Date: Sat, 14 Dec 2024 20:22:22 -0500 Subject: [PATCH] SMOOTH LIKE BUTTER --- .../ftc/teamcode/constants/SlidesConstants.kt | 7 +- .../ftc/teamcode/opModes/teleOp/MainTeleOp.kt | 8 ++- .../tuning/slides/SlidesConstraintsTuner.kt | 18 +++-- .../teamcode/subsystems/arm/ArmSubsystem.kt | 9 +-- .../subsystems/slides/ElevatorSubsystem.kt | 67 +++++++++---------- 5 files changed, 59 insertions(+), 50 deletions(-) diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/SlidesConstants.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/SlidesConstants.kt index f04447e..53dc128 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/SlidesConstants.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/SlidesConstants.kt @@ -1,10 +1,13 @@ package org.firstinspires.ftc.teamcode.constants enum class SlidesConstants(val value: Double) { - kP(0.7), + kP(0.4), kI(0.0), kD(0.0), - kG(0.12), + kG(0.0), + + MAX_VELOCITY(36.0), // in/s + MAX_ACCELERATION(400.0), // in/s^2 MAX_HEIGHT_TICKS(2081.0), MAX_HEIGHT_INCH(33.329) diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/teleOp/MainTeleOp.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/teleOp/MainTeleOp.kt index a4b7c2b..f17c8dd 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/teleOp/MainTeleOp.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/teleOp/MainTeleOp.kt @@ -45,8 +45,6 @@ class MainTeleOp : CommandOpMode() { // outtakeCommand = IntakeCommand(false, intakeSubsystem) driveCommand = DriveCommand(DriveSubsystem, driver::getLeftX, driver::getLeftY, driver::getRightX, 0.0) - operator.getGamepadButton(GamepadKeys.Button.DPAD_UP).whileHeld(spinUpCommand) - operator.getGamepadButton(GamepadKeys.Button.DPAD_DOWN).whileHeld(spinDownCommand) operator.getGamepadButton(GamepadKeys.Button.DPAD_UP).whileHeld(spinUpCommand) operator.getGamepadButton(GamepadKeys.Button.DPAD_DOWN).whileHeld(spinDownCommand) operator.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER).whileHeld(armUpCommand) @@ -57,7 +55,11 @@ class MainTeleOp : CommandOpMode() { DriveSubsystem.defaultCommand = driveCommand RunCommand({ - telemetry.addData("Arm Position: ", ArmSubsystem.angle) + telemetry.addData("Arm Position", ArmSubsystem.angle) + + telemetry.addData("Slides Position", ElevatorSubsystem.position) + telemetry.addData("Slides Velocity", ElevatorSubsystem.velocity) + telemetry.update() }).perpetually().schedule() } diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesConstraintsTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesConstraintsTuner.kt index 1834681..83a8234 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesConstraintsTuner.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesConstraintsTuner.kt @@ -19,27 +19,31 @@ class SlidesConstraintsTuner : LinearOpMode() { waitForStart() - var dt = 0L - var dv = 0.0 + var dt = 0.0 + var prev = 0.0 timer.reset() while (opModeIsActive()) { while (timer.seconds() < TIME_TO_RUN) { ElevatorSubsystem.spinUp() - dt = timer.nanoseconds() - dt - dv = ElevatorSubsystem.velocity - dv / dt + dt = timer.seconds() - dt + + val curr = ElevatorSubsystem.velocity + val dv = curr - prev maxVel = ElevatorSubsystem.velocity.coerceAtLeast(maxVel) - maxAccel = dv.coerceAtLeast(maxAccel) + maxAccel = maxAccel.coerceAtLeast(dv / dt) + + prev = curr telemetry.addData("Velocity", ElevatorSubsystem.velocity) telemetry.addData("Position", ElevatorSubsystem.position) telemetry.addData("Max Velocity", maxVel) telemetry.addData("Max Acceleration", maxAccel) - telemetry.update() + telemetry.update() if (isStopRequested) break } @@ -52,6 +56,6 @@ class SlidesConstraintsTuner : LinearOpMode() { companion object { @JvmField - var TIME_TO_RUN = 1.5 + var TIME_TO_RUN = 0.7 } } \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmSubsystem.kt index d45b014..3a9a665 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmSubsystem.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmSubsystem.kt @@ -11,7 +11,6 @@ import org.firstinspires.ftc.teamcode.constants.ArmConstants import org.firstinspires.ftc.teamcode.constants.ControlBoard import org.firstinspires.ftc.teamcode.utils.PIDSubsystem import kotlin.math.PI -import kotlin.math.cos @Config object ArmSubsystem : PIDSubsystem( @@ -41,21 +40,23 @@ object ArmSubsystem : PIDSubsystem( turnMotors.resetEncoder() turnMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE) + disable() + return this } // Here are functions that work the motor, and the double is the speed of the motor, 1 being 100%. fun clockwise() { - turnMotors.set(0.50) + turnMotors.set(1.0) } fun anticlockwise() { - turnMotors.set(-0.50) + turnMotors.set(-1.0) } fun stop() { - turnMotors.set(ArmConstants.kCos.value * cos(angle)) + turnMotors.set(0.0) } override fun useOutput(output: Double, setpoint: Double) { diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt index 6a65265..29de3fa 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt @@ -1,82 +1,81 @@ package org.firstinspires.ftc.teamcode.subsystems.slides +import com.acmerobotics.dashboard.config.Config import com.arcrobotics.ftclib.command.SubsystemBase import com.arcrobotics.ftclib.controller.wpilibcontroller.ProfiledPIDController +import com.arcrobotics.ftclib.hardware.motors.Motor +import com.arcrobotics.ftclib.hardware.motors.MotorGroup import com.arcrobotics.ftclib.trajectory.TrapezoidProfile -import com.qualcomm.robotcore.hardware.DcMotor -import com.qualcomm.robotcore.hardware.DcMotorEx +import com.arcrobotics.ftclib.util.MathUtils.clamp import com.qualcomm.robotcore.hardware.HardwareMap import org.firstinspires.ftc.teamcode.constants.ControlBoard import org.firstinspires.ftc.teamcode.constants.SlidesConstants +import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem +import kotlin.math.sin +@Config object ElevatorSubsystem: SubsystemBase() { -// private lateinit var elevatorMotors: MotorGroup - private lateinit var elevator: DcMotorEx + private lateinit var elevatorMotors: MotorGroup private val controller = ProfiledPIDController( SlidesConstants.kP.value, SlidesConstants.kI.value, SlidesConstants.kD.value, TrapezoidProfile.Constraints( - 30.0, // in/s - 30.0 // in/s^2 + SlidesConstants.MAX_VELOCITY.value, + SlidesConstants.MAX_ACCELERATION.value ) ) var setpoint = 0.0 set(value) { -// val clamped = if (ArmSubsystem.angle < Math.toRadians(75.0)) -// clamp(value, 0.0, 20.0) -// else -// clamp(value, 0.0, 30.0) -// -// controller.goal = TrapezoidProfile.State(clamped, 0.0) -// field = clamped - elevator.targetPosition = value.toInt() - field = value + val clamped = if (ArmSubsystem.angle < Math.toRadians(75.0)) + clamp(value, 0.0, 20.0) + else + clamp(value, 0.0, 30.0) + + controller.goal = TrapezoidProfile.State(clamped, 0.0) + field = clamped } val position: Double -// get() = elevatorMotors.positions[0] * SlidesConstants.MAX_HEIGHT_INCH.value / SlidesConstants.MAX_HEIGHT_TICKS.value - get() = elevator.currentPosition.toDouble() + get() = elevatorMotors.positions[0] * SlidesConstants.MAX_HEIGHT_INCH.value / SlidesConstants.MAX_HEIGHT_TICKS.value val velocity: Double -// get() = -elevatorMotors.velocities[0] * SlidesConstants.MAX_HEIGHT_INCH.value / SlidesConstants.MAX_HEIGHT_TICKS.value - get() = elevator.velocity + get() = -elevatorMotors.velocities[0] * SlidesConstants.MAX_HEIGHT_INCH.value / SlidesConstants.MAX_HEIGHT_TICKS.value var enabled = true fun initialize(hardwareMap: HardwareMap) { -// val elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName) -// val elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) + val elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName) + val elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) - val elevator = hardwareMap[DcMotorEx::class.java, ControlBoard.SLIDES_RIGHT.deviceName] - elevator.mode = DcMotor.RunMode.RUN_TO_POSITION + elevatorRight.inverted = true + elevatorRight.encoder.setDirection(Motor.Direction.REVERSE) - elevator.power = 1.0 -// elevatorLeft.inverted = true -// elevatorMotors = MotorGroup(elevatorLeft, elevatorRight) + elevatorMotors = MotorGroup(elevatorRight, elevatorLeft) + elevatorMotors.resetEncoder() } fun spinUp() { -// elevatorMotors.set(1.0) + elevatorMotors.set(1.0) } fun spinDown() { -// elevatorMotors.set(-1.0) + elevatorMotors.set(-1.0) } fun stop() { -// elevatorMotors.set(SlidesConstants.kG.value * sin(ArmSubsystem.angle)) + elevatorMotors.set(SlidesConstants.kG.value * sin(ArmSubsystem.angle)) } override fun periodic() { -// controller.setPID(kP, kI, kD) - -// val output = controller.calculate(position) + val output = controller.calculate(position) + + SlidesConstants.kG.value * sin(ArmSubsystem.angle) -// if (enabled) -// elevatorMotors.set(output + SlidesConstants.kG.value * sin(ArmSubsystem.angle)) + if (enabled) { + elevatorMotors.set(output + SlidesConstants.kG.value * sin(ArmSubsystem.angle)) + } } fun toggle() {