Tail subsystem and commands

src/main/java/org/sert2521/bunnybots2024/Constants.kt

@@ -10,9 +10,12 @@ object PhysicalConstants {
 
 object ElectricIDs{
 const val INTAKE_ID = -1
+    const val TAIL_ID = -1
 }
 
 object TunedConstants{
-
+    const val TAIL_P = 0.5
+    const val TAIL_I = 0.0
+    const val TAIL_D = 0.5
 }
 

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()
+    }
+}

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()
+    }
+}

src/main/java/org/sert2521/bunnybots2024/subsystems/TailSubsystem.kt

@@ -0,0 +1,48 @@
+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)
+    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)
+
+    }
+
+    fun setVoltage(voltage: Double) {
+        tailMotor.setVoltage(voltage)
+    }
+
+    fun setInversion(inverted: Boolean) {
+        tailMotor.inverted = inverted
+    }
+
+    fun stopMotor() {
+        tailMotor.stopMotor()
+    }
+
+    fun getAngleRads(): Double {
+        return tailEncoder.position
+    }
+
+//    fun getVelocity(): Double {
+//        return tailEncoder.velocity
+//    }
+}