From f29fcef55e4c45f8716982fd1dc989cbaf44b696 Mon Sep 17 00:00:00 2001 From: Madison C Date: Thu, 21 Nov 2024 17:46:48 -0800 Subject: [PATCH 1/6] created tail subsystem and crated tail motor object and init --- .../org/sert2521/bunnybots2024/Constants.kt | 1 + .../sert2521/bunnybots2024/subsystems/Tail.kt | 21 +++++++++++++++++++ 2 files changed, 22 insertions(+) create mode 100644 src/main/java/org/sert2521/bunnybots2024/subsystems/Tail.kt diff --git a/src/main/java/org/sert2521/bunnybots2024/Constants.kt b/src/main/java/org/sert2521/bunnybots2024/Constants.kt index f346e30..5d8dce9 100644 --- a/src/main/java/org/sert2521/bunnybots2024/Constants.kt +++ b/src/main/java/org/sert2521/bunnybots2024/Constants.kt @@ -10,6 +10,7 @@ object PhysicalConstants { object ElectricIDs{ const val INTAKE_ID = -1 + const val TAIL_ID = -1 } object TunedConstants{ diff --git a/src/main/java/org/sert2521/bunnybots2024/subsystems/Tail.kt b/src/main/java/org/sert2521/bunnybots2024/subsystems/Tail.kt new file mode 100644 index 0000000..234d2a6 --- /dev/null +++ b/src/main/java/org/sert2521/bunnybots2024/subsystems/Tail.kt @@ -0,0 +1,21 @@ +package org.sert2521.bunnybots2024.subsystems + +import com.revrobotics.CANSparkBase +import com.revrobotics.CANSparkLowLevel +import com.revrobotics.CANSparkMax +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.sert2521.bunnybots2024.ElectricIDs + +object TailSubsystem : SubsystemBase() { + private val tailMotor = CANSparkMax(ElectricIDs.TAIL_ID, CANSparkLowLevel.MotorType.kBrushless) + + init { + tailMotor.setSmartCurrentLimit(10) + tailMotor.idleMode = CANSparkBase.IdleMode.kBrake + tailMotor.inverted = false + } + + + + +} \ No newline at end of file From a69616d2f00a4d72537932ab786ad337459dab52 Mon Sep 17 00:00:00 2001 From: Madison C Date: Mon, 2 Dec 2024 17:42:25 -0800 Subject: [PATCH 2/6] finished tail subsystem I believe --- .../sert2521/bunnybots2024/subsystems/Tail.kt | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/sert2521/bunnybots2024/subsystems/Tail.kt b/src/main/java/org/sert2521/bunnybots2024/subsystems/Tail.kt index 234d2a6..c6942f5 100644 --- a/src/main/java/org/sert2521/bunnybots2024/subsystems/Tail.kt +++ b/src/main/java/org/sert2521/bunnybots2024/subsystems/Tail.kt @@ -8,14 +8,30 @@ import org.sert2521.bunnybots2024.ElectricIDs object TailSubsystem : SubsystemBase() { private val tailMotor = CANSparkMax(ElectricIDs.TAIL_ID, CANSparkLowLevel.MotorType.kBrushless) + private val tailEncoder = tailMotor.encoder init { - tailMotor.setSmartCurrentLimit(10) + tailMotor.setSmartCurrentLimit(40) tailMotor.idleMode = CANSparkBase.IdleMode.kBrake tailMotor.inverted = false + + tailEncoder.setPositionConversionFactor(Math.PI/24) // conversion factor pi/24 + } + fun setVoltage(voltage: Double) { + tailMotor.setVoltage(voltage) + } + fun stopMotor() { + tailMotor.stopMotor() + } + fun getAngleRads(): Double { + return tailEncoder.position + } + fun getVelocity(): Double { + return tailEncoder.velocity + } } \ No newline at end of file From 20d5748c4edcb815e49579251cd5e5f2931f4324 Mon Sep 17 00:00:00 2001 From: Dylan Myers Date: Mon, 2 Dec 2024 17:49:46 -0800 Subject: [PATCH 3/6] testing git config --- src/main/java/org/sert2521/bunnybots2024/subsystems/Tail.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/sert2521/bunnybots2024/subsystems/Tail.kt b/src/main/java/org/sert2521/bunnybots2024/subsystems/Tail.kt index c6942f5..cb4b528 100644 --- a/src/main/java/org/sert2521/bunnybots2024/subsystems/Tail.kt +++ b/src/main/java/org/sert2521/bunnybots2024/subsystems/Tail.kt @@ -34,4 +34,4 @@ object TailSubsystem : SubsystemBase() { fun getVelocity(): Double { return tailEncoder.velocity } -} \ No newline at end of file +} //test \ No newline at end of file From 03c0beb98dbe156888ae37e9f06838ea59967fe3 Mon Sep 17 00:00:00 2001 From: Dylan Myers Date: Mon, 2 Dec 2024 17:52:49 -0800 Subject: [PATCH 4/6] changed position conversion factor to a constant in tail subsystem --- src/main/java/org/sert2521/bunnybots2024/subsystems/Tail.kt | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/sert2521/bunnybots2024/subsystems/Tail.kt b/src/main/java/org/sert2521/bunnybots2024/subsystems/Tail.kt index cb4b528..e2c2f6b 100644 --- a/src/main/java/org/sert2521/bunnybots2024/subsystems/Tail.kt +++ b/src/main/java/org/sert2521/bunnybots2024/subsystems/Tail.kt @@ -9,13 +9,14 @@ import org.sert2521.bunnybots2024.ElectricIDs object TailSubsystem : SubsystemBase() { private val tailMotor = CANSparkMax(ElectricIDs.TAIL_ID, CANSparkLowLevel.MotorType.kBrushless) private val tailEncoder = tailMotor.encoder + private const val positionConversionFactor = Math.PI/24 init { tailMotor.setSmartCurrentLimit(40) tailMotor.idleMode = CANSparkBase.IdleMode.kBrake tailMotor.inverted = false - tailEncoder.setPositionConversionFactor(Math.PI/24) // conversion factor pi/24 + tailEncoder.setPositionConversionFactor(positionConversionFactor) } @@ -34,4 +35,4 @@ object TailSubsystem : SubsystemBase() { fun getVelocity(): Double { return tailEncoder.velocity } -} //test \ No newline at end of file +} \ No newline at end of file From 03bc4198bc939fbec3cb9f791ec61240b3e2b4fc Mon Sep 17 00:00:00 2001 From: Dylan Myers Date: Thu, 5 Dec 2024 17:38:05 -0800 Subject: [PATCH 5/6] Attempted tail up and tail down commands w/ PID I explain my trouble in TailSubsystem.kt --- .../org/sert2521/bunnybots2024/Constants.kt | 4 +- .../bunnybots2024/commands/TailDownCommand.kt | 41 +++++++++++++++++++ .../bunnybots2024/commands/TailUpCommand.kt | 40 ++++++++++++++++++ .../subsystems/{Tail.kt => TailSubsystem.kt} | 22 +++++++--- 4 files changed, 100 insertions(+), 7 deletions(-) create mode 100644 src/main/java/org/sert2521/bunnybots2024/commands/TailDownCommand.kt create mode 100644 src/main/java/org/sert2521/bunnybots2024/commands/TailUpCommand.kt rename src/main/java/org/sert2521/bunnybots2024/subsystems/{Tail.kt => TailSubsystem.kt} (57%) diff --git a/src/main/java/org/sert2521/bunnybots2024/Constants.kt b/src/main/java/org/sert2521/bunnybots2024/Constants.kt index 5d8dce9..e181365 100644 --- a/src/main/java/org/sert2521/bunnybots2024/Constants.kt +++ b/src/main/java/org/sert2521/bunnybots2024/Constants.kt @@ -14,6 +14,8 @@ const val INTAKE_ID = -1 } object TunedConstants{ - + const val TAIL_P = 0.5 + const val TAIL_I = 0.0 + const val TAIL_D = 0.5 } diff --git a/src/main/java/org/sert2521/bunnybots2024/commands/TailDownCommand.kt b/src/main/java/org/sert2521/bunnybots2024/commands/TailDownCommand.kt new file mode 100644 index 0000000..4255b25 --- /dev/null +++ b/src/main/java/org/sert2521/bunnybots2024/commands/TailDownCommand.kt @@ -0,0 +1,41 @@ +package org.sert2521.bunnybots2024.commands + + +import edu.wpi.first.math.controller.PIDController +import edu.wpi.first.wpilibj2.command.Command +import org.sert2521.bunnybots2024.TunedConstants +import org.sert2521.bunnybots2024.subsystems.TailSubsystem + + +class TailDownCommand : Command() { + private val tailSubsystem = TailSubsystem + private val tailPid = PIDController( + TunedConstants.TAIL_P, + TunedConstants.TAIL_I, + TunedConstants.TAIL_D + ) + + init { + addRequirements(tailSubsystem) + } + + override fun initialize() { + tailSubsystem.setInversion(false) + tailPid.setpoint = Math.PI / 2 /* assuming that the only position the tail can start this command + * in is the raised position. */ + tailPid.setTolerance(0.01) + } + + override fun execute() { + val calculate = tailPid.calculate(tailSubsystem.getAngleRads()) + tailSubsystem.setVoltage(calculate) + } + + override fun isFinished(): Boolean { + return tailPid.atSetpoint() + } + + override fun end(interrupted: Boolean) { + tailSubsystem.stopMotor() + } +} diff --git a/src/main/java/org/sert2521/bunnybots2024/commands/TailUpCommand.kt b/src/main/java/org/sert2521/bunnybots2024/commands/TailUpCommand.kt new file mode 100644 index 0000000..7efac53 --- /dev/null +++ b/src/main/java/org/sert2521/bunnybots2024/commands/TailUpCommand.kt @@ -0,0 +1,40 @@ +package org.sert2521.bunnybots2024.commands + + +import edu.wpi.first.math.controller.PIDController +import edu.wpi.first.wpilibj2.command.Command +import org.sert2521.bunnybots2024.TunedConstants +import org.sert2521.bunnybots2024.subsystems.TailSubsystem + + +class TailUpCommand : Command() { + private val tailSubsystem = TailSubsystem + private val tailPid = PIDController( + TunedConstants.TAIL_P, + TunedConstants.TAIL_I, + TunedConstants.TAIL_D + ) + + init { + addRequirements(tailSubsystem) + } + + override fun initialize() { + tailSubsystem.setInversion(true) + tailPid.setpoint = Math.PI / 2 /* here it is being assumed that the only position the tail can start this command + * in is the lowered position. */ + } + + override fun execute() { + val calculate = tailPid.calculate(tailSubsystem.getAngleRads()) + tailSubsystem.setVoltage(calculate) + } + + override fun isFinished(): Boolean { + return tailPid.atSetpoint() + } + + override fun end(interrupted: Boolean) { + tailSubsystem.stopMotor() + } +} diff --git a/src/main/java/org/sert2521/bunnybots2024/subsystems/Tail.kt b/src/main/java/org/sert2521/bunnybots2024/subsystems/TailSubsystem.kt similarity index 57% rename from src/main/java/org/sert2521/bunnybots2024/subsystems/Tail.kt rename to src/main/java/org/sert2521/bunnybots2024/subsystems/TailSubsystem.kt index e2c2f6b..61e4e4d 100644 --- a/src/main/java/org/sert2521/bunnybots2024/subsystems/Tail.kt +++ b/src/main/java/org/sert2521/bunnybots2024/subsystems/TailSubsystem.kt @@ -9,14 +9,20 @@ import org.sert2521.bunnybots2024.ElectricIDs object TailSubsystem : SubsystemBase() { private val tailMotor = CANSparkMax(ElectricIDs.TAIL_ID, CANSparkLowLevel.MotorType.kBrushless) private val tailEncoder = tailMotor.encoder - private const val positionConversionFactor = Math.PI/24 + /* + I'm not sure if using a relative encoder is the right move here + because i'm relying on the up and down commands finishing at the + same angles every time. I imagine that can lead to a compounding of slight + errors over time. + */ + private const val POSITION_CONVERSION_FACTOR = Math.PI/24 init { tailMotor.setSmartCurrentLimit(40) tailMotor.idleMode = CANSparkBase.IdleMode.kBrake - tailMotor.inverted = false +// setInversion(false) - tailEncoder.setPositionConversionFactor(positionConversionFactor) + tailEncoder.setPositionConversionFactor(POSITION_CONVERSION_FACTOR) } @@ -24,6 +30,10 @@ object TailSubsystem : SubsystemBase() { tailMotor.setVoltage(voltage) } + fun setInversion(inverted: Boolean) { + tailMotor.inverted = inverted + } + fun stopMotor() { tailMotor.stopMotor() } @@ -32,7 +42,7 @@ object TailSubsystem : SubsystemBase() { return tailEncoder.position } - fun getVelocity(): Double { - return tailEncoder.velocity - } +// fun getVelocity(): Double { +// return tailEncoder.velocity +// } } \ No newline at end of file From 5ce49023068cb1e0479bb784671429d931577ffa Mon Sep 17 00:00:00 2001 From: Dylan Myers Date: Mon, 9 Dec 2024 17:38:29 -0800 Subject: [PATCH 6/6] Implemented suggested changes --- .../bunnybots2024/commands/EmptyCommand.kt | 4 -- ...Command.kt => TailRaiseAndLowerCommand.kt} | 21 ++++++---- .../commands/TailStayStillCommand.kt | 29 ++++++++++++++ .../bunnybots2024/commands/TailUpCommand.kt | 40 ------------------- .../bunnybots2024/subsystems/TailSubsystem.kt | 24 ++++++----- 5 files changed, 57 insertions(+), 61 deletions(-) delete mode 100644 src/main/java/org/sert2521/bunnybots2024/commands/EmptyCommand.kt rename src/main/java/org/sert2521/bunnybots2024/commands/{TailDownCommand.kt => TailRaiseAndLowerCommand.kt} (53%) create mode 100644 src/main/java/org/sert2521/bunnybots2024/commands/TailStayStillCommand.kt delete mode 100644 src/main/java/org/sert2521/bunnybots2024/commands/TailUpCommand.kt diff --git a/src/main/java/org/sert2521/bunnybots2024/commands/EmptyCommand.kt b/src/main/java/org/sert2521/bunnybots2024/commands/EmptyCommand.kt deleted file mode 100644 index f5b5d49..0000000 --- a/src/main/java/org/sert2521/bunnybots2024/commands/EmptyCommand.kt +++ /dev/null @@ -1,4 +0,0 @@ -package org.sert2521.bunnybots2024.commands - -class EmptyCommand { -} \ No newline at end of file diff --git a/src/main/java/org/sert2521/bunnybots2024/commands/TailDownCommand.kt b/src/main/java/org/sert2521/bunnybots2024/commands/TailRaiseAndLowerCommand.kt similarity index 53% rename from src/main/java/org/sert2521/bunnybots2024/commands/TailDownCommand.kt rename to src/main/java/org/sert2521/bunnybots2024/commands/TailRaiseAndLowerCommand.kt index 4255b25..fbd7c19 100644 --- a/src/main/java/org/sert2521/bunnybots2024/commands/TailDownCommand.kt +++ b/src/main/java/org/sert2521/bunnybots2024/commands/TailRaiseAndLowerCommand.kt @@ -7,7 +7,7 @@ import org.sert2521.bunnybots2024.TunedConstants import org.sert2521.bunnybots2024.subsystems.TailSubsystem -class TailDownCommand : Command() { +class TailRaiseAndLowerCommand(private val setPoint: Double) : Command() { private val tailSubsystem = TailSubsystem private val tailPid = PIDController( TunedConstants.TAIL_P, @@ -20,15 +20,19 @@ class TailDownCommand : Command() { } override fun initialize() { - tailSubsystem.setInversion(false) - tailPid.setpoint = Math.PI / 2 /* assuming that the only position the tail can start this command - * in is the raised position. */ - tailPid.setTolerance(0.01) + tailPid.setpoint = setPoint + + if (setPoint > tailSubsystem.getAngleRads()) { + tailSubsystem.setInversion(false) + } + else if (setPoint < tailSubsystem.getAngleRads()) { + tailSubsystem.setInversion(true) + } } override fun execute() { - val calculate = tailPid.calculate(tailSubsystem.getAngleRads()) - tailSubsystem.setVoltage(calculate) + val nextVoltage = tailPid.calculate(tailSubsystem.getAngleRads()) + tailSubsystem.setVoltage(nextVoltage) } override fun isFinished(): Boolean { @@ -36,6 +40,7 @@ class TailDownCommand : Command() { } override fun end(interrupted: Boolean) { - tailSubsystem.stopMotor() + // not stopping motor because TailStayStillCommand() should hold the motor in place + // when the tail is not being raised or lowered. } } diff --git a/src/main/java/org/sert2521/bunnybots2024/commands/TailStayStillCommand.kt b/src/main/java/org/sert2521/bunnybots2024/commands/TailStayStillCommand.kt new file mode 100644 index 0000000..da1a11b --- /dev/null +++ b/src/main/java/org/sert2521/bunnybots2024/commands/TailStayStillCommand.kt @@ -0,0 +1,29 @@ +package org.sert2521.bunnybots2024.commands + +import edu.wpi.first.wpilibj2.command.Command +import org.sert2521.bunnybots2024.subsystems.TailSubsystem + +class TailStayStillCommand : Command() { + private val tailSubsystem = TailSubsystem + + + init { + addRequirements(tailSubsystem) + } + + override fun initialize() { + tailSubsystem.setVoltage(0.0) + } + + override fun execute() {} + + override fun isFinished(): Boolean { + return false + } + + override fun end(interrupted: Boolean) { + if (!interrupted) { // + tailSubsystem.stopMotor() + } + } +} diff --git a/src/main/java/org/sert2521/bunnybots2024/commands/TailUpCommand.kt b/src/main/java/org/sert2521/bunnybots2024/commands/TailUpCommand.kt deleted file mode 100644 index 7efac53..0000000 --- a/src/main/java/org/sert2521/bunnybots2024/commands/TailUpCommand.kt +++ /dev/null @@ -1,40 +0,0 @@ -package org.sert2521.bunnybots2024.commands - - -import edu.wpi.first.math.controller.PIDController -import edu.wpi.first.wpilibj2.command.Command -import org.sert2521.bunnybots2024.TunedConstants -import org.sert2521.bunnybots2024.subsystems.TailSubsystem - - -class TailUpCommand : Command() { - private val tailSubsystem = TailSubsystem - private val tailPid = PIDController( - TunedConstants.TAIL_P, - TunedConstants.TAIL_I, - TunedConstants.TAIL_D - ) - - init { - addRequirements(tailSubsystem) - } - - override fun initialize() { - tailSubsystem.setInversion(true) - tailPid.setpoint = Math.PI / 2 /* here it is being assumed that the only position the tail can start this command - * in is the lowered position. */ - } - - override fun execute() { - val calculate = tailPid.calculate(tailSubsystem.getAngleRads()) - tailSubsystem.setVoltage(calculate) - } - - override fun isFinished(): Boolean { - return tailPid.atSetpoint() - } - - override fun end(interrupted: Boolean) { - tailSubsystem.stopMotor() - } -} diff --git a/src/main/java/org/sert2521/bunnybots2024/subsystems/TailSubsystem.kt b/src/main/java/org/sert2521/bunnybots2024/subsystems/TailSubsystem.kt index 61e4e4d..0eab8ec 100644 --- a/src/main/java/org/sert2521/bunnybots2024/subsystems/TailSubsystem.kt +++ b/src/main/java/org/sert2521/bunnybots2024/subsystems/TailSubsystem.kt @@ -3,27 +3,30 @@ package org.sert2521.bunnybots2024.subsystems import com.revrobotics.CANSparkBase import com.revrobotics.CANSparkLowLevel import com.revrobotics.CANSparkMax +import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.sert2521.bunnybots2024.ElectricIDs +import org.sert2521.bunnybots2024.commands.TailStayStillCommand object TailSubsystem : SubsystemBase() { private val tailMotor = CANSparkMax(ElectricIDs.TAIL_ID, CANSparkLowLevel.MotorType.kBrushless) private val tailEncoder = tailMotor.encoder - /* - I'm not sure if using a relative encoder is the right move here - because i'm relying on the up and down commands finishing at the - same angles every time. I imagine that can lead to a compounding of slight - errors over time. - */ - private const val POSITION_CONVERSION_FACTOR = Math.PI/24 init { tailMotor.setSmartCurrentLimit(40) tailMotor.idleMode = CANSparkBase.IdleMode.kBrake -// setInversion(false) - tailEncoder.setPositionConversionFactor(POSITION_CONVERSION_FACTOR) + tailEncoder.setPositionConversionFactor(Math.PI/24) + // if inversion is false that means the tail was lowered when the motor was powered off + if (!tailEncoder.inverted) { + tailEncoder.setPosition(Math.PI/2) + } + + } + + override fun setDefaultCommand(defaultCommand: Command?) { + super.setDefaultCommand(TailStayStillCommand()) } fun setVoltage(voltage: Double) { @@ -36,12 +39,15 @@ object TailSubsystem : SubsystemBase() { fun stopMotor() { tailMotor.stopMotor() + } fun getAngleRads(): Double { return tailEncoder.position } + + // fun getVelocity(): Double { // return tailEncoder.velocity // }