From f1ea5dbc4908a467b84acf9ad6a565b12dc8b607 Mon Sep 17 00:00:00 2001 From: Sachet Korada Date: Sat, 14 Dec 2024 16:50:09 -0500 Subject: [PATCH] Run to position --- .../teamcode/commands/arm/OpenArmCommand.kt | 4 +- .../commands/elevator/SpinDownCommand.kt | 6 +- .../commands/elevator/SpinUpCommand.kt | 6 +- .../ftc/teamcode/opModes/teleOp/MainTeleOp.kt | 22 ++-- .../teamcode/opModes/tuning/IntakeTuner.kt | 4 +- .../opModes/tuning/arm/ArmPIDTuner.kt | 16 +-- .../tuning/slides/SlidesConstraintsTuner.kt | 22 +--- .../opModes/tuning/slides/SlidesPIDTuner.kt | 37 ++---- .../roadrunner/tuning/TuningOpModes.kt | 14 ++- .../teamcode/subsystems/arm/ArmSubsystem.kt | 59 +++++----- .../subsystems/arm/OpenArmSubsystem.kt | 46 -------- .../subsystems/drive/DriveSubsystem.kt | 67 ++++++++--- .../subsystems/slides/ElevatorSubsystem.kt | 111 ++++++++---------- .../slides/OpenElevatorSubsystem.kt | 42 ------- 14 files changed, 178 insertions(+), 278 deletions(-) delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/OpenArmSubsystem.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenElevatorSubsystem.kt diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/arm/OpenArmCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/arm/OpenArmCommand.kt index 70c6daf..c5ff6c2 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/arm/OpenArmCommand.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/arm/OpenArmCommand.kt @@ -1,10 +1,10 @@ package org.firstinspires.ftc.teamcode.commands.arm import com.arcrobotics.ftclib.command.CommandBase -import org.firstinspires.ftc.teamcode.subsystems.arm.OpenArmSubsystem +import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem class OpenArmCommand( - private val subsystem: OpenArmSubsystem, + private val subsystem: ArmSubsystem, private val turn: Boolean, ) : CommandBase() { diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinDownCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinDownCommand.kt index 9fb0a0b..418a926 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinDownCommand.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinDownCommand.kt @@ -1,15 +1,15 @@ package org.firstinspires.ftc.teamcode.commands.elevator import com.arcrobotics.ftclib.command.CommandBase -import org.firstinspires.ftc.teamcode.subsystems.slides.OpenElevatorSubsystem +import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem class SpinDownCommand( - private val subsystem: OpenElevatorSubsystem + private val subsystem: ElevatorSubsystem ) : CommandBase() { override fun execute() { - subsystem.down() + subsystem.spinDown() } override fun end(interrupted: Boolean) { diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinUpCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinUpCommand.kt index fd8c755..eb081f3 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinUpCommand.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinUpCommand.kt @@ -1,14 +1,14 @@ package org.firstinspires.ftc.teamcode.commands.elevator import com.arcrobotics.ftclib.command.CommandBase -import org.firstinspires.ftc.teamcode.subsystems.slides.OpenElevatorSubsystem +import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem class SpinUpCommand( - private val subsystem: OpenElevatorSubsystem + private val subsystem: ElevatorSubsystem ) : CommandBase() { override fun execute() { - subsystem.up() + subsystem.spinUp() } override fun end(interrupted: Boolean) { 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 a843761..a4b7c2b 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 @@ -10,10 +10,10 @@ import org.firstinspires.ftc.teamcode.commands.drive.DriveCommand import org.firstinspires.ftc.teamcode.commands.elevator.SpinDownCommand import org.firstinspires.ftc.teamcode.commands.elevator.SpinUpCommand import org.firstinspires.ftc.teamcode.commands.intake.IntakeCommand -import org.firstinspires.ftc.teamcode.subsystems.arm.OpenArmSubsystem +import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem -import org.firstinspires.ftc.teamcode.subsystems.slides.OpenElevatorSubsystem +import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem @TeleOp class MainTeleOp : CommandOpMode() { @@ -32,17 +32,17 @@ class MainTeleOp : CommandOpMode() { driver = GamepadEx(gamepad1) operator = GamepadEx(gamepad2) - OpenElevatorSubsystem.initialize(hardwareMap) - OpenArmSubsystem.initialize(hardwareMap) + ElevatorSubsystem.initialize(hardwareMap) + ArmSubsystem.initialize(hardwareMap) DriveSubsystem.initialize(hardwareMap) IntakeSubsystem.initialize(hardwareMap) - spinUpCommand = SpinUpCommand(OpenElevatorSubsystem) - spinDownCommand = SpinDownCommand(OpenElevatorSubsystem) - armUpCommand = OpenArmCommand(OpenArmSubsystem, true) - armDownCommand = OpenArmCommand(OpenArmSubsystem, false) - //intakeCommand = IntakeCommand(true, intakeSubsystem) - //outtakeCommand = IntakeCommand(false, intakeSubsystem) + spinUpCommand = SpinUpCommand(ElevatorSubsystem) + spinDownCommand = SpinDownCommand(ElevatorSubsystem) + armUpCommand = OpenArmCommand(ArmSubsystem, true) + armDownCommand = OpenArmCommand(ArmSubsystem, false) +// intakeCommand = IntakeCommand(true, intakeSubsystem) +// outtakeCommand = IntakeCommand(false, intakeSubsystem) driveCommand = DriveCommand(DriveSubsystem, driver::getLeftX, driver::getLeftY, driver::getRightX, 0.0) operator.getGamepadButton(GamepadKeys.Button.DPAD_UP).whileHeld(spinUpCommand) @@ -57,7 +57,7 @@ class MainTeleOp : CommandOpMode() { DriveSubsystem.defaultCommand = driveCommand RunCommand({ - telemetry.addData("Arm Position: ", OpenArmSubsystem.armAngle) + telemetry.addData("Arm Position: ", ArmSubsystem.angle) telemetry.update() }).perpetually().schedule() } diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt index 16f0c6c..15c9dbc 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt @@ -24,8 +24,8 @@ class IntakeTuner : CommandOpMode() { override fun initialize() { telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) - intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName) - intakeBelt = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE_BELT.deviceName) + intake = hardwareMap[Servo::class.java, ControlBoard.INTAKE.deviceName] + intakeBelt = hardwareMap[Servo::class.java, ControlBoard.INTAKE_BELT.deviceName] IntakeSubsystem.initialize(hardwareMap) intakeBeltSubsystem = IntakeBeltSubsystem(intakeBelt) diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/arm/ArmPIDTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/arm/ArmPIDTuner.kt index d3f2555..5da3242 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/arm/ArmPIDTuner.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/arm/ArmPIDTuner.kt @@ -7,7 +7,6 @@ import com.arcrobotics.ftclib.command.CommandOpMode import com.arcrobotics.ftclib.command.RunCommand import com.arcrobotics.ftclib.hardware.motors.Motor import com.qualcomm.robotcore.eventloop.opmode.TeleOp -import org.firstinspires.ftc.teamcode.constants.ControlBoard import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem @TeleOp @@ -16,27 +15,22 @@ class ArmPIDTuner : CommandOpMode() { private lateinit var armLeft: Motor private lateinit var armRight: Motor - private lateinit var armSubsystem: ArmSubsystem - override fun initialize() { telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) - armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName, Motor.GoBILDA.RPM_312) - armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName, Motor.GoBILDA.RPM_312) - - armSubsystem = ArmSubsystem(armRight, armLeft) + ArmSubsystem.initialize(hardwareMap) RunCommand({ - armSubsystem.setpoint = Math.toRadians(target) + ArmSubsystem.setpoint = Math.toRadians(target) }).perpetually().schedule() RunCommand({ - telemetry.addData("Arm Angle: ", Math.toDegrees(armSubsystem.armAngle)) - telemetry.addData("Setpoint: ", Math.toDegrees(armSubsystem.setpoint)) + telemetry.addData("Arm Angle: ", Math.toDegrees(ArmSubsystem.angle)) + telemetry.addData("Setpoint: ", Math.toDegrees(ArmSubsystem.setpoint)) telemetry.update() }).perpetually().schedule() - register(armSubsystem) + register(ArmSubsystem) } companion object { 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 c72bf98..1834681 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 @@ -2,31 +2,21 @@ package org.firstinspires.ftc.teamcode.opModes.tuning.slides import com.acmerobotics.dashboard.FtcDashboard import com.acmerobotics.dashboard.telemetry.MultipleTelemetry -import com.arcrobotics.ftclib.hardware.motors.Motor import com.qualcomm.robotcore.eventloop.opmode.Autonomous import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode import com.qualcomm.robotcore.util.ElapsedTime -import org.firstinspires.ftc.teamcode.constants.ControlBoard import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem @Autonomous class SlidesConstraintsTuner : LinearOpMode() { - private lateinit var leftMotor: Motor - private lateinit var rightMotor: Motor - - private lateinit var elevator: ElevatorSubsystem - private var maxVel = 0.0 private var maxAccel = 0.0 private val timer = ElapsedTime(ElapsedTime.Resolution.SECONDS) override fun runOpMode() { telemetry = MultipleTelemetry(FtcDashboard.getInstance().telemetry, telemetry) + ElevatorSubsystem.initialize(hardwareMap) - leftMotor = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName) - rightMotor = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) - - elevator = ElevatorSubsystem(rightMotor, leftMotor) { 0.0 } waitForStart() var dt = 0L @@ -35,16 +25,16 @@ class SlidesConstraintsTuner : LinearOpMode() { timer.reset() while (opModeIsActive()) { while (timer.seconds() < TIME_TO_RUN) { - elevator.spinUp() + ElevatorSubsystem.spinUp() dt = timer.nanoseconds() - dt - dv = elevator.slideVelocity - dv / dt + dv = ElevatorSubsystem.velocity - dv / dt - maxVel = elevator.slideVelocity.coerceAtLeast(maxVel) + maxVel = ElevatorSubsystem.velocity.coerceAtLeast(maxVel) maxAccel = dv.coerceAtLeast(maxAccel) - telemetry.addData("Velocity", elevator.slideVelocity) - telemetry.addData("Position", elevator.slidePos) + telemetry.addData("Velocity", ElevatorSubsystem.velocity) + telemetry.addData("Position", ElevatorSubsystem.position) telemetry.addData("Max Velocity", maxVel) telemetry.addData("Max Acceleration", maxAccel) diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesPIDTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesPIDTuner.kt index 010af0a..f8f45b9 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesPIDTuner.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesPIDTuner.kt @@ -7,25 +7,14 @@ import com.arcrobotics.ftclib.command.CommandOpMode import com.arcrobotics.ftclib.command.RunCommand import com.arcrobotics.ftclib.gamepad.GamepadEx import com.arcrobotics.ftclib.gamepad.GamepadKeys -import com.arcrobotics.ftclib.hardware.motors.Motor import com.qualcomm.robotcore.eventloop.opmode.TeleOp import org.firstinspires.ftc.teamcode.commands.arm.OpenArmCommand -import org.firstinspires.ftc.teamcode.constants.ControlBoard -import org.firstinspires.ftc.teamcode.subsystems.arm.OpenArmSubsystem +import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem @TeleOp @Config class SlidesPIDTuner : CommandOpMode() { - private lateinit var slidesLeft: Motor - private lateinit var slidesRight: Motor - - private lateinit var armLeft: Motor - private lateinit var armRight: Motor - - private lateinit var armSubsystem: OpenArmSubsystem - private lateinit var slidesSubsystem: ElevatorSubsystem - private lateinit var armUpCommand: OpenArmCommand private lateinit var armDownCommand: OpenArmCommand @@ -36,34 +25,28 @@ class SlidesPIDTuner : CommandOpMode() { telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) - armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName, Motor.GoBILDA.RPM_60) - armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName, Motor.GoBILDA.RPM_60) - - slidesLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName, Motor.GoBILDA.RPM_1150) - slidesRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName, Motor.GoBILDA.RPM_1150) - - OpenArmSubsystem.initialize(hardwareMap) - slidesSubsystem = ElevatorSubsystem(slidesRight, slidesLeft, armSubsystem::armAngle) + ArmSubsystem.initialize(hardwareMap) + ElevatorSubsystem.initialize(hardwareMap) - armUpCommand = OpenArmCommand(OpenArmSubsystem, true) - armDownCommand = OpenArmCommand(OpenArmSubsystem, false) + armUpCommand = OpenArmCommand(ArmSubsystem, true) + armDownCommand = OpenArmCommand(ArmSubsystem, false) operator.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER).whileHeld(armUpCommand) operator.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER).whileHeld(armDownCommand) RunCommand({ - slidesSubsystem.setpoint = target + ElevatorSubsystem.setpoint = target }).perpetually().schedule() RunCommand({ - telemetry.addData("Slides Position", slidesSubsystem.slidePos) + telemetry.addData("Slides Position", ElevatorSubsystem.position) telemetry.addData("Setpoint", target) - telemetry.addData("arm angle", armSubsystem.armAngle) - telemetry.addData("slides Velocity", slidesSubsystem.slideVelocity) + telemetry.addData("Arm Angle", ArmSubsystem.angle) + telemetry.addData("Slides Velocity", ElevatorSubsystem.velocity) telemetry.update() }).perpetually().schedule() - register(slidesSubsystem) + register(ElevatorSubsystem) } companion object { diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/tuning/TuningOpModes.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/tuning/TuningOpModes.kt index 35f305a..752b8ca 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/tuning/TuningOpModes.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/roadrunner/tuning/TuningOpModes.kt @@ -28,12 +28,13 @@ import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem object TuningOpModes { // TODO: change this to TankDrive.class if you're using tank - val DRIVE_CLASS: Class<*> = DriveSubsystem::class.java + val DRIVE_CLASS = DriveSubsystem::class.java - const val GROUP: String = "quickstart" - const val DISABLED: Boolean = false + val GROUP = "quickstart" + val DISABLED = false - private fun metaForClass(cls: Class): OpModeMeta { + @JvmStatic + private fun metaForClass(cls: Class): OpModeMeta { return OpModeMeta.Builder() .setName(cls.simpleName) .setGroup(GROUP) @@ -42,6 +43,7 @@ object TuningOpModes { } @OpModeRegistrar + @JvmStatic fun register(manager: OpModeManager) { if (DISABLED) return @@ -61,7 +63,7 @@ object TuningOpModes { when (md.localizer) { is DriveSubsystem.DriveLocalizer -> { - val dl = md.localizer + val dl = md.localizer as DriveSubsystem.DriveLocalizer leftEncs.add(dl.leftFront) leftEncs.add(dl.leftBack) rightEncs.add(dl.rightFront) @@ -69,7 +71,7 @@ object TuningOpModes { } is ThreeDeadWheelLocalizer -> { - val dl = md.localizer + val dl = md.localizer as ThreeDeadWheelLocalizer parEncs.add(dl.par0) parEncs.add(dl.par1) perpEncs.add(dl.perp) 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 1eea847..d45b014 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 @@ -6,59 +6,64 @@ import com.arcrobotics.ftclib.controller.wpilibcontroller.ArmFeedforward import com.arcrobotics.ftclib.hardware.motors.Motor import com.arcrobotics.ftclib.hardware.motors.Motor.GoBILDA import com.arcrobotics.ftclib.hardware.motors.MotorGroup +import com.qualcomm.robotcore.hardware.HardwareMap 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 -class ArmSubsystem( - armRight: Motor, - armLeft: Motor, -) : PIDSubsystem( +object ArmSubsystem : PIDSubsystem( PIDController( ArmConstants.kP.value, ArmConstants.kI.value, ArmConstants.kD.value ) ) { - private val turnMotors = MotorGroup(armRight, armLeft) - - val armAngle: Double - get() = turnMotors.positions[0] / GoBILDA.RPM_60.cpr * PI + private lateinit var turnMotors: MotorGroup + private var feedforward = ArmFeedforward(0.0, ArmConstants.kCos.value, 0.0); - val armVelocity: Double + val velocity: Double get() = turnMotors.velocities[0] / GoBILDA.RPM_60.cpr * PI - private var feedforward = ArmFeedforward(0.0, ArmConstants.kCos.value, 0.0); + val angle: Double + get() = turnMotors.positions[0] / GoBILDA.RPM_60.cpr * PI + + fun initialize(hardwareMap: HardwareMap) : ArmSubsystem { + val armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) + val armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) - init { armLeft.inverted = true + turnMotors = MotorGroup(armRight, armLeft) + turnMotors.resetEncoder() turnMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE) - } - override fun useOutput(output: Double, setpoint: Double) { -// controller.setPIDF(kP, kI, kD, 0.0) -// feedforward = ArmFeedforward(0.0, kCos, 0.0) - - turnMotors.set(output + feedforward.calculate(armAngle, armVelocity)) + return this } - override fun getMeasurement() = armAngle - companion object { - @JvmField - var kP = 2.0 +// 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) + } - @JvmField - var kI = 0.0 + fun anticlockwise() { + turnMotors.set(-0.50) + } - @JvmField - var kD = 0.0 + fun stop() { + turnMotors.set(ArmConstants.kCos.value * cos(angle)) + } - @JvmField - var kCos = 0.1 + override fun useOutput(output: Double, setpoint: Double) { +// controller.setPIDF(kP, kI, kD, 0.0) +// feedforward = ArmFeedforward(0.0, kCos, 0.0) + turnMotors.set(output + feedforward.calculate(angle, velocity)) } + + override fun getMeasurement() = angle } \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/OpenArmSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/OpenArmSubsystem.kt deleted file mode 100644 index d185d1c..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/OpenArmSubsystem.kt +++ /dev/null @@ -1,46 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems.arm - -import com.acmerobotics.dashboard.config.Config -import com.arcrobotics.ftclib.command.SubsystemBase -import com.arcrobotics.ftclib.hardware.motors.Motor -import com.arcrobotics.ftclib.hardware.motors.Motor.GoBILDA -import com.arcrobotics.ftclib.hardware.motors.MotorGroup -import com.qualcomm.robotcore.hardware.HardwareMap -import org.firstinspires.ftc.teamcode.constants.ArmConstants -import org.firstinspires.ftc.teamcode.constants.ControlBoard -import kotlin.math.PI -import kotlin.math.cos - -@Config -object OpenArmSubsystem : SubsystemBase() { - private lateinit var turnMotors: MotorGroup - - val armAngle: Double - get() = turnMotors.positions[0] / GoBILDA.RPM_60.cpr * PI - - fun initialize(hardwareMap: HardwareMap) { - val armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) - val armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) - - armLeft.inverted = true - - turnMotors = MotorGroup(armRight, armLeft) - - turnMotors.resetEncoder() - turnMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE) - } - - -// 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) - } - - fun anticlockwise() { - turnMotors.set(-0.50) - } - - fun stop() { - turnMotors.set(ArmConstants.kCos.value * cos(armAngle)) - } -} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/drive/DriveSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/drive/DriveSubsystem.kt index dd42eab..8931d2d 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/drive/DriveSubsystem.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/drive/DriveSubsystem.kt @@ -1,6 +1,7 @@ package org.firstinspires.ftc.teamcode.subsystems.drive import com.acmerobotics.dashboard.canvas.Canvas +import com.acmerobotics.dashboard.config.Config import com.acmerobotics.dashboard.telemetry.TelemetryPacket import com.acmerobotics.roadrunner.AccelConstraint import com.acmerobotics.roadrunner.Action @@ -57,53 +58,76 @@ import java.util.LinkedList import kotlin.math.ceil import kotlin.math.max +@Config object DriveSubsystem : SubsystemBase() { private lateinit var hardwareMap: HardwareMap var pose: Pose2d = Pose2d(0.0, 0.0, 0.0) - var PARAMS: Params = Params() - - fun initialize(hardwareMap: HardwareMap, - pose2d: Pose2d = Pose2d(0.0, 0.0, 0.0) - ) { - this.hardwareMap = hardwareMap - this.pose = pose2d - } + @JvmField + var PARAMS = Params() class Params { // IMU orientation // TODO: fill in these values based on // see https://ftc-docs.firstinspires.org/en/latest/programming_resources/imu/imu.html?highlight=imu#physical-hub-mounting var logoFacingDirection: RevHubOrientationOnRobot.LogoFacingDirection = - RevHubOrientationOnRobot.LogoFacingDirection.UP - var usbFacingDirection: UsbFacingDirection = UsbFacingDirection.FORWARD + RevHubOrientationOnRobot.LogoFacingDirection.DOWN + var usbFacingDirection: UsbFacingDirection = UsbFacingDirection.RIGHT // drive model parameters + @JvmField var inPerTick: Double = 1.0 + + @JvmField var lateralInPerTick: Double = inPerTick + + @JvmField var trackWidthTicks: Double = 0.0 // feedforward parameters (in tick units) + @JvmField var kS: Double = 0.0 + + @JvmField var kV: Double = 0.0 + + @JvmField var kA: Double = 0.0 // path profile parameters (in inches) + @JvmField var maxWheelVel: Double = 50.0 + + @JvmField var minProfileAccel: Double = -30.0 + + @JvmField var maxProfileAccel: Double = 50.0 // turn profile parameters (in radians) + @JvmField var maxAngVel: Double = Math.PI // shared with path + + @JvmField var maxAngAccel: Double = Math.PI // path controller gains + @JvmField var axialGain: Double = 0.0 + + @JvmField var lateralGain: Double = 0.0 + + @JvmField var headingGain: Double = 0.0 // shared with turn + @JvmField var axialVelGain: Double = 0.0 + + @JvmField var lateralVelGain: Double = 0.0 + + @JvmField var headingVelGain: Double = 0.0 // shared with turn } @@ -123,16 +147,16 @@ object DriveSubsystem : SubsystemBase() { val defaultAccelConstraint: AccelConstraint = ProfileAccelConstraint(PARAMS.minProfileAccel, PARAMS.maxProfileAccel) - val leftFront: DcMotorEx - val leftBack: DcMotorEx - val rightBack: DcMotorEx - val rightFront: DcMotorEx + lateinit var leftFront: DcMotorEx + lateinit var leftBack: DcMotorEx + lateinit var rightBack: DcMotorEx + lateinit var rightFront: DcMotorEx - val voltageSensor: VoltageSensor + lateinit var voltageSensor: VoltageSensor - val lazyImu: LazyImu + lateinit var lazyImu: LazyImu - val localizer: Localizer + lateinit var localizer: Localizer private val poseHistory = LinkedList() @@ -240,7 +264,12 @@ object DriveSubsystem : SubsystemBase() { } } - init { + fun initialize(hardwareMap: HardwareMap, + pose2d: Pose2d = Pose2d(0.0, 0.0, 0.0) + ) : DriveSubsystem { + this.hardwareMap = hardwareMap + this.pose = pose2d + throwIfModulesAreOutdated(hardwareMap) for (module in hardwareMap.getAll(LynxModule::class.java)) { @@ -275,6 +304,8 @@ object DriveSubsystem : SubsystemBase() { localizer = DriveLocalizer() write("MECANUM_PARAMS", PARAMS) + + return this } fun setDrivePowers(powers: PoseVelocity2d) { 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 45f3c20..6a65265 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,22 +1,18 @@ 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.arcrobotics.ftclib.util.MathUtils.clamp +import com.qualcomm.robotcore.hardware.DcMotor +import com.qualcomm.robotcore.hardware.DcMotorEx +import com.qualcomm.robotcore.hardware.HardwareMap +import org.firstinspires.ftc.teamcode.constants.ControlBoard import org.firstinspires.ftc.teamcode.constants.SlidesConstants -import kotlin.math.sin - -@Config -class ElevatorSubsystem( - elevatorRight : Motor, - elevatorLeft : Motor, - private val slideAngle: () -> Double -) : SubsystemBase() { - private val extendMotors = MotorGroup(elevatorRight, elevatorLeft) + +object ElevatorSubsystem: SubsystemBase() { +// private lateinit var elevatorMotors: MotorGroup + private lateinit var elevator: DcMotorEx + private val controller = ProfiledPIDController( SlidesConstants.kP.value, SlidesConstants.kI.value, @@ -29,78 +25,65 @@ class ElevatorSubsystem( var setpoint = 0.0 set(value) { - val clamped = if (slideAngle.invoke() < 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 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 slidePos: Double - get() = extendMotors.positions[0] * SlidesConstants.MAX_HEIGHT_INCH.value / SlidesConstants.MAX_HEIGHT_TICKS.value + val position: Double +// get() = elevatorMotors.positions[0] * SlidesConstants.MAX_HEIGHT_INCH.value / SlidesConstants.MAX_HEIGHT_TICKS.value + get() = elevator.currentPosition.toDouble() - val slideVelocity: Double - get() = -extendMotors.velocities[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 var enabled = true - init { - elevatorRight.inverted = true - elevatorRight.encoder.setDirection(Motor.Direction.REVERSE) - - extendMotors.resetEncoder() - extendMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE) - - setpoint = 0.0 - enabled = true - } - - override fun periodic() { - controller.setPID(kP, kI, kD) - - val output = controller.calculate(slidePos) + fun initialize(hardwareMap: HardwareMap) { +// val elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName) +// val elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) - if (enabled) - extendMotors.set(output + kG * sin(slideAngle.invoke())) - } - - fun toggle() { - enabled != enabled - } + val elevator = hardwareMap[DcMotorEx::class.java, ControlBoard.SLIDES_RIGHT.deviceName] + elevator.mode = DcMotor.RunMode.RUN_TO_POSITION - fun disable() { - enabled = false + elevator.power = 1.0 +// elevatorLeft.inverted = true +// elevatorMotors = MotorGroup(elevatorLeft, elevatorRight) } fun spinUp() { - extendMotors.set(1.0) +// elevatorMotors.set(1.0) } fun spinDown() { - extendMotors.set(-1.0) +// elevatorMotors.set(-1.0) } fun stop() { - extendMotors.set(kG) +// elevatorMotors.set(SlidesConstants.kG.value * sin(ArmSubsystem.angle)) } - companion object { - @JvmField - var kP = 0.7 - // kP = 0.01 - - @JvmField - var kI = 0.0 + override fun periodic() { +// controller.setPID(kP, kI, kD) - @JvmField - var kD = 0.0 +// val output = controller.calculate(position) - @JvmField - var kG = 0.12 - // kG = 0.115 +// if (enabled) +// elevatorMotors.set(output + SlidesConstants.kG.value * sin(ArmSubsystem.angle)) + } + fun toggle() { + enabled != enabled } -} + fun disable() { + enabled = false + } +} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenElevatorSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenElevatorSubsystem.kt deleted file mode 100644 index b6660a6..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenElevatorSubsystem.kt +++ /dev/null @@ -1,42 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems.slides - -import com.arcrobotics.ftclib.command.SubsystemBase -import com.arcrobotics.ftclib.hardware.motors.Motor -import com.arcrobotics.ftclib.hardware.motors.MotorGroup -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.OpenArmSubsystem -import kotlin.math.sin - -object OpenElevatorSubsystem: SubsystemBase() { - private lateinit var elevatorMotors: MotorGroup - - val slidePos: Double - get() = elevatorMotors.positions[0] * SlidesConstants.MAX_HEIGHT_INCH.value / SlidesConstants.MAX_HEIGHT_TICKS.value - - val slideVelocity: Double - get() = -elevatorMotors.velocities[0] * SlidesConstants.MAX_HEIGHT_INCH.value / SlidesConstants.MAX_HEIGHT_TICKS.value - - fun initialize(hardwareMap: HardwareMap) { - val elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName) - val elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName) - - elevatorLeft.inverted = true - - elevatorMotors = MotorGroup(elevatorLeft, elevatorRight) - } - - fun up() { - elevatorMotors.set(1.0) - } - - fun down() { - elevatorMotors.set(-1.0) - } - - fun stop() { - elevatorMotors.set(SlidesConstants.kG.value * sin(OpenArmSubsystem.armAngle)) - } - -} \ No newline at end of file