diff --git a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt index ddec2ecb..f696341c 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt @@ -142,7 +142,7 @@ object AutonomousSelector { } val waitTime: Time - get() = 0.0.seconds + get() = waitBeforeCommandSlider.getDouble(0.0).seconds val secondaryWaitTime: Time get() = secondaryWaitInAuto.getDouble(0.0).seconds diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineSourceAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineSourceAutoPath.kt index a21987b0..8d7c54a5 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineSourceAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineSourceAutoPath.kt @@ -67,7 +67,7 @@ class ThreeNoteAndPickupCenterlineSourceAutoPath( ) } ), - superstructure.prepManualSpeakerCommand(-2.5.degrees, 4400.rotations.perMinute), + superstructure.prepManualSpeakerCommand(-4.5.degrees, 4000.rotations.perMinute), superstructure.scoreCommand(), ParallelCommandGroup( DrivePathCommand.createPathInFieldFrame( @@ -113,7 +113,7 @@ class ThreeNoteAndPickupCenterlineSourceAutoPath( WaitCommand(1.0) .andThen(superstructure.groundIntakeCommand()) ), - superstructure.prepManualSpeakerCommand(-2.5.degrees, 44000.rotations.perMinute), + superstructure.prepManualSpeakerCommand(-4.5.degrees, 4000.rotations.perMinute), superstructure.scoreCommand(), ParallelCommandGroup( DrivePathCommand.createPathInFieldFrame( diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath.kt index 0410ccad..eabcfc2f 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath.kt @@ -40,11 +40,11 @@ class ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath( null, 180.degrees.inRotation2ds ), - FieldWaypoint( - Translation2d(8.32.meters, 0.78.meters).translation2d, - null, - 180.degrees.inRotation2ds - ) + FieldWaypoint( + Translation2d(8.32.meters, 2.44.meters).translation2d, + null, + 220.degrees.inRotation2ds + ) ) } ), @@ -56,9 +56,9 @@ class ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath( { listOf( FieldWaypoint( - Translation2d(8.32.meters, 0.78.meters).translation2d, + Translation2d(8.32.meters, 2.44.meters).translation2d, null, - 180.degrees.inRotation2ds + 220.degrees.inRotation2ds ), FieldWaypoint( Translation2d(6.89.meters, 1.73.meters).translation2d, @@ -73,68 +73,7 @@ class ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath( ) } ), - superstructure.prepManualSpeakerCommand(-2.2.degrees, 4000.rotations.perMinute) - ) - .andThen(superstructure.scoreCommand()) - .andThen( - ParallelCommandGroup( - DrivePathCommand.createPathInFieldFrame( - drivetrain, - { - listOf( - FieldWaypoint( - Translation2d(4.33.meters, 1.67.meters).translation2d, - null, - (180 - 43.3758).degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d( - (4.33.meters + 8.32.meters) / 2, - (2.44 + 0.78).meters / 2 - 0.1.meters - ) - .translation2d, - null, - ((180 - 43.3758 + 220) / 2 + 10).degrees.inRotation2ds, - ), - FieldWaypoint( - Translation2d(8.32.meters, 2.44.meters).translation2d, - null, - 220.degrees.inRotation2ds - ) - ) - } - ), - WaitCommand(1.0).andThen(superstructure.groundIntakeCommand()), - ) - ), - ParallelCommandGroup( - DrivePathCommand.createPathInFieldFrame( - drivetrain, - { - listOf( - FieldWaypoint( - Translation2d(8.32.meters, 2.44.meters).translation2d, - null, - 220.degrees.inRotation2ds - ), - FieldWaypoint( - Translation2d( - (4.33.meters + 8.32.meters) / 2, - (2.44 + 0.78).meters / 2 + 0.1.meters - ) - .translation2d, - null, - ((180 - 43.3758 + 220) / 2 + 10).degrees.inRotation2ds, - ), - FieldWaypoint( - Translation2d(4.33.meters, 1.67.meters).translation2d, - null, - (180 - 43.3758).degrees.inRotation2ds - ) - ) - } - ), - superstructure.prepManualSpeakerCommand(-2.2.degrees, 4000.rotations.perMinute) + superstructure.prepManualSpeakerCommand(-4.5.degrees, 4000.rotations.perMinute) ) .andThen(superstructure.scoreCommand()) .andThen( @@ -164,9 +103,9 @@ class ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath( 180.degrees.inRotation2ds ), FieldWaypoint( - Translation2d(5.5.meters, 1.85.meters).translation2d, + Translation2d(4.33.meters, 1.67.meters).translation2d, null, - (180 - 35.02902884839783).degrees.inRotation2ds + (180 - 43.37583640633171).degrees.inRotation2ds ) ) } @@ -176,7 +115,7 @@ class ThreeNoteAndPickupCenterlineWithoutFirstNoteSourceAutoPath( .andThen(WaitCommand(2.0)) .andThen( superstructure.prepManualSpeakerCommand( - -2.0.degrees, 4400.rotations.perMinute + -4.5.degrees, 4000.rotations.perMinute ) ) ) diff --git a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt index 1627d261..8eff7609 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt @@ -6,6 +6,7 @@ import edu.wpi.first.wpilibj.GenericHID import edu.wpi.first.wpilibj2.command.button.Trigger import org.team4099.lib.joystick.XboxOneGamepad import java.util.function.Consumer +import kotlin.math.absoluteValue /** * Maps buttons on the driver and operator controllers to specific actions with meaningful variable @@ -29,7 +30,7 @@ object ControlBoard { val turn: Double get() { - return if (driver.rightXAxis < 0.85) { + return if (driver.rightXAxis.absoluteValue < 0.80) { driver.rightXAxis * DrivetrainConstants.TELEOP_TURNING_SPEED_PERCENT } else { driver.rightXAxis diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt index 21fab9c7..97638736 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -33,7 +33,7 @@ object DrivetrainConstants { const val FOC_ENABLED = true const val MINIMIZE_SKEW = false - const val TELEOP_TURNING_SPEED_PERCENT = 0.7 + const val TELEOP_TURNING_SPEED_PERCENT = 0.5 const val OMOMETRY_UPDATE_FREQUENCY = 250.0 diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt index 1e29ae4c..fe741d10 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt @@ -37,9 +37,9 @@ object WristConstants { val ABSOLUTE_ENCODER_OFFSET = ( 97.72227856659904.degrees - 35.degrees + 1.90.degrees - - 0.5.degrees - - 1.0.degrees - // add to drop angle - 1.degrees - + 0.55.degrees - + 0.6.degrees - // add to drop angle + 1.degrees - 0.5.degrees - 96.3.degrees ) * ABSOLUTE_ENCODER_TO_MECHANISM_GEAR_RATIO val WRIST_LENGTH = 18.6.inches diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/falconspin/Motor.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/falconspin/Motor.kt index a738e9ae..02cb16b3 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/falconspin/Motor.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/falconspin/Motor.kt @@ -23,6 +23,8 @@ interface MotorType typealias REVNeo = MotorType +typealias Kraken = MotorType + typealias Falcon = MotorType typealias SimMotor = MotorType