Skip to content
This repository has been archived by the owner on Dec 11, 2024. It is now read-only.

Commit

Permalink
feat(shooter): add rev shooterIO implementation
Browse files Browse the repository at this point in the history
  • Loading branch information
@rotatingcow
rotatingcow committed Feb 2, 2024
1 parent f64ea6d commit 3c79f6e
Show file tree
Hide file tree
Showing 3 changed files with 174 additions and 18 deletions.
31 changes: 18 additions & 13 deletions src/main/java/com/frcteam3636/frc2024/subsystems/shooter/Shooter.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@ import com.ctre.phoenix6.controls.TorqueCurrentFOC
import com.frcteam3636.frc2024.subsystems.drivetrain.Drivetrain
import com.frcteam3636.frc2024.utils.math.PIDController
import com.frcteam3636.frc2024.utils.math.PIDGains
import com.frcteam3636.frc2024.utils.math.dot
import edu.wpi.first.math.filter.SlewRateLimiter
import edu.wpi.first.math.geometry.Rotation2d
import edu.wpi.first.math.geometry.Translation2d
Expand All @@ -33,7 +34,7 @@ object Shooter : Subsystem {
const val JERK_PROFILE = 0.0

private val io: ShooterIO = if (RobotBase.isReal()) {
ShooterIOReal()
ShooterIORealTalon()
} else {
TODO()
}
Expand All @@ -46,7 +47,6 @@ object Shooter : Subsystem {
val tab = Shuffleboard.getTab("Shooter")
val shouldSpin = tab.add("Should Spin", true).withWidget(BuiltInWidgets.kToggleSwitch).entry

val motionMagicTorqueCurrentFOCRequest = MotionMagicTorqueCurrentFOC(0.0)

val targetVelocity = tab.add("Target Velocity", 0.0).withWidget(BuiltInWidgets.kNumberSlider)
.withProperties(mapOf(Pair("min", 0.0), Pair("max", 6000.0))).entry
Expand Down Expand Up @@ -75,23 +75,29 @@ object Shooter : Subsystem {
* @param targetPosition the target to aim at
*/
fun aimAtAndTrack(position: Translation2d, targetPosition: TargetPosition): Command {

val setpoint = getAngleTo(targetPosition.position, position)
val distanceVector = targetPosition.position.toTranslation2d().minus(position)
val targetHeight = targetPosition.position.z
val distance = distanceVector.norm
val normalizedDistanceVector = distanceVector.div(distance)
val derivativeDistance = Drivetrain.chassisSpeeds.vxMetersPerSecond * normalizedDistanceVector.x +
Drivetrain.chassisSpeeds.vyMetersPerSecond * normalizedDistanceVector.y

val derivativeDistance =
Translation2d(Drivetrain.chassisSpeeds.vxMetersPerSecond, Drivetrain.chassisSpeeds.vyMetersPerSecond)
.dot(normalizedDistanceVector)

return runOnce {
io.setPivotControlRequest(
PositionTorqueCurrentFOC(0.0).withSlot(0).apply {
Position = setpoint.rotations
Velocity = getVelocityToTarget(distance, targetHeight).rotations * derivativeDistance
}
io.setPivotPositionWithVelocity(
setpoint,
Rotation2d.fromRotations(
Shooter.getVelocityToTarget(
distance,
targetHeight
).rotations * derivativeDistance
)

)
}

}

/**
Expand All @@ -101,9 +107,7 @@ object Shooter : Subsystem {
*/

fun startPivotingTo(setpoint: Rotation2d): Command = runOnce {
io.setPivotControlRequest(
motionMagicTorqueCurrentFOCRequest.withPosition(setpoint.rotations)
)
io.setPivotPosition(setpoint)
}

/**
Expand Down Expand Up @@ -179,6 +183,7 @@ enum class PivotPosition(val position: Rotation2d) {

enum class TargetPosition(val position: Translation3d) {
Speaker(Translation3d()),

//this shit prolly never gonna happen lol
Trap1(Translation3d()),
Trap2(Translation3d()),
Expand Down
157 changes: 152 additions & 5 deletions src/main/java/com/frcteam3636/frc2024/subsystems/shooter/ShooterIO.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,16 +3,25 @@ package com.frcteam3636.frc2024.subsystems.shooter
import com.ctre.phoenix6.configs.MotionMagicConfigs
import com.ctre.phoenix6.configs.TalonFXConfiguration
import com.ctre.phoenix6.controls.ControlRequest
import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC
import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC
import com.frcteam3636.frc2024.CANSparkMax
import com.frcteam3636.frc2024.CTREMotorControllerId
import com.frcteam3636.frc2024.REVMotorControllerId
import com.frcteam3636.frc2024.TalonFX
import com.frcteam3636.frc2024.utils.math.PIDController
import com.frcteam3636.frc2024.utils.math.PIDGains
import com.revrobotics.CANSparkLowLevel
import edu.wpi.first.math.controller.ArmFeedforward
import edu.wpi.first.math.geometry.Rotation2d
import edu.wpi.first.math.trajectory.TrapezoidProfile
import edu.wpi.first.math.util.Units
import edu.wpi.first.wpilibj.Timer
import org.littletonrobotics.junction.LogTable
import org.littletonrobotics.junction.Logger
import org.littletonrobotics.junction.inputs.LoggableInputs
import kotlin.math.abs


interface ShooterIO {
class ShooterIOInputs : LoggableInputs {
Expand Down Expand Up @@ -51,10 +60,138 @@ interface ShooterIO {
/** Run the launcher flywheels in reverse to intake at the specified percent speed. */
fun intake(speed: Double)

fun setPivotControlRequest(control: ControlRequest)
fun setPivotPosition(setpoint: Rotation2d)

fun setPivotPositionWithVelocity(setpoint: Rotation2d, velocity: Rotation2d)
}


class ShooterIORealSparkMax : ShooterIO {
internal companion object Constants {
val ACCELERATION_PROFILE = 0.0
val VELOCITY_PROFILE = 0.0

const val PIVOT_GEAR_RATIO = 1 / 90.0
const val FLYWHEEL_GEAR_RATIO = 1.0
}

private val profile: TrapezoidProfile =
TrapezoidProfile(
TrapezoidProfile.Constraints(
ACCELERATION_PROFILE,
VELOCITY_PROFILE
))


private val leftFlywheels =
CANSparkMax(
REVMotorControllerId.LeftShooterFlywheel,
CANSparkLowLevel.MotorType.kBrushless
).apply {
inverted = true
encoder.positionConversionFactor = Units.rotationsToRadians(1.0) * FLYWHEEL_GEAR_RATIO
encoder.velocityConversionFactor = Units.rotationsToRadians(1.0) * FLYWHEEL_GEAR_RATIO / 60
}

private val rightFlywheels = CANSparkMax(
REVMotorControllerId.RightShooterFlywheel,
CANSparkLowLevel.MotorType.kBrushless
).apply {
inverted = true
encoder.positionConversionFactor = Units.rotationsToRadians(1.0) * FLYWHEEL_GEAR_RATIO
encoder.velocityConversionFactor = Units.rotationsToRadians(1.0) * FLYWHEEL_GEAR_RATIO / 60
}

private val leftPivot =
CANSparkMax(
REVMotorControllerId.LeftPivotMotor,
CANSparkLowLevel.MotorType.kBrushless)
.apply {
inverted = true
encoder.positionConversionFactor = Units.rotationsToRadians(1.0) * PIVOT_GEAR_RATIO
encoder.velocityConversionFactor = Units.rotationsToRadians(1.0) * PIVOT_GEAR_RATIO / 60
}

private val rightPivot =
CANSparkMax(
REVMotorControllerId.RightPivotMotor,
CANSparkLowLevel.MotorType.kBrushless)
.apply {
inverted = true
encoder.positionConversionFactor = Units.rotationsToRadians(1.0) * PIVOT_GEAR_RATIO
encoder.velocityConversionFactor = Units.rotationsToRadians(1.0) * PIVOT_GEAR_RATIO / 60
}

private val timer: Timer = Timer()

private val pivotPID = PIDController(PIDGains(0.0, 0.0, 0.0))
private val pivotFeedForward = ArmFeedforward(0.0, 0.0, 0.0, 0.0)


override fun updateInputs(inputs: ShooterIO.ShooterIOInputs) {
inputs.leftSpeed = Rotation2d(leftFlywheels.encoder.velocity)
inputs.rightSpeed = Rotation2d(rightFlywheels.encoder.velocity)
inputs.pivotPosition = Rotation2d(leftPivot.encoder.position * PIVOT_GEAR_RATIO)
inputs.pivotVelocity = Rotation2d(leftPivot.encoder.velocity * PIVOT_GEAR_RATIO)
inputs.pivotAcceleration = Rotation2d(leftPivot.encoder.velocity * PIVOT_GEAR_RATIO)
}

override fun shoot(speed: Double, spin: Boolean) {
if (spin) {
leftFlywheels.set(speed)
rightFlywheels.set(speed)
} else {
leftFlywheels.set(speed)
rightFlywheels.set(speed * 0.75)
}
}

override fun intake(speed: Double) {
leftFlywheels.set(-speed)
rightFlywheels.set(-speed)
}

override fun setPivotPosition(setpoint: Rotation2d) {

val goalState = TrapezoidProfile.State(setpoint.radians, 0.0)
timer.start()
do {
val current = TrapezoidProfile.State(
Rotation2d(leftPivot.encoder.position * PIVOT_GEAR_RATIO).radians,
Rotation2d(leftPivot.encoder.velocity * PIVOT_GEAR_RATIO / 60).radians
)
val goal = profile.calculate(timer.get(), current, goalState)
val voltage = pivotFeedForward.calculate(current.position, goal.velocity) + pivotPID.calculate(current.position, goal.position)
leftPivot.setVoltage(voltage)
rightPivot.setVoltage(voltage)
}
while(!( (abs(leftPivot.encoder.position - setpoint.radians) > 0.1) || abs(rightPivot.encoder.position - setpoint.radians) > 0.1 ))

timer.stop()
timer.reset()
}

override fun setPivotPositionWithVelocity(setpoint: Rotation2d, velocity: Rotation2d) {
val goalState = TrapezoidProfile.State(setpoint.radians, velocity.radians)
timer.start()
do {
val current = TrapezoidProfile.State(
Rotation2d(leftPivot.encoder.position * PIVOT_GEAR_RATIO).radians,
Rotation2d(leftPivot.encoder.velocity * PIVOT_GEAR_RATIO / 60).radians
)
val goal = profile.calculate(timer.get(), current, goalState)
val voltage = pivotFeedForward.calculate(current.position, goal.velocity) + pivotPID.calculate(current.position, goal.position)
leftPivot.setVoltage(voltage)
rightPivot.setVoltage(voltage)
}
while(!( (abs(leftPivot.encoder.position - setpoint.radians) > 0.1) || abs(rightPivot.encoder.position - setpoint.radians) > 0.1 ))

timer.stop()
timer.reset()
}
}

class ShooterIOReal : ShooterIO {
class ShooterIORealTalon : ShooterIO {

internal companion object Constants {
const val FLYWHEEL_GEAR_RATIO = 1.0
Expand Down Expand Up @@ -157,7 +294,19 @@ class ShooterIOReal : ShooterIO {
Logger.recordOutput("Shooter/Right Power", rSpeed)
}

override fun setPivotControlRequest(control: ControlRequest) {
override fun setPivotPosition(setpoint: Rotation2d) {
val control = MotionMagicTorqueCurrentFOC(0.0).withPosition(setpoint.rotations)
pivotLeftKraken.setControl(control)
pivotRightKraken.setControl(control)
}

override fun setPivotPositionWithVelocity(setpoint: Rotation2d, velocity: Rotation2d) {

val control = PositionTorqueCurrentFOC(0.0).withSlot(0).apply {
Position = setpoint.rotations
Velocity = velocity.rotations
}

pivotLeftKraken.setControl(control)
pivotRightKraken.setControl(control)
}
Expand All @@ -170,6 +319,4 @@ class ShooterIOReal : ShooterIO {
}




}
4 changes: 4 additions & 0 deletions src/main/java/com/frcteam3636/frc2024/utils/math/Math.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,3 +8,7 @@ const val TAU = PI * 2
fun Translation2d.fromPolar(magnitude: Double, angle: Double): Translation2d {
return Translation2d(magnitude * Math.cos(angle), magnitude * Math.sin(angle))
}

fun Translation2d.dot(other: Translation2d): Double {
return x * other.x + y * other.y
}

0 comments on commit 3c79f6e

Please sign in to comment.