diff --git a/src/main/java/frc/robot/Constants.kt b/src/main/java/frc/robot/Constants.kt index 83748e4f..adf54126 100644 --- a/src/main/java/frc/robot/Constants.kt +++ b/src/main/java/frc/robot/Constants.kt @@ -99,10 +99,10 @@ object Constants { const val slope = -0.33 val distanceMap: HashMap = hashMapOf( - Pair(0.0 + 48.5, 81.5), - Pair(25.5 + 48.5, 69.1), - Pair(69.0 + 48.5, 61.7), - Pair(94.0 + 48.5, 57.0), + Pair(0.0 + 54.17, 81.5), + Pair(78.75, 72.75), + Pair(104.33, 65.78), + Pair(187.4, 60.5) ) } diff --git a/src/main/java/frc/robot/RobotContainer.kt b/src/main/java/frc/robot/RobotContainer.kt index 832f8951..62c52c6d 100644 --- a/src/main/java/frc/robot/RobotContainer.kt +++ b/src/main/java/frc/robot/RobotContainer.kt @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj2.command.Commands import edu.wpi.first.wpilibj2.command.InstantCommand import edu.wpi.first.wpilibj2.command.ParallelCommandGroup import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup import edu.wpi.first.wpilibj2.command.button.CommandXboxController import edu.wpi.first.wpilibj2.command.button.Trigger import frc.robot.commands.Autos @@ -83,10 +84,10 @@ object RobotContainer { { controller.leftTriggerAxis } ) - val trackSpeakerCommand = ParallelCommandGroup( - RotateTowardsTargetCommand(LimelightSubsystem.odometryLimelight), - QuickPivotCommand(0.0, false, true) - ) +// val trackSpeakerCommand = ParallelCommandGroup( +// RotateTowardsTargetCommand(LimelightSubsystem.odometryLimelight), +// QuickPivotCommand(0.0, false, true) +// ) val intakePivot: QuickPivotCommand = QuickPivotCommand(Constants.PivotConstants.INTAKE_POSITION, false, false) val subwooferPivot: QuickPivotCommand = QuickPivotCommand(Constants.PivotConstants.SUBWOOFER_POSITION, false, false) @@ -155,23 +156,29 @@ object RobotContainer { ) ) - controller.rightBumper().onTrue( - Commands.runOnce({ - if (teleopDrive.isScheduled) { - teleopDrive.cancel() - trackSpeakerCommand.schedule() - } else { - trackSpeakerCommand.cancel() - teleopDrive.schedule() - } - }) - ) +// controller.rightBumper().onTrue( +// Commands.runOnce({ +// if (teleopDrive.isScheduled) { +// teleopDrive.cancel() +// trackSpeakerCommand.schedule() +// } else { +// trackSpeakerCommand.cancel() +// teleopDrive.schedule() +// } +// }) +// ) controller.leftStick().onTrue(InstantCommand(SwerveSubsystem::zeroGyro)) controller.povUp().onTrue(ampPivot) controller.povRight().onTrue(intakePivot) controller.povDown().onTrue(subwooferPivot) - controller.povLeft().onTrue(autoAim) // TODO: Implement auto-aiming + controller.povLeft().onTrue( + autoAim + ) // TODO: Implement auto-aiming + controller.rightBumper().toggleOnTrue( + RotateTowardsTargetCommand(LimelightSubsystem.odometryLimelight) + ) + controller.y().onTrue(HomePivotCommand()) // TODO: Implement homing launcher controller.b().toggleOnTrue(manualPivot) controller.x().onTrue(manualClimb) diff --git a/src/main/java/frc/robot/commands/Autos.kt b/src/main/java/frc/robot/commands/Autos.kt index 51d8f76d..eaa28ab1 100644 --- a/src/main/java/frc/robot/commands/Autos.kt +++ b/src/main/java/frc/robot/commands/Autos.kt @@ -57,7 +57,7 @@ object Autos { )) NamedCommands.registerCommand("Pull Note", IntakeNoteCommand()) NamedCommands.registerCommand("Intake Note", ToggleIntakeCommand()) - NamedCommands.registerCommand("Set Down", SetKnownPosition(90.1)) + NamedCommands.registerCommand("Set Down", SetKnownPosition(91.28)) NamedCommands.registerCommand("Intake Sequence", ParallelDeadlineGroup( IntakeNoteCommand(), diff --git a/src/main/java/frc/robot/commands/launcher/LaunchCommand.kt b/src/main/java/frc/robot/commands/launcher/LaunchCommand.kt index b06c945f..c8e81995 100644 --- a/src/main/java/frc/robot/commands/launcher/LaunchCommand.kt +++ b/src/main/java/frc/robot/commands/launcher/LaunchCommand.kt @@ -33,6 +33,7 @@ class LaunchCommand( } override fun initialize() { + launcherSubsystem.setFlywheelBrake(false) timer.restart() println("Starting Launch") } @@ -42,7 +43,7 @@ class LaunchCommand( launcherSubsystem.setFlywheelSpeeds(speed.asDouble / 4) minVelocity = 1200.0 } else { - launcherSubsystem.setFlywheelVelocity(6000.0.rpm) + launcherSubsystem.setFlywheelVelocity(5800.0.rpm) minVelocity = 5500.0 } @@ -66,6 +67,7 @@ class LaunchCommand( launcherSubsystem.stopFlywheels() launcherSubsystem.rollerMotor.setIdleMode(CANSparkBase.IdleMode.kBrake) launcherSubsystem.stopRoller() + launcherSubsystem.setFlywheelBrake(true) println("Launch Ending $interrupted") } } diff --git a/src/main/java/frc/robot/commands/pivot/ManualPivotCommand.kt b/src/main/java/frc/robot/commands/pivot/ManualPivotCommand.kt index 9be48e84..7060eed0 100644 --- a/src/main/java/frc/robot/commands/pivot/ManualPivotCommand.kt +++ b/src/main/java/frc/robot/commands/pivot/ManualPivotCommand.kt @@ -27,6 +27,6 @@ class ManualPivotCommand(voltage: DoubleSupplier) : Command() { } override fun end(interrupted: Boolean) { - QuickPivotCommand(pivotSubsystem.getRelativePosition(), false, false).schedule() +// QuickPivotCommand(pivotSubsystem.getRelativePosition(), false, false).schedule() } } diff --git a/src/main/java/frc/robot/subsystems/LauncherSubsystem.kt b/src/main/java/frc/robot/subsystems/LauncherSubsystem.kt index 841829ad..5f84d47d 100644 --- a/src/main/java/frc/robot/subsystems/LauncherSubsystem.kt +++ b/src/main/java/frc/robot/subsystems/LauncherSubsystem.kt @@ -4,7 +4,6 @@ import edu.wpi.first.units.Measure import edu.wpi.first.units.Units import edu.wpi.first.units.Voltage import edu.wpi.first.wpilibj.DigitalInput -import edu.wpi.first.wpilibj.Timer import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog import edu.wpi.first.wpilibj2.command.Command @@ -16,10 +15,7 @@ import frc.robot.Constants import frc.robot.Robot import lib.math.units.RotationVelocity import lib.math.units.into -import lib.math.units.rpm -import lib.math.units.velocity -import lib.near -import lib.zoneTrigger +import kotlin.math.min object LauncherSubsystem : SubsystemBase() { // TODO: Get the actual IDs @@ -120,6 +116,16 @@ object LauncherSubsystem : SubsystemBase() { bottomFlywheels.encoder.setPosition(0.0) } + fun setFlywheelBrake(brake: Boolean){ + if(brake){ + topFlywheels.setIdleMode(CANSparkBase.IdleMode.kBrake) + bottomFlywheels.setIdleMode(CANSparkBase.IdleMode.kBrake) + } else { + topFlywheels.setIdleMode(CANSparkBase.IdleMode.kCoast) + bottomFlywheels.setIdleMode(CANSparkBase.IdleMode.kCoast) + } + } + fun setFlywheelSpeeds(rawSpeed: Double) { topFlywheels.set(rawSpeed) bottomFlywheels.set(rawSpeed) @@ -130,8 +136,12 @@ object LauncherSubsystem : SubsystemBase() { } fun setFlywheelVelocity(velocity: RotationVelocity){ - topFlywheels.setVoltage(topFlywheelFeedforward.calculate(velocity into Units.RotationsPerSecond)); - bottomFlywheels.setVoltage(bottomFlywheelFeedforward.calculate(velocity into Units.RotationsPerSecond)) + val currentV = min(topFlywheels.encoder.velocity, bottomFlywheels.encoder.velocity) + val error = velocity.into(Units.RPM) - currentV + val extraVoltage = error * 0.002 + + topFlywheels.setVoltage(topFlywheelFeedforward.calculate(velocity into Units.RotationsPerSecond) + extraVoltage); + bottomFlywheels.setVoltage(bottomFlywheelFeedforward.calculate(velocity into Units.RotationsPerSecond) + extraVoltage) } fun setFlywheelVoltage(voltage: Double){ diff --git a/src/main/java/frc/robot/subsystems/LimelightSubsystem.kt b/src/main/java/frc/robot/subsystems/LimelightSubsystem.kt index 00b2bbdb..024eb1e4 100644 --- a/src/main/java/frc/robot/subsystems/LimelightSubsystem.kt +++ b/src/main/java/frc/robot/subsystems/LimelightSubsystem.kt @@ -1,7 +1,9 @@ package frc.robot.subsystems +import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj2.command.SubsystemBase import lib.vision.Limelight +import java.sql.Driver /** * The subsystem that controls the limelight. @@ -13,4 +15,14 @@ object LimelightSubsystem : SubsystemBase() { init { odometryLimelight.setTargetTag(7) } + + override fun periodic() { + if(DriverStation.getAlliance().isPresent){ + if(DriverStation.getAlliance().get() == DriverStation.Alliance.Red){ + odometryLimelight.setTargetTag(4) + } else { + odometryLimelight.setTargetTag(7) + } + } + } }