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

Commit

Permalink
feat: log shooter inputs
Browse files Browse the repository at this point in the history
  • Loading branch information
doinkythederp committed Jan 24, 2024
1 parent 6f9783e commit 9dfc031
Show file tree
Hide file tree
Showing 4 changed files with 41 additions and 13 deletions.
12 changes: 6 additions & 6 deletions src/main/java/com/frcteam3636/frc2024/CAN.kt
Original file line number Diff line number Diff line change
Expand Up @@ -12,12 +12,12 @@ enum class REVMotorControllerId(val num: Int) {
BackLeftTurningMotor(2),
BackRightTurningMotor(3),
FrontRightTurningMotor(4),
LeftShooterFlywheel(7),
RightShooterFlywheel(15),
OverTheBumperIntakeArm(0),
OverTheBumperIntakeFeed(0),
UnderTheBumperIntakeRoller(0),
ClimberMotor(0),
LeftShooterFlywheel(15),
RightShooterFlywheel(7),
OverTheBumperIntakeArm(31),
OverTheBumperIntakeFeed(32),
UnderTheBumperIntakeRoller(33),
ClimberMotor(34),

}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@ package com.frcteam3636.frc2024.subsystems.shooter
import edu.wpi.first.wpilibj.RobotBase
import edu.wpi.first.wpilibj2.command.Command
import edu.wpi.first.wpilibj2.command.Subsystem
import org.littletonrobotics.junction.Logger

object Shooter : Subsystem {
private val io: ShooterIO = if (RobotBase.isReal()) {
Expand All @@ -15,6 +16,7 @@ object Shooter : Subsystem {

override fun periodic() {
io.updateInputs(inputs)
Logger.processInputs("Shooter", inputs)
}

fun shootCommand(): Command {
Expand Down
Original file line number Diff line number Diff line change
@@ -1,12 +1,24 @@
package com.frcteam3636.frc2024.subsystems.shooter

import org.littletonrobotics.junction.AutoLog
import edu.wpi.first.math.geometry.Rotation2d
import org.littletonrobotics.junction.LogTable
import org.littletonrobotics.junction.inputs.LoggableInputs


interface ShooterIO {
@AutoLog
class ShooterIOInputs {
class ShooterIOInputs: LoggableInputs {
var leftSpeed = Rotation2d()
var rightSpeed = Rotation2d()

override fun toLog(table: LogTable) {
table.put("Left Speed", leftSpeed)
table.put("Right Speed", rightSpeed)
}

override fun fromLog(table: LogTable) {
leftSpeed = table.get("Left Speed", leftSpeed)[0]
rightSpeed = table.get("Right Speed", rightSpeed)[0]
}
}

/** Updates the set of loggable inputs. */
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,25 +2,39 @@ package com.frcteam3636.frc2024.subsystems.shooter

import com.frcteam3636.frc2024.can.CANSparkMax
import com.frcteam3636.frc2024.can.REVMotorControllerId
import com.frcteam3636.frc2024.subsystems.climber.ClimberIOReal
import com.revrobotics.CANSparkLowLevel
import edu.wpi.first.math.geometry.Rotation2d
import edu.wpi.first.math.util.Units


class ShooterIOReal : ShooterIO {

private val left = CANSparkMax(REVMotorControllerId.LeftShooterFlywheel, CANSparkLowLevel.MotorType.kBrushless)
private val right = CANSparkMax(REVMotorControllerId.RightShooterFlywheel, CANSparkLowLevel.MotorType.kBrushless)
companion object {
const val FLYWHEEL_GEAR_RATIO = 1.0
}

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

private val right = CANSparkMax(REVMotorControllerId.RightShooterFlywheel, CANSparkLowLevel.MotorType.kBrushless).apply {
encoder.velocityConversionFactor = Units.rotationsToRadians(1.0) * ClimberIOReal.CLIMBER_GEAR_RATIO / 60
}

init {
left.inverted = true
}

override fun updateInputs(inputs: ShooterIO.ShooterIOInputs?) {
// no-op
inputs?.leftSpeed = Rotation2d(left.encoder.velocity)
inputs?.rightSpeed = Rotation2d(right.encoder.velocity)
}

override fun shoot(speed: Double) {
left.set(speed)
right.set(speed * 0.5)
right.set(speed)
}

override fun intake(speed: Double) {
Expand Down

0 comments on commit 9dfc031

Please sign in to comment.