diff --git a/src/main/java/cshcyberhawks/lib/commands/ForCommand.kt b/src/main/java/cshcyberhawks/lib/commands/ForCommand.kt index 2ed6eec..8537e8e 100644 --- a/src/main/java/cshcyberhawks/lib/commands/ForCommand.kt +++ b/src/main/java/cshcyberhawks/lib/commands/ForCommand.kt @@ -35,6 +35,20 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup // } //} +/** + * A command that executes a sequence of commands based on a loop counter. + * + * @param max The maximum value for the loop counter (exclusive) + * @param callback A function that takes the loop counter as input and returns a Command to execute + * + * Example usage: + * ``` + * ForCommand(5) { i -> + * Commands.runOnce { println("Loop iteration $i") } + * } + * ``` + * This would print the loop counter 5 times in sequence upon scheduling the ForCommand. + */ class ForCommand(private val max: Int, private val callback: (i: Int) -> Command) : SequentialCommandGroup( *(0.. = mutableListOf() + private var deadline: Optional<() -> Boolean> = Optional.empty() /** * Adds a prerequisite to the current request. @@ -51,7 +54,15 @@ abstract class Request { return this } + fun withDeadline(deadline: () -> Boolean): Request { + this.deadline = Optional.of(deadline) + + return this + } + fun allowed(): Boolean { return prerequisites.all { prerequisite -> prerequisite.met() } } + + fun deadlineHit() = deadline.isPresent && deadline.get()() } \ No newline at end of file diff --git a/src/main/java/cshcyberhawks/lib/requests/SequentialRequest.kt b/src/main/java/cshcyberhawks/lib/requests/SequentialRequest.kt index 5c57146..872b8e7 100644 --- a/src/main/java/cshcyberhawks/lib/requests/SequentialRequest.kt +++ b/src/main/java/cshcyberhawks/lib/requests/SequentialRequest.kt @@ -13,7 +13,7 @@ class SequentialRequest(vararg requests: Request) : Request() { private var startedCurrentRequest = false private fun startNextRequest() { - if (idleRequests.isNotEmpty() && currentRequest.isFinished()) { + if (idleRequests.isNotEmpty() && (currentRequest.isFinished() || currentRequest.deadlineHit()) && startedCurrentRequest) { currentRequest = idleRequests.removeAt(0) startedCurrentRequest = false startIfAllowed() diff --git a/src/main/java/frc/robot/Robot.kt b/src/main/java/frc/robot/Robot.kt index c03aeaa..5a53ecb 100644 --- a/src/main/java/frc/robot/Robot.kt +++ b/src/main/java/frc/robot/Robot.kt @@ -55,6 +55,8 @@ object Robot : TimedRobot() { SmartDashboard.putBoolean("Start by unclimbing?", false) + SmartDashboard.putBoolean("Start with a disabled climb", false) + CanBridge.runTCP() } @@ -75,6 +77,8 @@ object Robot : TimedRobot() { SmartDashboard.putBoolean("Teleop pose difference?", true) + + RobotContainer OperatorControls } @@ -117,6 +121,8 @@ object Robot : TimedRobot() { RobotContainer.startByUnclimbing = SmartDashboard.getBoolean("Start by unclimbing?", false) // println(RobotContainer.startByUnclimbing) + RobotContainer.startWithADisabledClimb = SmartDashboard.getBoolean("Start with a disabled climb", false) + RobotContainer.vision.updateOdometryFromDisabled() } @@ -165,6 +171,12 @@ object Robot : TimedRobot() { println("unclilmbing") Superstructure.unclimb() } + + if (RobotContainer.startWithADisabledClimb) { + println("starting with climb disabled") + Superstructure.disableClimb() + } + // } else if (!Superstructure.climbSystem.isStow() || !Superstructure.funnelSystem.isStow()) { // Superstructure.climbStowThenStow() // } else { diff --git a/src/main/java/frc/robot/RobotContainer.kt b/src/main/java/frc/robot/RobotContainer.kt index 8dead42..ea056dc 100644 --- a/src/main/java/frc/robot/RobotContainer.kt +++ b/src/main/java/frc/robot/RobotContainer.kt @@ -24,6 +24,8 @@ object RobotContainer { var startByUnclimbing = false + var startWithADisabledClimb = false + val xbox = CommandXboxController(2) @@ -59,7 +61,7 @@ object RobotContainer { // NamedCommands.registerCommand(pathplannerCommand.name, pathplannerCommand.cmd) // } - autoCommand = frc.robot.commands.auto.AutoBuilder.twoL4Left() + autoCommand = frc.robot.commands.auto.AutoBuilder.twoL4Right() // autoCommand = getAutonomousCommand() // SmartDashboard.putData("Auto Chooser", autoChooser) } diff --git a/src/main/java/frc/robot/commands/GoToPose.kt b/src/main/java/frc/robot/commands/GoToPose.kt index dada3b9..29065d2 100644 --- a/src/main/java/frc/robot/commands/GoToPose.kt +++ b/src/main/java/frc/robot/commands/GoToPose.kt @@ -96,10 +96,16 @@ open class GoToPose( RobotContainer.drivetrain.applyDriveRequest(xVel, yVel, rotVel) } - override fun isFinished(): Boolean = endCondition() + override fun isFinished(): Boolean { + + val isDone =endCondition() + SmartDashboard.putBoolean("GoToPose Is Done?", isDone) + + return isDone + } override fun end(interrupted: Boolean) { - println("drive command and then ending") +// println("drive command and then ending") RobotState.autoDriving = false RobotContainer.currentDriveCommand = Optional.empty() } diff --git a/src/main/java/frc/robot/commands/TeleopAutoAlign.kt b/src/main/java/frc/robot/commands/TeleopAutoAlign.kt index 42ca6c5..3736103 100644 --- a/src/main/java/frc/robot/commands/TeleopAutoAlign.kt +++ b/src/main/java/frc/robot/commands/TeleopAutoAlign.kt @@ -1,6 +1,7 @@ package frc.robot.commands import cshcyberhawks.lib.math.MiscCalculations +import edu.wpi.first.math.geometry.Rotation2d import edu.wpi.first.wpilibj2.command.Command import frc.robot.RobotContainer import frc.robot.constants.AutoScoringConstants @@ -36,14 +37,18 @@ object TeleopAutoAlign { if (OperatorControls.noWalk) { AllianceFlipUtil.apply(AutoScoringConstants.getReefPoseAtOffset(position.ordinal, side, 0.0)) } else { - val goalPose = AllianceFlipUtil.apply(AutoScoringConstants.getReefPoseAtOffset(position.ordinal, side, 0.0)) + val goalPose = + AllianceFlipUtil.apply(AutoScoringConstants.getReefPoseAtOffset(position.ordinal, side, 0.0)) // Make the position get closer as it gets closer to the final goal // val adjustX: Double = 0.3 - var adjustX: Double = min(1.0, RobotContainer.drivetrain.getSwervePose().translation.getDistance(goalPose.translation) * 0.75) + var adjustX: Double = min( + 1.0, + RobotContainer.drivetrain.getSwervePose().translation.getDistance(goalPose.translation) * 0.5 + ) // val adjustX = 1.0 // Once we're close enough we can just jump to the goal - if (adjustX < 0.25) { + if (adjustX < 0.2) { adjustX = 0.0 } @@ -62,11 +67,14 @@ object TeleopAutoAlign { val goalPose = AllianceFlipUtil.apply(AutoScoringConstants.getAlgaePoseAtOffset(position.ordinal, 0.0)) // Make the position get closer as it gets closer to the final goal // val adjustX: Double = 0.3 - var adjustX: Double = min(1.0, RobotContainer.drivetrain.getSwervePose().translation.getDistance(goalPose.translation) * 0.75) + var adjustX: Double = min( + 1.0, + RobotContainer.drivetrain.getSwervePose().translation.getDistance(goalPose.translation) * .5 + ) // val adjustX = 1.0 // Once we're close enough we can just jump to the goal - if (adjustX < 0.25) { + if (adjustX < 0.2) { adjustX = 0.0 } @@ -75,18 +83,48 @@ object TeleopAutoAlign { }) } - fun processorAlign(): Command = GoToPose({ - AllianceFlipUtil.apply(FieldConstants.processorPosition) - }, { false }) + fun l1ReefAlign(): Command { + return GoToPose({ + val position = OperatorControls.reefPosition - fun feederAlign(): Command = GoToPose({ - val leftPos = AllianceFlipUtil.apply(FieldConstants.leftFeederPosition) - val rightPos = AllianceFlipUtil.apply(FieldConstants.rightFeederPosition) + if (OperatorControls.noWalk) { + AllianceFlipUtil.apply(AutoScoringConstants.getL1PoseAtOffset(position.ordinal, 0.0)) + } else { + val goalPose = + AllianceFlipUtil.apply(AutoScoringConstants.getL1PoseAtOffset(position.ordinal, 0.0)) + // Make the position get closer as it gets closer to the final goal +// val adjustX: Double = 0.3 + var adjustX: Double = min( + 1.0, + RobotContainer.drivetrain.getSwervePose().translation.getDistance(goalPose.translation) * 0.5 + ) +// val adjustX = 1.0 + + // Once we're close enough we can just jump to the goal + if (adjustX < 0.2) { + adjustX = 0.0 + } - if (RobotContainer.drivetrain.getSwervePose().translation.getDistance(leftPos.translation) < RobotContainer.drivetrain.getSwervePose().translation.getDistance(rightPos.translation)) { - leftPos - } else { - rightPos + AllianceFlipUtil.apply(AutoScoringConstants.getL1PoseAtOffset(position.ordinal, adjustX)) + } + }) } - }, { false }) -} \ No newline at end of file + + fun processorAlign(): Command = GoToPose({ + AllianceFlipUtil.apply(FieldConstants.processorPosition) + }, { false }) + + fun feederAlign(): Command = GoToPose({ + val leftPos = AllianceFlipUtil.apply(FieldConstants.leftFeederPosition) + val rightPos = AllianceFlipUtil.apply(FieldConstants.rightFeederPosition) + + if (RobotContainer.drivetrain.getSwervePose().translation.getDistance(leftPos.translation) < RobotContainer.drivetrain.getSwervePose().translation.getDistance( + rightPos.translation + ) + ) { + leftPos + } else { + rightPos + } + }, { false }) + } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/auto/AutoBuilder.kt b/src/main/java/frc/robot/commands/auto/AutoBuilder.kt index 0b29a7a..e621952 100644 --- a/src/main/java/frc/robot/commands/auto/AutoBuilder.kt +++ b/src/main/java/frc/robot/commands/auto/AutoBuilder.kt @@ -61,7 +61,7 @@ object AutoBuilder { Commands.runOnce({ Superstructure.scoreL4() }), AutoCommands.coralReefAlign(targets[i].position, targets[i].side) ), - WaitCommand(0.35), + WaitCommand(0.7), Commands.runOnce({ RobotState.actionConfirmed = true println("Score $i: ${autoTimer.get()}") @@ -132,6 +132,15 @@ object AutoBuilder { Commands.runOnce({ Superstructure.removeAlgaeLow() }), AutoCommands.algaeReefAlign(AutoScoringConstants.ReefPositions.D) ), - AutoCommands.safeReefExit(AutoScoringConstants.ReefPositions.D) + ParallelCommandGroup( + AutoCommands.safeReefExit(AutoScoringConstants.ReefPositions.D), + Commands.runOnce({ Superstructure.algaeSpit() }), + ), + ParallelDeadlineGroup( + Commands.waitUntil { RobotState.gamePieceState == GamePieceState.Algae }, + Commands.runOnce({ Superstructure.removeAlgaeHigh() }), + AutoCommands.algaeReefAlign(AutoScoringConstants.ReefPositions.C) + ), + AutoCommands.safeReefExit(AutoScoringConstants.ReefPositions.C) ) } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/auto/AutoCommands.kt b/src/main/java/frc/robot/commands/auto/AutoCommands.kt index 25311cf..87f134d 100644 --- a/src/main/java/frc/robot/commands/auto/AutoCommands.kt +++ b/src/main/java/frc/robot/commands/auto/AutoCommands.kt @@ -36,7 +36,7 @@ object AutoCommands { // val adjustX = 1.0 // Once we're close enough we can just jump to the goal - if (adjustX < 0.25) { + if (adjustX < 0.2) { adjustX = 0.0 } @@ -93,7 +93,7 @@ object AutoCommands { // val adjustX = 1.0 // Once we're close enough we can just jump to the goal - if (adjustX < 0.3) { + if (adjustX < 0.2) { adjustX = 0.0 } diff --git a/src/main/java/frc/robot/constants/AutoScoringConstants.kt b/src/main/java/frc/robot/constants/AutoScoringConstants.kt index 8a37837..2c14d57 100644 --- a/src/main/java/frc/robot/constants/AutoScoringConstants.kt +++ b/src/main/java/frc/robot/constants/AutoScoringConstants.kt @@ -62,6 +62,27 @@ object AutoScoringConstants { ) } + fun getL1PoseAtOffset(face: Int, x: Double): Pose2d { + val poseDirection = + Pose2d(FieldConstants.Reef.center, Rotation2d.fromDegrees((180 - (60 * face)).toDouble())) + val adjustX: Double = reefFaceOffset + x - 0.425// + Units.inchesToMeters(3.0) + val adjustY: Double = 1.075 + + return Pose2d( + Translation2d( + poseDirection + .transformBy(Transform2d(adjustX, adjustY, Rotation2d())) + .x, + poseDirection + .transformBy(Transform2d(adjustX, adjustY, Rotation2d())) + .y + ), + Rotation2d( + poseDirection.rotation.radians + ).rotateBy(Rotation2d.fromDegrees(76.0)) + ) + } + enum class ReefPositions(var left: Pose2d, var right: Pose2d, var center: Pose2d) { A(Pose2d(), Pose2d(), Pose2d()), B(Pose2d(), Pose2d(), Pose2d()), diff --git a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.kt b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.kt index ea7cf5c..e3d8459 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.kt @@ -27,6 +27,7 @@ import frc.robot.subsystems.superstructure.pivot.implementation.PivotIOEmpty import frc.robot.subsystems.superstructure.pivot.implementation.PivotIOPID import frc.robot.subsystems.superstructure.pivot.implementation.PivotIOSim import frc.robot.util.AllianceFlipUtil +import frc.robot.util.input.OperatorControls import java.util.* object Superstructure : SubsystemBase() { @@ -114,7 +115,7 @@ object Superstructure : SubsystemBase() { allRequestsComplete = true } else { request(queuedRequests.removeAt(0)) - } + } // TODO: This doesn't handle deadlines (I don't think it should) } else if (activeRequest.get().isFinished()) { activeRequest = Optional.empty() } @@ -178,7 +179,13 @@ object Superstructure : SubsystemBase() { ), EmptyRequest(), ParallelRequest( - stowRequest(), + IfRequest( + {RobotState.gamePieceState == GamePieceState.Coral && OperatorControls.highStowPosition}, + ParallelRequest( + elevatorSystem.stowPosition(), pivotSystem.highStowAngle() + ), + stowRequest() + ), IfRequest( { RobotState.gamePieceState == GamePieceState.Coral }, intakeSystem.coralHolding(), @@ -201,6 +208,14 @@ object Superstructure : SubsystemBase() { ) ) + fun scoreL1() = requestSuperstructureAction( + SuperstructureAction.create( + ParallelRequest(pivotSystem.l1Angle(), elevatorSystem.l1Position()), + intakeSystem.coralScore(), + stowRequest() + ) + ) + fun scoreL2() = requestSuperstructureAction( SuperstructureAction.create( @@ -214,7 +229,8 @@ object Superstructure : SubsystemBase() { fun scoreL3() = requestSuperstructureAction( SuperstructureAction.create( - ParallelRequest(pivotSystem.l3Angle(), elevatorSystem.l3Position(), intakeSystem.coralHalfSpit()), + ParallelRequest(pivotSystem.l3Angle(), elevatorSystem.l3Position(), intakeSystem.coralHalfSpit().withPrerequisite( + elevatorSystem.closeL3())), intakeSystem.coralScore(), stowRequest(), safeRetract = true @@ -225,11 +241,11 @@ object Superstructure : SubsystemBase() { ParallelRequest( pivotSystem.l4Angle(), elevatorSystem.safeUpPosition(), - intakeSystem.coralHalfSpit() ), elevatorSystem .l4Position() .withPrerequisite(pivotSystem.safeTravelUp()), + intakeSystem.coralHalfSpit().withPrerequisite(elevatorSystem.closeL4()) ) fun scoreL4() = @@ -328,14 +344,24 @@ object Superstructure : SubsystemBase() { ) ) + fun algaeSpit() = requestSuperstructureAction( + ParallelRequest( + intakeSystem.algaeScore(), + WaitRequest(0.75), + intakeSystem.idle() + ) + ) + fun climb() = requestSuperstructureAction( SuperstructureAction.create( ParallelRequest( - pivotSystem.oldClimbAngle(), - climbSystem.deploy(), - funnelSystem.deploy() + climbSystem.unspoolForTime(), +// pivotSystem.climbAngle(), + //TODO: a timed climb system unspool +// climbSystem.deploy(), + funnelSystem.deploy() ), - climbSystem.climb(), + SequentialRequest(pivotSystem.oldClimbAngleWithPID(), WaitRequest(1.5), climbSystem.climb()), EmptyRequest()//IfRequest({RobotState.actionCancelled}, SequentialRequest(ParallelRequest(funnelSystem.stow(), climbSystem.stow())), pivotSystem.stowAngle()) ) ) @@ -352,6 +378,10 @@ object Superstructure : SubsystemBase() { ) ) + fun disableClimb() = requestSuperstructureAction( + Superstructure.climbSystem.setDisable(RobotContainer.startWithADisabledClimb) + ) + fun climbStowThenStow() = requestSuperstructureAction( SequentialRequest( ParallelRequest(climbSystem.stow(), pivotSystem.climbStowAngle()), diff --git a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureAction.kt b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureAction.kt index 4f57bcf..ab337c8 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureAction.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureAction.kt @@ -17,7 +17,7 @@ object SuperstructureAction { RobotState.actionConfirmed = false RobotState.superstructureActionRunning = true }, - prepAction, + prepAction.withDeadline { confirmed() || cancelled() }, AwaitRequest { confirmed() || cancelled() }, diff --git a/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbConstants.kt b/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbConstants.kt index fa6e8f1..772bef5 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbConstants.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbConstants.kt @@ -2,8 +2,8 @@ package frc.robot.subsystems.superstructure.climb object ClimbConstants { val stowAngle = 223.0 - val deployAngle = 80.0 + val deployAngle = 70.0 // val climbAngle = 145.0 - var climbAngle = 125.0 + var climbAngle = 141.0 } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbIO.kt b/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbIO.kt index 2244f64..8422584 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbIO.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbIO.kt @@ -3,6 +3,7 @@ package frc.robot.subsystems.superstructure.climb interface ClimbIO { fun getAngle(): Double fun angleDegrees(angleDegrees: Double) + fun setMotor(current: Double) fun periodic() var climbing: Boolean diff --git a/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbSystem.kt b/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbSystem.kt index a91a869..5e7c2cb 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbSystem.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/climb/ClimbSystem.kt @@ -3,19 +3,31 @@ package frc.robot.subsystems.superstructure.climb import cshcyberhawks.lib.math.MiscCalculations import cshcyberhawks.lib.requests.Request import cshcyberhawks.lib.requests.SequentialRequest +import cshcyberhawks.lib.requests.WaitRequest import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard import edu.wpi.first.wpilibj2.command.SubsystemBase class ClimbSystem(private val io: ClimbIO): SubsystemBase() { private fun setAngle(angleDegrees: Double) = Request.withAction { io.angleDegrees(angleDegrees) } private fun setClimbing(climbing: Boolean) = Request.withAction { io.climbing = climbing } - private fun setDisable(disable: Boolean) = Request.withAction {io.disable = disable} + fun setDisable(disable: Boolean) = Request.withAction { + println("set disable to: " + disable) + io.disable = disable + } + fun setMotor(current: Double) = Request.withAction { io.setMotor(current) } fun deploy() = SequentialRequest(setClimbing(false), setDisable(false), setAngle(ClimbConstants.deployAngle)) fun climb() = SequentialRequest(setClimbing(true), setDisable(false), setAngle(ClimbConstants.climbAngle)) fun stow() = SequentialRequest(setClimbing(false), setDisable(false), setAngle(ClimbConstants.stowAngle)) + fun unspoolForTime() = SequentialRequest( + setDisable(true), + setMotor(-40.0), + WaitRequest(8.0), + setMotor(0.0) + ) + fun isStow(): Boolean { return MiscCalculations.calculateDeadzone(io.getAngle() - ClimbConstants.stowAngle, 5.0) == 0.0 } diff --git a/src/main/java/frc/robot/subsystems/superstructure/climb/implementation/ClimbIOEmpty.kt b/src/main/java/frc/robot/subsystems/superstructure/climb/implementation/ClimbIOEmpty.kt index aa758e0..32ec52f 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/climb/implementation/ClimbIOEmpty.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/climb/implementation/ClimbIOEmpty.kt @@ -10,6 +10,10 @@ class ClimbIOEmpty(): ClimbIO { return 0.0 } + override fun setMotor(current: Double) { + + } + override fun angleDegrees(angleDegrees: Double) { } diff --git a/src/main/java/frc/robot/subsystems/superstructure/climb/implementation/ClimbIOReal.kt b/src/main/java/frc/robot/subsystems/superstructure/climb/implementation/ClimbIOReal.kt index 962ce30..6011af3 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/climb/implementation/ClimbIOReal.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/climb/implementation/ClimbIOReal.kt @@ -26,12 +26,16 @@ class ClimbIOReal() : ClimbIO { override var climbing = false override var disable = false + set(value) { + motor.setControl(torqueRequest.withOutput(0.0)) + field = value + } private var neutralCoast = false private val pidController = ProfiledPIDController( - 0.2315, + 1.2315, 0.0, 0.0315, TrapezoidProfile.Constraints( @@ -88,22 +92,28 @@ class ClimbIOReal() : ClimbIO { pidController.goal = TrapezoidProfile.State(angleDegrees, 0.0) } + override fun setMotor(current: Double) { + motor.setControl(torqueRequest.withOutput(current)) + } + override fun periodic() { + if (disable) { +// println("climb io disabled") +// motor.setControl(torqueRequest.withOutput(0.0)) + return + } val positionPIDOut = pidController.calculate(getAngle()) // val gravityFF = 3.15 * sin(Math.toRadians(getAngle())) - val motorOut = if (climbing && (ClimbConstants.climbAngle - getAngle()) > 10.0) {positionPIDOut + 30.0} else if (climbing) {positionPIDOut + 15.0} else positionPIDOut + val motorOut = if (climbing && (ClimbConstants.climbAngle - getAngle()) > 10.0) {positionPIDOut + 30.0} else if (climbing) {positionPIDOut + 8.5} else positionPIDOut - if (!disable) { // println(motorOut) motor.setControl(torqueRequest.withOutput(motorOut)) - } - val sdCoast = SmartDashboard.getBoolean("Climb coast", false) // println(sdCoast) diff --git a/src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorConstants.kt b/src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorConstants.kt index d6291cd..85c4436 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorConstants.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorConstants.kt @@ -4,14 +4,17 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile import edu.wpi.first.math.util.Units object ElevatorConstants { - const val velocityInches = 40.0 // Guessed values for now - const val accelationInches = 30.0 + const val velocityInches = 70.0 // Guessed values for now + const val accelationInches = 75.0 // Conversion so one (I guess its mechanism rotation) is 1 inch on the elevator const val conversionFactor = - 20.0 * 1.751 * Math.PI / 30.0 +// 20.0 * 1.751 * Math.PI / 30.0 + 9.0 * 1.751 * Math.PI / 30.0 - const val positionTolerance = 0.1 + + // Large tolerance, not used for high accuracy things + const val positionTolerance = 0.5 // The torque to hold the elevator against gravity (Newton-Meters) val kGNM = Units.inchesToMeters(1.751 / 2.0) * (25.6 * 4.4482216153) diff --git a/src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorSystem.kt b/src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorSystem.kt index 38e937d..b20e228 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorSystem.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/elevator/ElevatorSystem.kt @@ -12,7 +12,7 @@ import frc.robot.subsystems.superstructure.elevator.ElevatorIO // By making a subsystem a Kotlin object, we ensure there is only ever one instance of it. // It also reduces the need to have reference variables for the subsystems to be passed around. class ElevatorSystem(private val io: ElevatorIO) : SubsystemBase() { - private val safeUpPosition = 9.0 + private val safeUpPosition = 7.0 private val safeDownPosition = 4.0 fun getPosition(): Double = io.getPosition() @@ -23,10 +23,13 @@ class ElevatorSystem(private val io: ElevatorIO) : SubsystemBase() { fun atDesiredPosition() = io.atDesiredPosition() fun awaitDesiredPosition() = AwaitRequest { atDesiredPosition() } + fun prereqAtDesiredPosition() = Prerequisite.withCondition { io.atDesiredPosition() } fun belowSafeUpPosition() = Prerequisite.withCondition { getPosition() < safeUpPosition } fun safeIntakePosition() = Prerequisite.withCondition { getPosition() < 7.0 } fun safeToFlipPivot() = Prerequisite.withCondition { getPosition() > 4.5 } + fun closeL3() = Prerequisite.withCondition { getPosition() > 3.5 } + fun closeL4() = Prerequisite.withCondition { getPosition() > 29.5 } private fun setPosition(positionInches: Double) = Request.withAction { io.setPosition(positionInches) @@ -46,6 +49,7 @@ class ElevatorSystem(private val io: ElevatorIO) : SubsystemBase() { fun safeUpPosition() = setPosition(safeUpPosition) fun safeDownPosition() = setPosition(safeDownPosition) + fun l1Position() = setPosition(0.0) fun l2Position() = setPosition(0.0) fun l3Position() = setPosition(4.0) fun l4Position() = setPosition(29.95) // Should be 30 eventually but not safe right now diff --git a/src/main/java/frc/robot/subsystems/superstructure/elevator/implementation/ElevatorIOPID.kt b/src/main/java/frc/robot/subsystems/superstructure/elevator/implementation/ElevatorIOPID.kt index 7558cca..5041d51 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/elevator/implementation/ElevatorIOPID.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/elevator/implementation/ElevatorIOPID.kt @@ -19,9 +19,9 @@ class ElevatorIOPID() : ElevatorIO { val elevatorPIDController = ProfiledPIDController( - 11.5, - 0.0, - 0.6, + 6.5, + 0.003, + 0.616, TrapezoidProfile.Constraints( ElevatorConstants.velocityInches, ElevatorConstants.accelationInches @@ -113,7 +113,9 @@ class ElevatorIOPID() : ElevatorIO { override fun periodic() { SmartDashboard.putNumber("Elevator Desired Position", desiredPosition) - val gravityFF = 3.85 +// val gravityFF = 3.85 + val gravityFF = 7.15 + val positionPIDOut = elevatorPIDController.calculate(getPosition()) SmartDashboard.putNumber("Elevator Position Error", elevatorPIDController.positionError) diff --git a/src/main/java/frc/robot/subsystems/superstructure/funnel/FunnelConstants.kt b/src/main/java/frc/robot/subsystems/superstructure/funnel/FunnelConstants.kt index c613f2c..109b76b 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/funnel/FunnelConstants.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/funnel/FunnelConstants.kt @@ -5,7 +5,7 @@ object FunnelConstants { const val accelerationDegrees = 250.0 // const val stowAngle = 215.0 - const val stowAngle = 212.0 + const val stowAngle = 218.0 // const val deployAngle = 50.5 const val deployAngle = 65.5 diff --git a/src/main/java/frc/robot/subsystems/superstructure/funnel/implementation/FunnelIOReal.kt b/src/main/java/frc/robot/subsystems/superstructure/funnel/implementation/FunnelIOReal.kt index eae5ab1..e35af6f 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/funnel/implementation/FunnelIOReal.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/funnel/implementation/FunnelIOReal.kt @@ -23,7 +23,7 @@ class FunnelIOReal(): FunnelIO { private val encoder = CANcoder(CANConstants.Funnel.encoderId) private val pidController = ProfiledPIDController( - 0.145, + 0.265, 0.0, 0.0315, TrapezoidProfile.Constraints( diff --git a/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeConstants.kt b/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeConstants.kt index 7cc26ec..b1d6c59 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeConstants.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/intake/IntakeConstants.kt @@ -2,7 +2,7 @@ package frc.robot.subsystems.superstructure.intake object IntakeConstants { const val coralScoreTimeoutSeconds = 0.5 - const val coralHalfSpitTimeSeconds = 0.2 + const val coralHalfSpitTimeSeconds = 0.15 const val algaeScoreTimeoutSeconds = 0.5 const val intakeCurrentThreshold = 10.0 // 10 Amps before system knows it has a game piece diff --git a/src/main/java/frc/robot/subsystems/superstructure/intake/implementation/IntakeIOReal.kt b/src/main/java/frc/robot/subsystems/superstructure/intake/implementation/IntakeIOReal.kt index 7d9dc6d..23af2e5 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/intake/implementation/IntakeIOReal.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/intake/implementation/IntakeIOReal.kt @@ -26,7 +26,6 @@ class IntakeIOReal() : IntakeIO { init { val coralIntakeMotorConfiguration = TalonFXConfiguration() // coralIntakeMotorConfiguration.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive - println("WHY") // coralIntakeMotorConfiguration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive val currentConfigs = coralIntakeMotorConfiguration.CurrentLimits @@ -35,13 +34,17 @@ class IntakeIOReal() : IntakeIO { // intakeMotor.configurator.apply(coralIntakeMotorConfiguration) - coralLaserCAN.setRangingMode(LaserCanInterface.RangingMode.SHORT) - //coralLaserCAN.setRegionOfInterest(RegionOfInterest(8, 8, 16, 16)) - coralLaserCAN.setTimingBudget(LaserCanInterface.TimingBudget.TIMING_BUDGET_33MS) + try { + coralLaserCAN.setRangingMode(LaserCanInterface.RangingMode.SHORT) + //coralLaserCAN.setRegionOfInterest(RegionOfInterest(8, 8, 16, 16)) + coralLaserCAN.setTimingBudget(LaserCanInterface.TimingBudget.TIMING_BUDGET_33MS) - algaeLaserCAN.setRangingMode(LaserCanInterface.RangingMode.SHORT) - //algaeLaserCAN.setRegionOfInterest(RegionOfInterest(8, 8, 16, 16)) - algaeLaserCAN.setTimingBudget(LaserCanInterface.TimingBudget.TIMING_BUDGET_33MS) + algaeLaserCAN.setRangingMode(LaserCanInterface.RangingMode.SHORT) + //algaeLaserCAN.setRegionOfInterest(RegionOfInterest(8, 8, 16, 16)) + algaeLaserCAN.setTimingBudget(LaserCanInterface.TimingBudget.TIMING_BUDGET_33MS) + } catch (exception: Exception) { + println("Laser CAN Configuration Exception: $exception") + } } override fun setIntakeState(state: IntakeState) { @@ -71,7 +74,7 @@ class IntakeIOReal() : IntakeIO { val measurement: LaserCanInterface.Measurement = coralLaserCAN.measurement SmartDashboard.putNumber("Coral Measurement", measurement.distance_mm.toDouble()) @Suppress("SENSELESS_COMPARISON") - return (measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT && measurement.distance_mm < 40.0) + return (measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT && measurement.distance_mm < 32.0) } diff --git a/src/main/java/frc/robot/subsystems/superstructure/pivot/PivotConstants.kt b/src/main/java/frc/robot/subsystems/superstructure/pivot/PivotConstants.kt index fe7a87d..2fb2d83 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/pivot/PivotConstants.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/pivot/PivotConstants.kt @@ -3,9 +3,13 @@ package frc.robot.subsystems.superstructure.pivot import edu.wpi.first.math.trajectory.TrapezoidProfile object PivotConstants { - const val velocityDegrees = 180.0 // Guessed values for now +// const val velocityDegrees = 225.0 // Guessed values for now +const val velocityDegrees = 180.0 // Guessed values for now + +// const val accelerationDegrees = 375.0 const val accelerationDegrees = 250.0 + // Conversion so one (I guess its mechanism rotation) is 1 rotation of the pivot const val conversionFactor = 100.0 * (32.0 / 24.0) / 360.0 diff --git a/src/main/java/frc/robot/subsystems/superstructure/pivot/PivotSystem.kt b/src/main/java/frc/robot/subsystems/superstructure/pivot/PivotSystem.kt index e01c2a9..025f54f 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/pivot/PivotSystem.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/pivot/PivotSystem.kt @@ -35,6 +35,8 @@ class PivotSystem(private val io: PivotIO) : SubsystemBase() { } } + fun prereqAtDesiredAngle() = Prerequisite.withCondition { atDesiredAngle() } + fun safeTravelUp() = Prerequisite.withCondition { getAngle() < 245.0 } fun safeTravelDown() = Prerequisite.withCondition { getAngle() > 180.0 } fun safeToClimbDisable() = Prerequisite.withCondition { getAngle() < 20.0 } @@ -60,6 +62,9 @@ class PivotSystem(private val io: PivotIO) : SubsystemBase() { fun climbStowAngle() = setAngle(325.0) + fun highStowAngle() = setAngle(130.0) + + fun l1Angle() = setAngle(300.0) fun l2Angle() = setAngle(150.0) // fun l3Angle() = setAngle(SmartDashboard.getNumber("L3 Angle", 125.0)) fun l3Angle() = setAngle(120.0) diff --git a/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOPID.kt b/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOPID.kt index 0fde92e..fa2ef51 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOPID.kt +++ b/src/main/java/frc/robot/subsystems/superstructure/pivot/implementation/PivotIOPID.kt @@ -61,8 +61,7 @@ class PivotIOPID() : PivotIO { private var desiredAngle = 290.0 //138.6 = 180 - - private val tbOffset = -43.2 + private val tbOffset = -68 // private val tbOffset = -23.2//-43.2 // private fun getTBDegrees() = @@ -145,6 +144,7 @@ class PivotIOPID() : PivotIO { override fun periodic() { SmartDashboard.putNumber("Pivot Raw TB Angle", MiscCalculations.wrapAroundAngles(encoder.absolutePosition.valueAsDouble * 360.0)) SmartDashboard.putNumber("Pivot Desired Angle", desiredAngle) + SmartDashboard.putNumber("Pivot Velocity", encoder.velocity.valueAsDouble * 360.0) val sdCoast = SmartDashboard.getBoolean("Pivot coast", false) if (sdCoast != neutralCoast) { @@ -154,14 +154,14 @@ class PivotIOPID() : PivotIO { ) } - if (pidDistabled) { + if (pidDistabled || (desiredAngle == 295.0 && MiscCalculations.calculateDeadzone(desiredAngle, 4.0) == 0.0)) { motor.set(0.0) return } // val kG = if (getAngle() > 270) 8.25 else 10.5 - val kG = if (getAngle() > 270) 7.25 else 7.5 - val gravityFF = kG * sin(Math.toRadians(getAngle() + 90)) + val kG = if (getAngle() > 270 && getAngle() < 305.0) 0.0 else 7.5 + val gravityFF = kG * -sin(Math.toRadians(getAngle() - 110.0)) val positionPIDOut = pivotPIDController.calculate(getAngle()) // val correctivePIDOut = correctivePID.calculate(getAngle(), desiredAngle) diff --git a/src/main/java/frc/robot/util/Vision.kt b/src/main/java/frc/robot/util/Vision.kt index bc591de..d2a980b 100644 --- a/src/main/java/frc/robot/util/Vision.kt +++ b/src/main/java/frc/robot/util/Vision.kt @@ -55,6 +55,7 @@ class VisionSystem { if (!bottomM2Reset) { if (llName == "limelight-tright" || llName == "limelight-tleft") { +// if (llName == "limelight-tright") { continue; } } @@ -117,7 +118,9 @@ class VisionSystem { degStds = 300.0 } - if (llName == "limelight-tright" || llName == "limelight-tleft") { + if (llName == "limelight-tright") { + +// if (llName == "limelight-tright" || llName == "limelight-tleft") { xyStds *= 4.0 degStds *= 4.0 } @@ -189,7 +192,7 @@ class VisionSystem { xyStds = Math.sqrt((1.0/(2.0 * (llMeasure.avgTagArea + .25)))) - .15; if (llMeasure.tagCount >= 2) { - xyStds *= .4; + xyStds *= .6; } // if (llMeasure.tagCount >= 2) { @@ -214,9 +217,23 @@ class VisionSystem { // } - if (llName == "limelight-tright" || llName == "limelight-tleft") { + if (llName == "limelight-tright") { xyStds = 14.0 - degStds = 300.0 + degStds = 1800.0 + } + else if (llName == "limelight-tleft") { + if (llMeasure.avgTagDist < 1.75) { + xyStds = 0.35 + degStds = 1800.0; + } + else { + xyStds *= 2.5 + } + } + else { + if (llMeasure.avgTagDist > 2.45) { + xyStds *= 4.5 + } } @@ -226,6 +243,10 @@ class VisionSystem { VecBuilder.fill(xyStds, xyStds, Math.toRadians(degStds)) ) +// println("avg tag dist: " + llMeasure.avgTagDist) + + SmartDashboard.putNumber("LL ${llName} xy stds: ", xyStds) + // println("updating odometry with ll") // println("Updating with LL ${llName}: X = " + llMeasure.pose.x + " Y = " + llMeasure.pose.y) diff --git a/src/main/java/frc/robot/util/input/ManualDriverInput.kt b/src/main/java/frc/robot/util/input/ManualDriverInput.kt index 70f5d2c..8f73728 100644 --- a/src/main/java/frc/robot/util/input/ManualDriverInput.kt +++ b/src/main/java/frc/robot/util/input/ManualDriverInput.kt @@ -14,7 +14,8 @@ object ManualDriverInput { RobotContainer.rightJoystick.button(2) .onTrue(Commands.runOnce({ RobotContainer.drivetrain.seedFieldCentric(); println("seeding field relative") })) - RobotContainer.rightJoystick.button(cancel).onTrue(Commands.runOnce({ + //NOTE: rebound + RobotContainer.rightJoystick.button(10).onTrue(Commands.runOnce({ if (!RobotContainer.currentDriveCommand.isEmpty) { println("cancelling auto drive") RobotContainer.currentDriveCommand.get().cancel() @@ -23,6 +24,17 @@ object ManualDriverInput { } })) + RobotContainer.rightJoystick.button(4).onTrue(Commands.run({ + OperatorControls.noWalk = true + alignCommand.cancel() + alignCommand = OperatorControls.action.alignCommand + alignCommand.schedule() + })) + + RobotContainer.rightJoystick.button(4).onFalse(Commands.runOnce({ + alignCommand.cancel() + })) + RobotContainer.rightJoystick.button(3).onTrue(Commands.runOnce({ OperatorControls.noWalk = false alignCommand.cancel() diff --git a/src/main/java/frc/robot/util/input/OperatorControls.kt b/src/main/java/frc/robot/util/input/OperatorControls.kt index 833fd97..f4799d0 100644 --- a/src/main/java/frc/robot/util/input/OperatorControls.kt +++ b/src/main/java/frc/robot/util/input/OperatorControls.kt @@ -10,6 +10,7 @@ import frc.robot.subsystems.superstructure.Superstructure enum class DriverAction(val superStructureCommand: Command, val alignCommand: Command = Commands.runOnce({})) { // Stow(Commands.runOnce({ Superstructure.stow() })), IntakeFeeder(Commands.runOnce({ Superstructure.intakeFeeder() }), TeleopAutoAlign.feederAlign()), + ScoreL1(Commands.runOnce({ Superstructure.scoreL1() }), TeleopAutoAlign.l1ReefAlign()), ScoreL2(Commands.runOnce({ Superstructure.scoreL2() }), TeleopAutoAlign.coralReefAlign()), ScoreL3(Commands.runOnce({ Superstructure.scoreL3() }), TeleopAutoAlign.coralReefAlign()), ScoreL4(Commands.runOnce({ Superstructure.scoreL4() }), TeleopAutoAlign.coralReefAlign()), @@ -32,6 +33,7 @@ object OperatorControls { val action get() = when (SmartDashboard.getString("ConsoleDriverAction", "")) { "Feeder" -> DriverAction.IntakeFeeder + "L1" -> DriverAction.ScoreL1 "L2" -> DriverAction.ScoreL2 "L3" -> DriverAction.ScoreL3 "L4" -> DriverAction.ScoreL4 @@ -60,6 +62,9 @@ object OperatorControls { else -> AutoScoringConstants.ReefPositions.A } + val highStowPosition + get() = SmartDashboard.getBoolean("ConsoleHighStowPosition", false) + // val actionChooser = SendableChooser() // val coralSideChooser = SendableChooser()