From 509c48b4d3d9dafc4f68d6e8570ea723aa008f2c Mon Sep 17 00:00:00 2001 From: Sachet Korada Date: Mon, 11 Nov 2024 17:33:16 -0500 Subject: [PATCH] Arm PID infra: --- .../ftc/teamcode/commands/arm/ArmCommand.kt | 13 +++++ .../ArmCommand.kt => arm/OpenArmCommand.kt} | 8 +-- .../{drive => elevator}/SpinDownCommand.kt | 2 +- .../{drive => elevator}/SpinUpCommand.kt | 2 +- .../ftc/teamcode/opModes/teleOp/MainTeleOp.kt | 22 ++++---- .../opModes/tuning/arm/ArmPIDTuner.kt | 4 ++ .../subsystems/arm/ArmPIDSubsystem.kt | 34 ------------ .../teamcode/subsystems/arm/ArmSubsystem.kt | 53 +++++++++---------- .../subsystems/arm/OpenArmSubsystem.kt | 48 +++++++++++++++++ .../ftc/teamcode/utils/PIDSubsystem.kt | 4 +- 10 files changed, 108 insertions(+), 82 deletions(-) create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/arm/ArmCommand.kt rename TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/{drive/ArmCommand.kt => arm/OpenArmCommand.kt} (64%) rename TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/{drive => elevator}/SpinDownCommand.kt (85%) rename TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/{drive => elevator}/SpinUpCommand.kt (85%) create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/arm/ArmPIDTuner.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmPIDSubsystem.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/OpenArmSubsystem.kt diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/arm/ArmCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/arm/ArmCommand.kt new file mode 100644 index 0000000..aa3e83e --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/arm/ArmCommand.kt @@ -0,0 +1,13 @@ +package org.firstinspires.ftc.teamcode.commands.arm + +import com.arcrobotics.ftclib.command.CommandBase +import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem + +class ArmCommand( + private val setpoint: Double, + private val subsystem: ArmSubsystem +) : CommandBase() { + override fun initialize() { + subsystem.setpoint = setpoint + } +} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/ArmCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/arm/OpenArmCommand.kt similarity index 64% rename from TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/ArmCommand.kt rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/arm/OpenArmCommand.kt index 1757da7..70c6daf 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/ArmCommand.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/arm/OpenArmCommand.kt @@ -1,10 +1,10 @@ -package org.firstinspires.ftc.teamcode.commands.drive +package org.firstinspires.ftc.teamcode.commands.arm import com.arcrobotics.ftclib.command.CommandBase -import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem +import org.firstinspires.ftc.teamcode.subsystems.arm.OpenArmSubsystem -class ArmCommand( - private val subsystem: ArmSubsystem, +class OpenArmCommand( + private val subsystem: OpenArmSubsystem, private val turn: Boolean, ) : CommandBase() { diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/SpinDownCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinDownCommand.kt similarity index 85% rename from TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/SpinDownCommand.kt rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinDownCommand.kt index 62c81f2..2fc9bc4 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/SpinDownCommand.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinDownCommand.kt @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.commands.drive +package org.firstinspires.ftc.teamcode.commands.elevator import com.arcrobotics.ftclib.command.CommandBase import org.firstinspires.ftc.teamcode.subsystems.slides.SlidesSubsystem diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/SpinUpCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinUpCommand.kt similarity index 85% rename from TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/SpinUpCommand.kt rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinUpCommand.kt index 7bf40c7..192651a 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/SpinUpCommand.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinUpCommand.kt @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.commands.drive +package org.firstinspires.ftc.teamcode.commands.elevator import com.arcrobotics.ftclib.command.CommandBase import org.firstinspires.ftc.teamcode.subsystems.slides.SlidesSubsystem 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 241c293..1e39ce1 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 @@ -6,32 +6,30 @@ 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.drive.ArmCommand +import org.firstinspires.ftc.teamcode.commands.arm.OpenArmCommand import org.firstinspires.ftc.teamcode.commands.drive.DriveCommand -import org.firstinspires.ftc.teamcode.commands.drive.SpinDownCommand -import org.firstinspires.ftc.teamcode.commands.drive.SpinUpCommand +import org.firstinspires.ftc.teamcode.commands.elevator.SpinDownCommand +import org.firstinspires.ftc.teamcode.commands.elevator.SpinUpCommand import org.firstinspires.ftc.teamcode.constants.ControlBoard -import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem +import org.firstinspires.ftc.teamcode.subsystems.arm.OpenArmSubsystem import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem import org.firstinspires.ftc.teamcode.subsystems.slides.SlidesSubsystem @TeleOp class MainTeleOp : CommandOpMode() { - private lateinit var elevatorLeft: Motor private lateinit var elevatorRight: Motor private lateinit var armLeft: Motor private lateinit var armRight: Motor - private lateinit var slidesSubsystem: SlidesSubsystem - private lateinit var armSubsystem: ArmSubsystem + private lateinit var armSubsystem: OpenArmSubsystem private lateinit var driveSubsystem: DriveSubsystem private lateinit var spinUpCommand: SpinUpCommand private lateinit var spinDownCommand: SpinDownCommand - private lateinit var armUpCommand: ArmCommand - private lateinit var armDownCommand: ArmCommand + private lateinit var armUpCommand: OpenArmCommand + private lateinit var armDownCommand: OpenArmCommand private lateinit var driveCommand: DriveCommand private lateinit var driver: GamepadEx @@ -48,13 +46,13 @@ class MainTeleOp : CommandOpMode() { armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName, Motor.GoBILDA.RPM_435) slidesSubsystem = SlidesSubsystem(elevatorRight, elevatorLeft) - armSubsystem = ArmSubsystem(armRight, armLeft, true) + armSubsystem = OpenArmSubsystem(armRight, armLeft) driveSubsystem = DriveSubsystem(hardwareMap) spinUpCommand = SpinUpCommand(slidesSubsystem) spinDownCommand = SpinDownCommand(slidesSubsystem) - armUpCommand = ArmCommand(armSubsystem, true) - armDownCommand = ArmCommand(armSubsystem, false) + armUpCommand = OpenArmCommand(armSubsystem, true) + armDownCommand = OpenArmCommand(armSubsystem, false) driveCommand = DriveCommand(driveSubsystem, driver::getLeftX, driver::getLeftY, driver::getRightX, 0.0) 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 new file mode 100644 index 0000000..09f20fb --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/arm/ArmPIDTuner.kt @@ -0,0 +1,4 @@ +package org.firstinspires.ftc.teamcode.opModes.tuning.arm + +class ArmPIDTuner { +} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmPIDSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmPIDSubsystem.kt deleted file mode 100644 index 7f4e5ec..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmPIDSubsystem.kt +++ /dev/null @@ -1,34 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems.arm - -import com.arcrobotics.ftclib.controller.PIDController -import com.arcrobotics.ftclib.hardware.motors.Motor -import com.arcrobotics.ftclib.hardware.motors.MotorGroup -import org.firstinspires.ftc.teamcode.utils.PIDSubsystem - -class ArmPIDSubsystem( - armLeft: Motor, - armRight: Motor -) : PIDSubsystem( - PIDController( - 0.0, - 0.0, - 0.0, - ) -) { - private val turnMotors = MotorGroup(armLeft, armRight) - - init { - armLeft.inverted = true - turnMotors.resetEncoder() - turnMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE) - } - - override fun useOutput(output: Double, setpoint: Double) { - TODO("Not yet implemented") - } - - override fun getMeasurement(): Double { - TODO("Not yet implemented") - } - -} \ 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 b164f19..c60d46a 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 @@ -1,6 +1,5 @@ package org.firstinspires.ftc.teamcode.subsystems.arm -import com.acmerobotics.dashboard.config.Config import com.arcrobotics.ftclib.controller.PIDController import com.arcrobotics.ftclib.controller.wpilibcontroller.ArmFeedforward import com.arcrobotics.ftclib.hardware.motors.Motor @@ -9,58 +8,56 @@ import com.arcrobotics.ftclib.hardware.motors.MotorGroup import org.firstinspires.ftc.teamcode.utils.PIDSubsystem import kotlin.math.PI -@Config class ArmSubsystem( -// Here I am just declaring the motors and what they are called on our driver hub. - armRight: Motor, armLeft: Motor, - openLoop: Boolean + armRight: Motor ) : PIDSubsystem( PIDController( 0.0, 0.0, - 0.0 + 0.0, ) ) { + private val turnMotors = MotorGroup(armLeft, armRight) - //Here I am making a motor group, as the arm motors are going to work together to to turn the slides. - private val feedforward = ArmFeedforward(0.0, kCos, 0.0) - private val turnMotors = MotorGroup(armRight, armLeft) val armAngle: Double get() = turnMotors.positions[0] / GoBILDA.RPM_435.cpr * 2 * PI + val armVelocity: Double + get() = turnMotors.velocities[0] / GoBILDA.RPM_435.cpr * 2 * PI + + private val feedforward = ArmFeedforward(0.0, kCos, 0.0); + init { armLeft.inverted = true turnMotors.resetEncoder() turnMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE) - - if (openLoop) - disable() - else - enable() } override fun useOutput(output: Double, setpoint: Double) { - turnMotors.set(output + feedforward.calculate(getMeasurement(), 0.0)) - } + // For tuning only + super.setpoint = target + controller.setPIDF(kP, kI, kD, 0.0) - override fun getMeasurement(): Double = armAngle - - // Here are functions that work the motor, and the double is the speed of the motor, 1 being 100%. - fun clockwise() { - turnMotors.set(0.35) - } - - fun anticlockwise() { - turnMotors.set(-0.35) + turnMotors.set(output + feedforward.calculate(armAngle, armVelocity)) } - fun stop() { - turnMotors.set(feedforward.calculate(getMeasurement(), 0.0)) - } + override fun getMeasurement() = armAngle companion object { + @JvmField + var target = 0.0 + @JvmField var kCos = 0.3 + + @JvmField + var kP = 0.0 + + @JvmField + var kI = 0.0 + + @JvmField + var kD = 0.0 } } \ 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 new file mode 100644 index 0000000..3882ad5 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/OpenArmSubsystem.kt @@ -0,0 +1,48 @@ +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 kotlin.math.PI + +@Config +class OpenArmSubsystem( +// Here I am just declaring the motors and what they are called on our driver hub. + armRight : Motor, + armLeft : Motor, + +) : SubsystemBase() { + +//Here I am making a motor group, as the arm motors are going to work together to to turn the slides. + + private val turnMotors = MotorGroup(armRight, armLeft) + val armAngle: Double + get() = turnMotors.positions[0] / GoBILDA.RPM_435.cpr * 2 * PI + + init { + armLeft.inverted = true + 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.35) + } + + fun anticlockwise() { + turnMotors.set(-0.35) + } + + fun stop() { + turnMotors.set(kCos * Math.cos(armAngle)) + } + + companion object { + @JvmField + var kCos = 0.3 + } +} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/PIDSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/PIDSubsystem.kt index 9192ae8..555bc59 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/PIDSubsystem.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/PIDSubsystem.kt @@ -4,10 +4,10 @@ import com.arcrobotics.ftclib.command.SubsystemBase import com.arcrobotics.ftclib.controller.PIDFController abstract class PIDSubsystem( - private val controller: PIDFController, + val controller: PIDFController, ) : SubsystemBase() { private var enabled: Boolean = true - private var setpoint: Double = controller.setPoint + var setpoint: Double = controller.setPoint set(value) { controller.setPoint = value field = value