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/TailRaiseAndLowerCommand.kt

@@ -0,0 +1,46 @@
+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 TailRaiseAndLowerCommand(private val setPoint: Double) : Command() {
+    private val tailSubsystem = TailSubsystem
+    private val tailPid = PIDController(
+        TunedConstants.TAIL_P,
+        TunedConstants.TAIL_I,
+        TunedConstants.TAIL_D
+    )
+
+    init {
+        addRequirements(tailSubsystem)
+    }
+
+    override fun initialize() {
+        tailPid.setpoint = setPoint
+
+        if (setPoint > tailSubsystem.getAngleRads()) {
+            tailSubsystem.setInversion(false)
+        }
+        else if (setPoint < tailSubsystem.getAngleRads()) {
+            tailSubsystem.setInversion(true)
+        }
+    }
+
+    override fun execute() {
+        val nextVoltage = tailPid.calculate(tailSubsystem.getAngleRads())
+        tailSubsystem.setVoltage(nextVoltage)
+    }
+
+    override fun isFinished(): Boolean {
+        return tailPid.atSetpoint()
+    }
+
+    override fun end(interrupted: Boolean) {
+        // not stopping motor because TailStayStillCommand() should hold the motor in place
+        // when the tail is not being raised or lowered.
+    }
+}

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

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

@@ -0,0 +1,54 @@
+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
+
+    init {
+        tailMotor.setSmartCurrentLimit(40)
+        tailMotor.idleMode = CANSparkBase.IdleMode.kBrake
+
+        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) {
+        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
+//    }
+}