diff --git a/src/main/java/frc/robot/Robot.kt b/src/main/java/frc/robot/Robot.kt index 88b68c3..09875ff 100755 --- a/src/main/java/frc/robot/Robot.kt +++ b/src/main/java/frc/robot/Robot.kt @@ -35,7 +35,7 @@ class Robot : TimedRobot() { override fun disabledExit() {} override fun autonomousInit() { - m_autonomousCommand = m_robotContainer?.autonomousCommand + m_autonomousCommand = m_robotContainer?.autoCommand if (m_autonomousCommand != null) { m_autonomousCommand!!.schedule() diff --git a/src/main/java/frc/robot/RobotContainer.kt b/src/main/java/frc/robot/RobotContainer.kt index eaeb74e..591b121 100755 --- a/src/main/java/frc/robot/RobotContainer.kt +++ b/src/main/java/frc/robot/RobotContainer.kt @@ -7,94 +7,224 @@ import com.ctre.phoenix6.Utils import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest -import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.* +import com.pathplanner.lib.auto.AutoBuilder +import edu.wpi.first.math.MathUtil import edu.wpi.first.math.filter.SlewRateLimiter import edu.wpi.first.math.geometry.Pose2d import edu.wpi.first.math.geometry.Rotation2d import edu.wpi.first.math.geometry.Translation2d +import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.smartdashboard.SendableChooser +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Commands import edu.wpi.first.wpilibj2.command.button.CommandXboxController +import edu.wpi.first.wpilibj2.command.button.Trigger +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine +import frc.robot.subsystems.vision.Limelight import frc.robot.generated.TunerConstants import frc.robot.subsystems.ArmSubsystem import frc.robot.subsystems.CommandSwerveDrivetrain -import frc.robot.subsystems.vision.VisionSubsystem import java.util.function.Supplier +import kotlin.math.atan2 +import kotlin.math.cos +import kotlin.math.sin class RobotContainer { - private val autoChooser: SendableChooser? = null - private val controlChooser = SendableChooser() - private val speedChooser = SendableChooser() - private val MaxSpeed = TunerConstants.kSpeedAt12VoltsMps // Initial max is true top speed + private val autoChooser: SendableChooser + private val controlChooser: SendableChooser = SendableChooser() + private val speedChooser: SendableChooser = SendableChooser() + private var MaxSpeed: Double = TunerConstants.kSpeedAt12VoltsMps // Initial max is true top speed private val TurtleSpeed = 0.1 // Reduction in speed from Max Speed, 0.1 = 10% private val MaxAngularRate = Math.PI * 1.5 // .75 rotation per second max angular velocity. Adjust for max turning rate speed. private val TurtleAngularRate = Math.PI * 0.5 // .75 rotation per second max angular velocity. Adjust for max turning rate speed. - private val AngularRate = MaxAngularRate // This will be updated when turtle and reset to MaxAngularRate + private var AngularRate = MaxAngularRate // This will be updated when turtle and reset to MaxAngularRate /* Setting up bindings for necessary control of the swerve drive platform */ - private val joystick = CommandXboxController(0) + var drv: CommandXboxController = CommandXboxController(0) // driver xbox controller + var op: CommandXboxController = CommandXboxController(1) // operator xbox controller var drivetrain: CommandSwerveDrivetrain = TunerConstants.DriveTrain // drivetrain - val camera: VisionSubsystem = VisionSubsystem() val armSubsystem: ArmSubsystem = ArmSubsystem + val limelight: Limelight = Limelight(drivetrain) + // Slew Rate Limiters to limit acceleration of joystick inputs - private val xLimiter = SlewRateLimiter(2.0) - private val yLimiter = SlewRateLimiter(0.5) - private val rotLimiter = SlewRateLimiter(0.5) + private val xLimiter: SlewRateLimiter = SlewRateLimiter(2.0) + private val yLimiter: SlewRateLimiter = SlewRateLimiter(0.5) + private val rotLimiter: SlewRateLimiter = SlewRateLimiter(0.5) // Field-centric driving in Open Loop, can change to closed loop after characterization // For closed loop replace DriveRequestType.OpenLoopVoltage with DriveRequestType.Velocity - var drive: FieldCentric = FieldCentric() + var drive: SwerveRequest.FieldCentric = SwerveRequest.FieldCentric() .withDriveRequestType(DriveRequestType.OpenLoopVoltage) .withDeadband(MaxSpeed * 0.1) // Deadband is handled on input .withRotationalDeadband(AngularRate * 0.1) - var brake: SwerveDriveBrake = SwerveDriveBrake() - var forwardStraight: RobotCentric = RobotCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage) - var point: PointWheelsAt = PointWheelsAt() + var brake: SwerveRequest.SwerveDriveBrake = SwerveRequest.SwerveDriveBrake() + var forwardStraight: SwerveRequest.RobotCentric = SwerveRequest.RobotCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage) + var point: SwerveRequest.PointWheelsAt = SwerveRequest.PointWheelsAt() + + var vision: Limelight = Limelight(drivetrain) - private val logger = Telemetry(MaxSpeed) + var logger: Telemetry = Telemetry(MaxSpeed) var odomStart: Pose2d = Pose2d(0.0, 0.0, Rotation2d(0.0, 0.0)) - private val controlStyle: Supplier? = null + private var controlStyle: Supplier? = null - private val lastControl = "2 Joysticks" - private val lastSpeed = 0.65 + private var lastControl = "2 Joysticks" + private var lastSpeed = 0.65 private fun configureBindings() { - drivetrain.defaultCommand = drivetrain.applyRequest { - drive.withVelocityX(-joystick.leftY * MaxSpeed) // Drive forward with - // negative Y (forward) - .withVelocityY(-joystick.leftX * MaxSpeed) // Drive left with negative X (left) - .withRotationalRate(-joystick.rightX * MaxAngularRate) - } // Drive counterclockwise with negative X (left) - armSubsystem.defaultCommand = armSubsystem.getCommand { joystick.rightTriggerAxis - joystick.leftTriggerAxis } - joystick.a().whileTrue(drivetrain.applyRequest { brake }) - joystick.b().whileTrue( + newControlStyle() + newSpeed() + + armSubsystem.defaultCommand = armSubsystem.getCommand { op.rightTriggerAxis - op.leftTriggerAxis } + + drv.a().whileTrue(drivetrain.applyRequest { brake }) + drv.b().whileTrue( drivetrain - .applyRequest { point.withModuleDirection(Rotation2d(-joystick.leftY, -joystick.leftX)) }, + .applyRequest { + point.withModuleDirection( + Rotation2d( + -drv.leftY, + -drv.leftX + ) + ) + } ) - // reset the field-centric heading on left bumper press - joystick.leftBumper().onTrue(drivetrain.runOnce { drivetrain.seedFieldRelative() }) + // reset the field-centric heading on start button press + drv.start().onTrue(drivetrain.runOnce { drivetrain.seedFieldRelative() }) - // face robot toward the closest note - joystick.rightBumper().onTrue(drivetrain.runOnce { point.withModuleDirection(camera.getNearestRotation()) }) + // Turtle Mode while held + drv.leftBumper().onTrue( + Commands.runOnce({ + MaxSpeed = TunerConstants.kSpeedAt12VoltsMps * TurtleSpeed + }) + .andThen({ AngularRate = TurtleAngularRate }) + ) + drv.leftBumper().onFalse( + Commands.runOnce({ + MaxSpeed = TunerConstants.kSpeedAt12VoltsMps * speedChooser.getSelected() + }) + .andThen({ AngularRate = MaxAngularRate }) + ) if (Utils.isSimulation()) { - drivetrain.seedFieldRelative(Pose2d(Translation2d(), Rotation2d.fromDegrees(90.0))) + drivetrain.seedFieldRelative(Pose2d(Translation2d(), Rotation2d.fromDegrees(0.0))) } drivetrain.registerTelemetry { state: SwerveDrivetrain.SwerveDriveState -> logger.telemeterize(state) } + + val controlPick = Trigger { lastControl !== controlChooser.getSelected() } + controlPick.onTrue(Commands.runOnce({ newControlStyle() })) + + val speedPick = Trigger { lastSpeed !== speedChooser.getSelected() } + speedPick.onTrue(Commands.runOnce({ newSpeed() })) + + drv.x().and(drv.pov(0)).whileTrue(drivetrain.runDriveQuasiTest(SysIdRoutine.Direction.kForward)) + drv.x().and(drv.pov(180)).whileTrue(drivetrain.runDriveQuasiTest(SysIdRoutine.Direction.kReverse)) + + drv.y().and(drv.pov(0)).whileTrue(drivetrain.runDriveDynamTest(SysIdRoutine.Direction.kForward)) + drv.y().and(drv.pov(180)).whileTrue(drivetrain.runDriveDynamTest(SysIdRoutine.Direction.kReverse)) + + drv.a().and(drv.pov(0)).whileTrue(drivetrain.runSteerQuasiTest(SysIdRoutine.Direction.kForward)) + drv.a().and(drv.pov(180)).whileTrue(drivetrain.runSteerQuasiTest(SysIdRoutine.Direction.kReverse)) + + drv.b().and(drv.pov(0)).whileTrue(drivetrain.runSteerDynamTest(SysIdRoutine.Direction.kForward)) + drv.b().and(drv.pov(180)).whileTrue(drivetrain.runSteerDynamTest(SysIdRoutine.Direction.kReverse)) + + // Drivetrain needs to be placed against a sturdy wall and test stopped immediately upon wheel slip + drv.back().and(drv.pov(0)).whileTrue(drivetrain.runDriveSlipTest()) + + + drv.rightBumper().whileTrue(drivetrain.rotateToAngle(limelight.getNoteCamAngle())) } init { + // Detect if controllers are missing / Stop multiple warnings + DriverStation.silenceJoystickConnectionWarning(true) + + // Build an auto chooser. This will use Commands.none() as the default option. + autoChooser = AutoBuilder.buildAutoChooser() + + controlChooser.setDefaultOption("2 Joysticks", "2 Joysticks") + controlChooser.addOption("1 Joystick Rotation Triggers", "1 Joystick Rotation Triggers") + controlChooser.addOption("Split Joysticks Rotation Triggers", "Split Joysticks Rotation Triggers") + controlChooser.addOption("2 Joysticks with Gas Pedal", "2 Joysticks with Gas Pedal") + SmartDashboard.putData("Control Chooser", controlChooser) + + speedChooser.addOption("100%", 1.0) + speedChooser.addOption("95%", 0.95) + speedChooser.addOption("90%", 0.9) + speedChooser.addOption("85%", 0.85) + speedChooser.addOption("80%", 0.8) + speedChooser.addOption("75%", 0.75) + speedChooser.addOption("70%", 0.7) + speedChooser.setDefaultOption("65%", 0.65) + speedChooser.addOption("60%", 0.6) + speedChooser.addOption("55%", 0.55) + speedChooser.addOption("50%", 0.5) + speedChooser.addOption("35%", 0.35) + SmartDashboard.putData("Speed Limit", speedChooser) + + SmartDashboard.putData("Auto Chooser", autoChooser) + configureBindings() } - val autonomousCommand: Command - get() = Commands.print("No autonomous command configured") + val autoCommand: Command? + get() =/* First put the drivetrain into auto run mode, then run the auto */ + autoChooser.getSelected() + + private fun newControlStyle() { + lastControl = controlChooser.getSelected() + when (controlChooser.getSelected()) { + "2 Joysticks" -> controlStyle = Supplier { + drive.withVelocityX(-drv.leftY * MaxSpeed) // Drive forward -Y + .withVelocityY(-drv.leftX * MaxSpeed) // Drive left with negative X (left) + .withRotationalRate(-drv.rightX * AngularRate) + } // Drive counterclockwise with negative X (left) + "1 Joystick Rotation Triggers" -> controlStyle = Supplier { + drive.withVelocityX(-drv.leftY * MaxSpeed) // Drive forward -Y + .withVelocityY(-drv.leftX * MaxSpeed) // Drive left with negative X (left) + .withRotationalRate((drv.leftTriggerAxis - drv.rightTriggerAxis) * AngularRate) + } + + "Split Joysticks Rotation Triggers" -> controlStyle = Supplier { + drive.withVelocityX(-drv.leftY * MaxSpeed) // Left stick forward/back + .withVelocityY(-drv.rightX * MaxSpeed) // Right stick strafe + .withRotationalRate((drv.leftTriggerAxis - drv.rightTriggerAxis) * AngularRate) + } + + "2 Joysticks with Gas Pedal" -> controlStyle = Supplier { + val stickX: Double = -drv.leftX + val stickY: Double = -drv.leftY + val angle: Double = atan2(stickX, stickY) + drive.withVelocityX(cos(angle) * drv.rightTriggerAxis * MaxSpeed) // left x * gas + .withVelocityY(sin(angle) * drv.rightTriggerAxis * MaxSpeed) // Angle of left stick Y * gas pedal + .withRotationalRate(-drv.rightX * AngularRate) // Drive counterclockwise with negative X (left) + } + } + try { + drivetrain.defaultCommand.cancel() + } catch (_: Exception) { + } + drivetrain.defaultCommand = drivetrain.applyRequest(controlStyle!!).ignoringDisable(true) + } + + private fun newSpeed() { + lastSpeed = speedChooser.getSelected() + MaxSpeed = TunerConstants.kSpeedAt12VoltsMps * lastSpeed + } + + private fun conditionX(joystick: Double, deadband: Double): Double { + return xLimiter.calculate(MathUtil.applyDeadband(joystick, deadband)) + } + + fun getAutonomousCommand(): Command? { + return null + } } diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.kt b/src/main/java/frc/robot/subsystems/ArmSubsystem.kt index fab72ea..fcc6e8c 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.kt +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.kt @@ -1,26 +1,60 @@ package frc.robot.subsystems -import com.ctre.phoenix6.hardware.TalonFX +import com.ctre.phoenix.motorcontrol.ControlMode +import com.ctre.phoenix.motorcontrol.FeedbackDevice +import com.ctre.phoenix.motorcontrol.can.TalonSRX +import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration +import edu.wpi.first.math.controller.ArmFeedforward +import edu.wpi.first.math.controller.PIDController +import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard + import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Subsystem object ArmSubsystem : Subsystem { - var motor: TalonFX - override fun periodic() { - // This method will be called once per scheduler run - } + var motor: TalonSRX + var pid: PIDController + var config = TalonSRXConfiguration() + var ff: ArmFeedforward + + init { // Initialize the arm subsystem - motor = TalonFX(31) + motor = TalonSRX(31) + motor.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder) + // configure pid + pid = PIDController(0.0, 0.0, 0.0) + ff = ArmFeedforward(0.0, 0.0, 0.0) + +// config.slot0.kP = pid.p +// config.slot0.kI = pid.i +// config.slot0.kD = pid.d +// config.slot0.kF = ff. +// +// SmartDashboard.putData(pid) + } + + // quadrature encoder + override fun periodic() { + // This method will be called once per scheduler run + // check if pid controller changed +// if ((kp != pid.p) || (ki != pid.i) || (kd != pid.d)) { +// kp = pid.p +// ki = pid.i +// kd = pid.d +// } } fun extend(i: Double) { - motor.set(-0.4 * i) + motor.set(ControlMode.PercentOutput, -0.4 * i) } fun retract(i: Double) { - motor.set(0.4 * i) + motor.set(ControlMode.PercentOutput, 0.4 * i) } // i love programming fun getCommand(joystickTriggerValue: () -> Double): Command { diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.kt b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.kt index b95d378..47dfea0 100755 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.kt +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.kt @@ -5,21 +5,45 @@ import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest +import com.ctre.phoenix6.signals.NeutralModeValue +import com.pathplanner.lib.auto.AutoBuilder +import com.pathplanner.lib.commands.FollowPathCommand +import com.pathplanner.lib.commands.PathPlannerAuto +import com.pathplanner.lib.controllers.PPHolonomicDriveController +import com.pathplanner.lib.path.GoalEndState +import com.pathplanner.lib.path.PathConstraints +import com.pathplanner.lib.path.PathPlannerPath +import com.pathplanner.lib.util.HolonomicPathFollowerConfig +import com.pathplanner.lib.util.PIDConstants +import com.pathplanner.lib.util.ReplanningConfig +import edu.wpi.first.math.geometry.Pose2d import edu.wpi.first.math.geometry.Rotation2d +import edu.wpi.first.math.kinematics.ChassisSpeeds +import edu.wpi.first.units.Measure +import edu.wpi.first.units.Units.* +import edu.wpi.first.units.Voltage import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.DriverStation.Alliance import edu.wpi.first.wpilibj.Notifier import edu.wpi.first.wpilibj.RobotController import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Subsystem +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine +import frc.robot.Util.ModifiedSignalLogger +import frc.robot.Util.SwerveVoltageRequest +import frc.robot.generated.TunerConstants +import java.util.* import java.util.function.Supplier +import kotlin.math.PI +import kotlin.math.max /** - * Class that extends the Phoenix SwerveDrivetrain class and implements - * subsystem so it can be used in command-based projects easily. + * Class that extends the Phoenix SwerveDrivetrain class and implements subsystem + * so it can be used in command-based projects easily. */ -@Suppress("ConstPropertyName") class CommandSwerveDrivetrain : SwerveDrivetrain, Subsystem { + private val autoRequest: SwerveRequest.ApplyChassisSpeeds = SwerveRequest.ApplyChassisSpeeds() + private var driveBaseRadius = 0.0 private var simNotifier: Notifier? = null private var lastSimTime = 0.0 @@ -37,24 +61,52 @@ class CommandSwerveDrivetrain : SwerveDrivetrain, Subsystem { odometryUpdateFrequency: Double, vararg modules: SwerveModuleConstants?, ) : super(driveTrainConstants, odometryUpdateFrequency, *modules) { - if (Utils.isSimulation()) { - startSimThread() + configNeutralMode(NeutralModeValue.Coast) + for (moduleLocation in m_moduleLocations) { + driveBaseRadius = max(driveBaseRadius, moduleLocation.norm) + if (Utils.isSimulation()) { + startSimThread() + } } + configurePathPlanner() } constructor(driveTrainConstants: SwerveDrivetrainConstants, vararg modules: SwerveModuleConstants?) : super( driveTrainConstants, *modules, ) { + configNeutralMode(NeutralModeValue.Coast) + configurePathPlanner() if (Utils.isSimulation()) { startSimThread() } } + private fun configurePathPlanner() { + + + AutoBuilder.configureHolonomic( + { this.state.Pose }, // Supplier of current robot pose + this::seedFieldRelative, // Consumer for seeding pose against auto + { this.currentRobotChassisSpeeds }, + { speeds -> this.setControl(autoRequest.withSpeeds(speeds)) }, // Consumer of ChassisSpeeds to drive the robot + holonomicPathFollowerConfig(), + getAlliance(), + this + ) // Subsystem for requirements + } + + private fun holonomicPathFollowerConfig() = HolonomicPathFollowerConfig( + PIDConstants(10.0, 0.0, 0.0), + PIDConstants(10.0, 0.0, 0.0), + TunerConstants.kSpeedAt12VoltsMps, + driveBaseRadius, + ReplanningConfig() + ) + fun applyRequest(requestSupplier: Supplier): Command { return run { this.setControl(requestSupplier.get()) } } - private fun startSimThread() { lastSimTime = Utils.getCurrentTimeSeconds() @@ -70,6 +122,104 @@ class CommandSwerveDrivetrain : SwerveDrivetrain, Subsystem { simNotifier!!.startPeriodic(simLoopPeriod) } + fun getAutoPath(pathName: String?): Command { + return PathPlannerAuto(pathName) + } + + fun rotateToAngle(angle: Double): Command { + // this takes a lot of building up + // first get the current angle + val currentAngle = this.state.Pose.rotation.degrees + // then get the difference between the current angle and the target angle + val angleDifference = angle - currentAngle + // create a Pose2d + val targetPose = Pose2d(0.0, 0.0, Rotation2d.fromDegrees(angleDifference)) + // and now the full thing + val path = PathPlannerPath( + PathPlannerPath.bezierFromPoses(targetPose), constraints, GoalEndState(0.0, Rotation2d(angleDifference)) + ) + // follow path command + val followPathCommand = FollowPathCommand( + path, + { this.state.Pose }, + { this.currentRobotChassisSpeeds }, + { speeds -> useSpeeds(speeds) }, + getHolonomicDriveController(), + getReplanningConfig(), + getAlliance(), + this + ) + return followPathCommand // i hate this so much + } + + var constraints = PathConstraints(9.46, 2.0, 2 * PI, 4 * PI) + get() { + return field + } + + fun useSpeeds(speeds: ChassisSpeeds) { + autoRequest.withSpeeds(speeds) + } + + private fun getHolonomicDriveController() = PPHolonomicDriveController( + PIDConstants(10.0, 0.0, 0.0), PIDConstants(10.0, 0.0, 0.0), + // max module speed is 9.46 m/s, convert to ft/s + FeetPerSecond.of(9.46).`in`(MetersPerSecond), driveBaseRadius + + ) + + private fun getAlliance() = alliance@{ + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field during auto only. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + val alliance: Optional = DriverStation.getAlliance() + if (alliance.isPresent) { + return@alliance (alliance.get() == DriverStation.Alliance.Red) and !DriverStation.isTeleop() + } + false + } + + private fun getReplanningConfig() = ReplanningConfig(true, true) + + override fun simulationPeriodic() {/* Assume 20ms update rate, get battery voltage from WPILib */ + updateSimState(0.02, RobotController.getBatteryVoltage()) + } + + + val currentRobotChassisSpeeds: ChassisSpeeds + get() = m_kinematics.toChassisSpeeds(*state.ModuleStates) + + private val driveVoltageRequest: SwerveVoltageRequest = SwerveVoltageRequest(true) + + private val m_driveSysIdRoutine: SysIdRoutine = SysIdRoutine( + SysIdRoutine.Config(null, null, null, ModifiedSignalLogger.logState()), SysIdRoutine.Mechanism( + { volts: Measure -> + setControl( + driveVoltageRequest.withVoltage( + volts.`in`( + Volts + ) + ) + ) + }, null, this + ) + ) + + private val steerVoltageRequest: SwerveVoltageRequest = SwerveVoltageRequest(false) + + private val m_steerSysIdRoutine: SysIdRoutine = SysIdRoutine( + SysIdRoutine.Config(null, null, null, ModifiedSignalLogger.logState()), SysIdRoutine.Mechanism( + { volts: Measure -> + setControl( + steerVoltageRequest.withVoltage( + volts.`in`( + Volts + ) + ) + ) + }, null, this + ) + ) override fun periodic() { /* Periodically try to apply the operator perspective */ /* If we haven't applied the operator perspective before, then we should apply it regardless of DS state */ @@ -89,6 +239,42 @@ class CommandSwerveDrivetrain : SwerveDrivetrain, Subsystem { } } + private val m_slipSysIdRoutine: SysIdRoutine = SysIdRoutine( + SysIdRoutine.Config( + Volts.of(0.25).per(Seconds.of(1.0)), null, null, ModifiedSignalLogger.logState() + ), SysIdRoutine.Mechanism( + { volts: Measure -> + setControl( + driveVoltageRequest.withVoltage( + volts.`in`( + Volts + ) + ) + ) + }, null, this + ) + ) + + fun runDriveQuasiTest(direction: SysIdRoutine.Direction?): Command { + return m_driveSysIdRoutine.quasistatic(direction) + } + + fun runDriveDynamTest(direction: SysIdRoutine.Direction?): Command { + return m_driveSysIdRoutine.dynamic(direction) + } + + fun runSteerQuasiTest(direction: SysIdRoutine.Direction?): Command { + return m_steerSysIdRoutine.quasistatic(direction) + } + + fun runSteerDynamTest(direction: SysIdRoutine.Direction?): Command { + return m_steerSysIdRoutine.dynamic(direction) + } + + fun runDriveSlipTest(): Command { + return m_slipSysIdRoutine.quasistatic(SysIdRoutine.Direction.kForward) + } + companion object { private const val simLoopPeriod = 0.005 // 5 ms } diff --git a/src/main/java/frc/robot/subsystems/vision/Limelight.kt b/src/main/java/frc/robot/subsystems/vision/Limelight.kt new file mode 100755 index 0000000..96a09fd --- /dev/null +++ b/src/main/java/frc/robot/subsystems/vision/Limelight.kt @@ -0,0 +1,73 @@ +package frc.robot.subsystems.vision + +import edu.wpi.first.apriltag.AprilTagFields +import edu.wpi.first.math.geometry.* +import edu.wpi.first.wpilibj.DriverStation.Alliance +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard +import edu.wpi.first.wpilibj2.command.SubsystemBase +import frc.robot.subsystems.CommandSwerveDrivetrain +import frc.robot.Util.RectanglePoseArea +import org.photonvision.EstimatedRobotPose +import org.photonvision.PhotonCamera +import org.photonvision.PhotonPoseEstimator +import kotlin.jvm.optionals.getOrNull + +class Limelight(var drivetrain: CommandSwerveDrivetrain) : SubsystemBase() { + var alliance: Alliance? = null + private val leftCam = PhotonCamera("Left Camera") + private val rightCam = PhotonCamera("Right Camera") + private val noteCam = PhotonCamera("ShitCam") + private val botToLeftCamera = Transform3d(Translation3d(0.0, 0.0, 0.0), Rotation3d(0.0, 0.0, 0.0)) // TODO get this value + private val botToRightCamera = Transform3d(Translation3d(0.0, 0.0, 0.0), Rotation3d(0.0, 0.0, 0.0)) // TODO get this value + private var enable = false + private var trust = false + private var fieldError = 0 + private var distanceError = 0 + private var botpose: Pose2d? = null + private var aprilTagFieldLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField() + private var rightPhotonPoseEstimator = PhotonPoseEstimator(aprilTagFieldLayout, PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, rightCam, botToRightCamera) + private var leftPhotonPoseEstimator = PhotonPoseEstimator(aprilTagFieldLayout, PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, leftCam, botToLeftCamera) + + /** Creates a new Limelight. */ + init { + SmartDashboard.putNumber("Field Error", fieldError.toDouble()) + SmartDashboard.putNumber("Limelight Error", distanceError.toDouble()) + + } + + + fun updateLeft(): EstimatedRobotPose? { + return leftPhotonPoseEstimator.update().getOrNull() + } + fun updateRight(): EstimatedRobotPose? { + return rightPhotonPoseEstimator.update().getOrNull() + } + + + fun useLimelight(enable: Boolean) { + this.enable = enable + } + + fun trustLL(trust: Boolean) { + this.trust = trust + } + + override fun periodic() { + val right = updateRight() + val left = updateLeft() + if (right != null) { + drivetrain.addVisionMeasurement(right.estimatedPose.toPose2d(), right.timestampSeconds) + } + if (left != null) { + drivetrain.addVisionMeasurement(left.estimatedPose.toPose2d(), left.timestampSeconds) + } + } + + fun getNoteCamAngle(): Double { + return noteCam.getLatestResult().getBestTarget().yaw + } + + companion object { + private val field = RectanglePoseArea(Translation2d(0.0, 0.0), Translation2d(16.54, 8.02)) + } +} \ No newline at end of file diff --git a/vendordeps/Phoenix5-frc2024-latest.json b/vendordeps/Phoenix5-frc2024-latest.json new file mode 100644 index 0000000..ff7359e --- /dev/null +++ b/vendordeps/Phoenix5-frc2024-latest.json @@ -0,0 +1,151 @@ +{ + "fileName": "Phoenix5.json", + "name": "CTRE-Phoenix (v5)", + "version": "5.33.1", + "frcYear": 2024, + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2024-latest.json", + "requires": [ + { + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", + "offlineFileName": "Phoenix6.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.33.1" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.33.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.33.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.33.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.33.1", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.33.1", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.33.1", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "5.33.1", + "libName": "CTRE_Phoenix_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "api-cpp-sim", + "version": "5.33.1", + "libName": "CTRE_PhoenixSim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.33.1", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file