From ed6b3a3195fee0cad6dea11ef63cc255c7e80d22 Mon Sep 17 00:00:00 2001 From: a1cd <71281043+a1cd@users.noreply.github.com> Date: Sun, 2 Apr 2023 11:31:21 -0400 Subject: [PATCH 1/8] I fixed snap to rotation! --- .../robot/commands/drivetrain/DriverCommand.kt | 15 ++++++++++++++- .../frc/robot/commands/drivetrain/Nearest180.kt | 9 ++++++--- .../frc/robot/commands/drivetrain/RotateTo180.kt | 3 ++- .../frc/robot/controls/ChrisControlScheme.kt | 2 +- 4 files changed, 23 insertions(+), 6 deletions(-) diff --git a/src/main/kotlin/frc/robot/commands/drivetrain/DriverCommand.kt b/src/main/kotlin/frc/robot/commands/drivetrain/DriverCommand.kt index e28c45c..1b77444 100644 --- a/src/main/kotlin/frc/robot/commands/drivetrain/DriverCommand.kt +++ b/src/main/kotlin/frc/robot/commands/drivetrain/DriverCommand.kt @@ -8,7 +8,10 @@ import edu.wpi.first.math.util.Units.degreesToRadians import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj2.command.CommandBase import frc.kyberlib.command.Game +import frc.robot.commands.pathing.MoveToPosition import frc.robot.constants.Constants +import frc.robot.constants.drivetrain.maxAutonomousAngularAcceleration +import frc.robot.constants.drivetrain.maxAutonomousAngularVelocity import frc.robot.controls.ControlScheme import frc.robot.subsystems.Drivetrain import frc.robot.subsystems.slewLimited @@ -20,15 +23,25 @@ class DriverCommand( var controlScheme: ControlScheme, var nearestStation: () -> Boolean, ) : CommandBase() { - private val rotationPid = ProfiledPIDController(1.0, 0.0, 0.0, TrapezoidProfile.Constraints(2*PI, 4*PI)).apply { + private val rotationPid = ProfiledPIDController( + MoveToPosition.rP, 0.0, 0.0, TrapezoidProfile.Constraints( + maxAutonomousAngularVelocity, maxAutonomousAngularAcceleration + ) + ).apply { enableContinuousInput(-PI, PI) + } + var lastNearestStation = nearestStation() init { addRequirements(drivetrain) } override fun execute() { + if (nearestStation() != lastNearestStation) { + lastNearestStation = nearestStation() + rotationPid.reset(drivetrain.estimatedPose2d.rotation.radians) + } val allianceMulitplier = when (Game.alliance) { DriverStation.Alliance.Invalid -> 1.0 else -> Game.alliance.xMul diff --git a/src/main/kotlin/frc/robot/commands/drivetrain/Nearest180.kt b/src/main/kotlin/frc/robot/commands/drivetrain/Nearest180.kt index d1c3d42..757b6a4 100644 --- a/src/main/kotlin/frc/robot/commands/drivetrain/Nearest180.kt +++ b/src/main/kotlin/frc/robot/commands/drivetrain/Nearest180.kt @@ -7,12 +7,15 @@ fun findNearest180( // the output will be in degrees with a range of -180 to 180 and will be the closest 180 degree angle to the input // for example, if the input is 0, the output will be 0, if the input is 179, the output will be 180, if the input // is -179, the output will be 180 etc. - val nearest180 = currentAngle % 360 - return if (nearest180 > 180) { + var nearest180 = currentAngle % 360 + println("nearest180 = $nearest180") + nearest180 = if (nearest180 > 180) { nearest180 - 360 } else if (nearest180 < -180) { nearest180 + 360 } else { nearest180 } -} + return if (nearest180 < 90 && nearest180 > -90) 0.0 + else 180.0 +} \ No newline at end of file diff --git a/src/main/kotlin/frc/robot/commands/drivetrain/RotateTo180.kt b/src/main/kotlin/frc/robot/commands/drivetrain/RotateTo180.kt index 9fee675..f055c3c 100644 --- a/src/main/kotlin/frc/robot/commands/drivetrain/RotateTo180.kt +++ b/src/main/kotlin/frc/robot/commands/drivetrain/RotateTo180.kt @@ -6,9 +6,10 @@ import frc.robot.RobotContainer class RotateTo180( val robotContainer: RobotContainer, ) : CommandBase() { - override fun execute() { + override fun initialize() { robotContainer.rotateTo180 = true } + override fun end(interrupted: Boolean) { robotContainer.rotateTo180 = false } diff --git a/src/main/kotlin/frc/robot/controls/ChrisControlScheme.kt b/src/main/kotlin/frc/robot/controls/ChrisControlScheme.kt index f7f2cbd..3bbe363 100644 --- a/src/main/kotlin/frc/robot/controls/ChrisControlScheme.kt +++ b/src/main/kotlin/frc/robot/controls/ChrisControlScheme.kt @@ -31,5 +31,5 @@ class ChrisControlScheme( override val decreaseEncoderAngle: Trigger = xbox.povLeft() override val increaseEncoderAngle: Trigger = xbox.povRight() override val lockSwerveModulesCircle: Trigger = xbox.rightBumper() -// override val snapTo180: Trigger = xbox.rightTrigger() + override val snapTo180: Trigger = xbox.rightTrigger() } From e2df04f87c9e1ab310178353e878ac00213054b3 Mon Sep 17 00:00:00 2001 From: a1cd <71281043+a1cd@users.noreply.github.com> Date: Sun, 2 Apr 2023 11:34:31 -0400 Subject: [PATCH 2/8] changed rotation pose estimator standard deviations --- src/main/kotlin/frc/robot/subsystems/Drivetrain.kt | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt b/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt index a8b5d28..3dc07b4 100644 --- a/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt +++ b/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt @@ -107,8 +107,6 @@ class Drivetrain( configFactoryDefault() } - private val f2d = Field2d() - val Idrc = getTab("drivetrain") // pose shuffleboard stuff (using the field 2d widget) @@ -120,12 +118,13 @@ class Drivetrain( }.toTypedArray(), Pose2d(), VecBuilder.fill(0.1, 0.1, 0.1), - VecBuilder.fill(1.8, 1.8, 1.8) + VecBuilder.fill(1.8, 1.8, 3.1) ) @Deprecated("Use estimatedPose2d instead, this is only for internal drivetrain use") var simEstimatedPose2d: Pose2d = Pose2d() + @Suppress("DEPRECATION") inline val estimatedPose2d: Pose2d get() = if (!Game.sim) { poseEstimator.estimatedPosition @@ -230,6 +229,7 @@ class Drivetrain( if (Game.sim) { val vel = estimatedVelocity + @Suppress("DEPRECATION") simEstimatedPose2d = simEstimatedPose2d + (vel * 0.02 * 0.01) + simQueuedForce simQueuedForce = Transform2d() } From b2310baf718ca6f0cd6476a65aeb03a80f7fe877 Mon Sep 17 00:00:00 2001 From: Kanishk-Pandey Date: Sun, 2 Apr 2023 11:48:43 -0400 Subject: [PATCH 3/8] competition changes Signed-off-by: Kanishk-Pandey --- src/main/kotlin/frc/robot/Robot.kt | 2 +- src/main/kotlin/frc/robot/RobotContainer.kt | 20 ++++++++-- .../commands/alltogether/PlacementValues.kt | 8 ++-- .../alltogether/SetSubsystemPosition.kt | 2 + .../frc/robot/commands/balance/AutoBalance.kt | 2 +- .../robot/commands/drivetrain/DriveCommand.kt | 4 +- .../commands/drivetrain/DriverCommand.kt | 8 ++-- .../robot/commands/drivetrain/Nearest180.kt | 8 ++-- .../robot/commands/intake/ManualZeroIntake.kt | 33 +++++++++++++++ .../robot/commands/intake/ZeroModeMotor.kt | 15 +++---- .../commands/pathing/AutoPlaceAndBalance.kt | 40 ++++++++++++------- .../robot/commands/pathing/MoveToPosition.kt | 9 ++++- .../frc/robot/controls/ChrisControlScheme.kt | 9 +++-- .../frc/robot/controls/ControlScheme.kt | 4 +- .../kotlin/frc/robot/subsystems/Drivetrain.kt | 4 ++ 15 files changed, 122 insertions(+), 46 deletions(-) create mode 100644 src/main/kotlin/frc/robot/commands/intake/ManualZeroIntake.kt diff --git a/src/main/kotlin/frc/robot/Robot.kt b/src/main/kotlin/frc/robot/Robot.kt index 8a180bf..4e01e47 100644 --- a/src/main/kotlin/frc/robot/Robot.kt +++ b/src/main/kotlin/frc/robot/Robot.kt @@ -33,7 +33,7 @@ class Robot : TimedRobot() { } override fun teleopInit() { - ZeroModeMotor(robotContainer.intake).schedule() + ZeroModeMotor(robotContainer).schedule() } var auto: Command? = null diff --git a/src/main/kotlin/frc/robot/RobotContainer.kt b/src/main/kotlin/frc/robot/RobotContainer.kt index 4cc820f..a7ae6f5 100644 --- a/src/main/kotlin/frc/robot/RobotContainer.kt +++ b/src/main/kotlin/frc/robot/RobotContainer.kt @@ -12,6 +12,8 @@ import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj.DriverStation.Alliance.Blue import edu.wpi.first.wpilibj.DriverStation.Alliance.Red import edu.wpi.first.wpilibj.GenericHID +import edu.wpi.first.wpilibj.PowerDistribution +import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab @@ -32,10 +34,7 @@ import frc.robot.commands.alltogether.SetSubsystemPosition import frc.robot.commands.drivetrain.DriveCommand import frc.robot.commands.drivetrain.RotateTo180 import frc.robot.commands.elevator.ZeroElevatorAndIdle -import frc.robot.commands.intake.DeployIntake -import frc.robot.commands.intake.IdleIntake -import frc.robot.commands.intake.IntakeEject -import frc.robot.commands.intake.ShootToLTwo +import frc.robot.commands.intake.* import frc.robot.commands.manipulator.ManipulatorIO import frc.robot.commands.manipulator.SetManipulatorSpeed import frc.robot.commands.manipulator.Throw @@ -76,6 +75,7 @@ class RobotContainer { val intake = Intake(arm, { wantedObject }) val elevator = Elevator(this@RobotContainer, arm, this.intake) + val pdh = PowerDistribution(1, PowerDistribution.ModuleType.kRev) init { arrayOf(controlScheme0, controlScheme1).forEachIndexed { i, it -> @@ -318,6 +318,12 @@ class RobotContainer { snapTo180 .whileTrue(RotateTo180(this@RobotContainer)) + + zeroIntake + .whileTrue(ZeroModeMotor(this@RobotContainer)) + +// balanceIOLevel +// .whileTrue(SetSubsystemPosition(this@RobotContainer, { IOLevel.Balance }, { GamePiece.cone }, true)) } } } @@ -545,6 +551,10 @@ class RobotContainer { // shuffleboard auto chooser private val autoChooserTab: ShuffleboardTab = Shuffleboard.getTab("Autonomous") + val cameraTurnOffChooser = autoChooserTab.add("PowerToggle", true) + .withWidget(BuiltInWidgets.kToggleButton) + .entry + @Suppress("unused") val autoChooserWidget: ComplexWidget = autoChooserTab.add("Autonomous", autoChooser) val armFieldPosition: FieldObject2d = drivetrain.field2d.getObject("arm") @@ -564,6 +574,8 @@ class RobotContainer { leds.update() smartDashboardSelector.update() + pdh.switchableChannel = cameraTurnOffChooser.getBoolean(true) + // send subsystems to SmartDashboard SmartDashboard.putData("Drivetrain/sendable", drivetrain) SmartDashboard.putData("elevator/sendable", elevator) diff --git a/src/main/kotlin/frc/robot/commands/alltogether/PlacementValues.kt b/src/main/kotlin/frc/robot/commands/alltogether/PlacementValues.kt index cb5f980..71f475e 100644 --- a/src/main/kotlin/frc/robot/commands/alltogether/PlacementValues.kt +++ b/src/main/kotlin/frc/robot/commands/alltogether/PlacementValues.kt @@ -79,10 +79,10 @@ enum class IOLevel( 0.0, 0.0 ), Balance( - bottomLimit, - Rotation2d.fromRadians(-PI / 2), - bottomLimit, - Rotation2d.fromRadians(-PI / 2), + bottomLimit + 0.28, + Rotation2d.fromRadians(-PI / 2).plus(Rotation2d.fromDegrees(-10.0)), + bottomLimit + 0.28, + Rotation2d.fromRadians(-PI / 2).plus(Rotation2d.fromDegrees(-10.0)), 0.0, 0.0 ) } \ No newline at end of file diff --git a/src/main/kotlin/frc/robot/commands/alltogether/SetSubsystemPosition.kt b/src/main/kotlin/frc/robot/commands/alltogether/SetSubsystemPosition.kt index 343321d..56a78f4 100644 --- a/src/main/kotlin/frc/robot/commands/alltogether/SetSubsystemPosition.kt +++ b/src/main/kotlin/frc/robot/commands/alltogether/SetSubsystemPosition.kt @@ -78,3 +78,5 @@ class SetSubsystemPosition( (elevator.height - goalElevatorPosition).absoluteValue < 0.05 } + + diff --git a/src/main/kotlin/frc/robot/commands/balance/AutoBalance.kt b/src/main/kotlin/frc/robot/commands/balance/AutoBalance.kt index 7fc8634..948ca35 100644 --- a/src/main/kotlin/frc/robot/commands/balance/AutoBalance.kt +++ b/src/main/kotlin/frc/robot/commands/balance/AutoBalance.kt @@ -50,7 +50,7 @@ class AutoBalance(private val drivetrain: Drivetrain) : CommandBase() { true ) } else { - drivetrain.drive(ChassisSpeeds(2.0 * speedMul, 0.0, 0.0), true) + drivetrain.drive(ChassisSpeeds(3.0 * speedMul, 0.0, 0.0), true) } diff --git a/src/main/kotlin/frc/robot/commands/drivetrain/DriveCommand.kt b/src/main/kotlin/frc/robot/commands/drivetrain/DriveCommand.kt index a18233d..22e9930 100644 --- a/src/main/kotlin/frc/robot/commands/drivetrain/DriveCommand.kt +++ b/src/main/kotlin/frc/robot/commands/drivetrain/DriveCommand.kt @@ -20,7 +20,7 @@ class DriveCommand( override fun execute() { drivetrain.drive( ChassisSpeeds(x(), y(), rotation()), - true, + isFieldOriented, Translation2d(0.0, 0.0) ) } @@ -28,7 +28,7 @@ class DriveCommand( override fun end(interrupted: Boolean) { drivetrain.drive( ChassisSpeeds(0.0, 0.0, 0.0), - true, + isFieldOriented, Translation2d(0.0, 0.0) ) } diff --git a/src/main/kotlin/frc/robot/commands/drivetrain/DriverCommand.kt b/src/main/kotlin/frc/robot/commands/drivetrain/DriverCommand.kt index e28c45c..88d0e8e 100644 --- a/src/main/kotlin/frc/robot/commands/drivetrain/DriverCommand.kt +++ b/src/main/kotlin/frc/robot/commands/drivetrain/DriverCommand.kt @@ -8,7 +8,9 @@ import edu.wpi.first.math.util.Units.degreesToRadians import edu.wpi.first.wpilibj.DriverStation import edu.wpi.first.wpilibj2.command.CommandBase import frc.kyberlib.command.Game +import frc.robot.commands.pathing.MoveToPosition import frc.robot.constants.Constants +import frc.robot.constants.drivetrain.maxVelocity import frc.robot.controls.ControlScheme import frc.robot.subsystems.Drivetrain import frc.robot.subsystems.slewLimited @@ -20,7 +22,7 @@ class DriverCommand( var controlScheme: ControlScheme, var nearestStation: () -> Boolean, ) : CommandBase() { - private val rotationPid = ProfiledPIDController(1.0, 0.0, 0.0, TrapezoidProfile.Constraints(2*PI, 4*PI)).apply { + private val rotationPid = ProfiledPIDController(MoveToPosition.rP, 0.0, 0.0, TrapezoidProfile.Constraints(2*PI, 4*PI)).apply { enableContinuousInput(-PI, PI) } @@ -34,7 +36,7 @@ class DriverCommand( else -> Game.alliance.xMul } val vec = Translation2d(-controlScheme.forward, -controlScheme.strafe) - .times(3.5) + .times(maxVelocity) drivetrain.drive( ChassisSpeeds( vec.x * Constants.powerPercent * @@ -52,7 +54,7 @@ class DriverCommand( -controlScheme.rotation * 2 * Math.PI * Constants.powerPercent * .5 * (if (drivetrain.invertrot.getBoolean(false)) -1 else 1) * controlScheme.speedMutiplier } - ).slewLimited(drivetrain.xSlewRateLimiter, drivetrain.ySlewRateLimiter, drivetrain.rotSlewRateLimiter), + ),//.slewLimited(drivetrain.xSlewRateLimiter, drivetrain.ySlewRateLimiter, drivetrain.rotSlewRateLimiter), true, Translation2d() // chris wants in the middle ) diff --git a/src/main/kotlin/frc/robot/commands/drivetrain/Nearest180.kt b/src/main/kotlin/frc/robot/commands/drivetrain/Nearest180.kt index d1c3d42..9477e42 100644 --- a/src/main/kotlin/frc/robot/commands/drivetrain/Nearest180.kt +++ b/src/main/kotlin/frc/robot/commands/drivetrain/Nearest180.kt @@ -8,11 +8,9 @@ fun findNearest180( // for example, if the input is 0, the output will be 0, if the input is 179, the output will be 180, if the input // is -179, the output will be 180 etc. val nearest180 = currentAngle % 360 - return if (nearest180 > 180) { - nearest180 - 360 - } else if (nearest180 < -180) { - nearest180 + 360 + return if (nearest180 >= 90 || nearest180<=-90) { + 180.0 } else { - nearest180 + 0.0 } } diff --git a/src/main/kotlin/frc/robot/commands/intake/ManualZeroIntake.kt b/src/main/kotlin/frc/robot/commands/intake/ManualZeroIntake.kt new file mode 100644 index 0000000..3149617 --- /dev/null +++ b/src/main/kotlin/frc/robot/commands/intake/ManualZeroIntake.kt @@ -0,0 +1,33 @@ +//package frc.robot.commands.intake +// +//import edu.wpi.first.wpilibj.Timer +//import edu.wpi.first.wpilibj2.command.CommandBase +//import frc.robot.RobotContainer +//import frc.robot.constants.intake +//import frc.robot.subsystems.Intake +// +//class ManualZeroIntake(private val robotContainer: RobotContainer): CommandBase() { +// +//// val timer = Timer() +// init { +// addRequirements(robotContainer.intake) +// } +// +//// override fun initialize() { +//// timer.restart() +//// robotContainer.intake.modeZeroed = false +//// robotContainer.intake.setDeployAngle(robotContainer.intake.deployPosition) +//// } +// override fun execute() { +// robotContainer.intake.modeVoltage = -3.0 +// robotContainer.intake.setModeAngle(-1.7) +// } +// +// override fun end(interrupted: Boolean) { +// robotContainer.intake.zeroModeMotor() +// robotContainer.intake.modeVoltage = 0.0 +// } +// +// override fun isFinished() = intake.an +// +//} \ No newline at end of file diff --git a/src/main/kotlin/frc/robot/commands/intake/ZeroModeMotor.kt b/src/main/kotlin/frc/robot/commands/intake/ZeroModeMotor.kt index f4ad37c..5b037d1 100644 --- a/src/main/kotlin/frc/robot/commands/intake/ZeroModeMotor.kt +++ b/src/main/kotlin/frc/robot/commands/intake/ZeroModeMotor.kt @@ -2,27 +2,28 @@ package frc.robot.commands.intake import edu.wpi.first.wpilibj.Timer import edu.wpi.first.wpilibj2.command.CommandBase +import frc.robot.RobotContainer import frc.robot.subsystems.Intake -class ZeroModeMotor(private val intake: Intake): CommandBase() { +class ZeroModeMotor(private val robotContainer: RobotContainer): CommandBase() { val timer = Timer() init { - addRequirements(intake) + addRequirements(robotContainer.intake) } override fun initialize() { timer.restart() - intake.modeZeroed = false - intake.setDeployAngle(intake.deployPosition) + robotContainer.intake.modeZeroed = false + robotContainer.intake.setDeployAngle(robotContainer.intake.deployPosition) } override fun execute() { - intake.modeVoltage = -3.0 + robotContainer.intake.modeVoltage = -3.0 } override fun end(interrupted: Boolean) { - intake.zeroModeMotor() - intake.modeVoltage = 0.0 + robotContainer.intake.zeroModeMotor() + robotContainer.intake.modeVoltage = 0.0 } override fun isFinished() = timer.hasElapsed(1.0) diff --git a/src/main/kotlin/frc/robot/commands/pathing/AutoPlaceAndBalance.kt b/src/main/kotlin/frc/robot/commands/pathing/AutoPlaceAndBalance.kt index 5398b4c..fcfe997 100644 --- a/src/main/kotlin/frc/robot/commands/pathing/AutoPlaceAndBalance.kt +++ b/src/main/kotlin/frc/robot/commands/pathing/AutoPlaceAndBalance.kt @@ -16,6 +16,7 @@ import frc.robot.utils.GamePiece import frc.robot.utils.grid.PlacementGroup import frc.robot.utils.grid.PlacementLevel import frc.robot.utils.grid.PlacementSide +import java.lang.Exception class AutoPlaceAndBalance( private val robotContainer: RobotContainer, @@ -52,25 +53,31 @@ class AutoPlaceAndBalance( Throw( robotContainer.manipulator, { GamePiece.cone }, - { PlacementLevel.Level3 }).withTimeout(0.75) + { PlacementLevel.Level3 }) + .withTimeout(0.5) ) // shoot cube .withTimeout(4.0) .andThen( - MoveToPosition(robotContainer.drivetrain, 14.4, 2.72, 180.0, maxPosSpeed = 3.7) + MoveToPosition(robotContainer.drivetrain, + 14.4, 2.72, 180.0, + maxPosSpeed = 3.7 + ) .andThen( - MoveToPosition( - robotContainer.drivetrain, - (13.34 - 0.5), - 2.72, - 180.0, + MoveToPosition(robotContainer.drivetrain, + (13.34 - 0.5), 2.72, 180.0, maxPosSpeed = 3.7 ).withTimeout(4.0) ) - .alongWith( + .deadlineWith( + SetSubsystemPosition(robotContainer, { IOLevel.Balance }, { GamePiece.cone }, true) + ) + ) + .andThen( + AutoBalance(robotContainer.drivetrain) + .deadlineWith( SetSubsystemPosition(robotContainer, { IOLevel.Balance }, { GamePiece.cone }, true) ) ) - .andThen(AutoBalance(robotContainer.drivetrain)) } private fun pathBlue(robotContainer: RobotContainer): Command { @@ -80,7 +87,8 @@ class AutoPlaceAndBalance( { IOLevel.High }, { GamePiece.cone }, true - )) + ) + ) .andThen( SetSubsystemPosition( robotContainer, @@ -103,13 +111,17 @@ class AutoPlaceAndBalance( .andThen( MoveToPosition(robotContainer.drivetrain, 16.52 - (13.34 - 0.5), 2.72, 0.0, maxPosSpeed = 3.7 - ) - .withTimeout(4.0) + ).withTimeout(4.0) ) - .alongWith( + .deadlineWith( + SetSubsystemPosition(robotContainer, { IOLevel.Balance }, { GamePiece.cone }, true) + ) + ) + .andThen( + AutoBalance(robotContainer.drivetrain) + .deadlineWith( SetSubsystemPosition(robotContainer, { IOLevel.Balance }, { GamePiece.cone }, true) ) ) - .andThen(AutoBalance(robotContainer.drivetrain)) } } \ No newline at end of file diff --git a/src/main/kotlin/frc/robot/commands/pathing/MoveToPosition.kt b/src/main/kotlin/frc/robot/commands/pathing/MoveToPosition.kt index 23c8cfb..d4fd521 100644 --- a/src/main/kotlin/frc/robot/commands/pathing/MoveToPosition.kt +++ b/src/main/kotlin/frc/robot/commands/pathing/MoveToPosition.kt @@ -32,6 +32,7 @@ val Pose2d.flipped: Pose2d //fun safePrint(vararg message: String, separator: String = "") { // message.joinToString { } //} + open class MoveToPosition( private val drivetrain: Drivetrain, /** @@ -46,7 +47,7 @@ open class MoveToPosition( * The desired velocity of the robot (in meters per second) */ private val velocity: Transform2d = Transform2d(), - private val toleranceppos: Double = 0.02, + private val toleranceppos: Double = 0.075, private val tolerancepvel: Double = 0.1, private val tolerancerpos: Double = 0.01, private val tolerancervel: Double = 0.1, @@ -143,6 +144,12 @@ open class MoveToPosition( rPIDController.reset(drivetrain.estimatedPose2d.rotation.radians, drivetrain.estimatedVelocity.rotation.radians) visualization.pose = pose(xPIDController, yPIDController, rPIDController) + + val current = drivetrain.estimatedPose2d + val desired = pose(xPIDController, yPIDController, rPIDController) + if (desired.translation.getDistance(current.translation) > 8.0) { + print("Unable to path") + } } // on command start and every time the command is executed, calculate the diff --git a/src/main/kotlin/frc/robot/controls/ChrisControlScheme.kt b/src/main/kotlin/frc/robot/controls/ChrisControlScheme.kt index f7f2cbd..efcbc6d 100644 --- a/src/main/kotlin/frc/robot/controls/ChrisControlScheme.kt +++ b/src/main/kotlin/frc/robot/controls/ChrisControlScheme.kt @@ -11,7 +11,7 @@ class ChrisControlScheme( override val xbox = CommandXboxController(xboxNum) override val speedMutiplier: Double - get() = (1.0 - (0.8*(round(xbox.rightTriggerAxis * 3)/3.0))) + get() = (1.0 - (0.3 * (round(xbox.rightTriggerAxis * 3) / 3.0))) .coerceIn(0.0, 1.0) override val rotation: Double @@ -31,5 +31,8 @@ class ChrisControlScheme( override val decreaseEncoderAngle: Trigger = xbox.povLeft() override val increaseEncoderAngle: Trigger = xbox.povRight() override val lockSwerveModulesCircle: Trigger = xbox.rightBumper() -// override val snapTo180: Trigger = xbox.rightTrigger() -} + + //override val snapTo180: Trigger = xbox.rightTrigger() + override val zeroIntake: Trigger = xbox.povUp() +// override val balanceIOLevel: Trigger = xbox.povDown() +} \ No newline at end of file diff --git a/src/main/kotlin/frc/robot/controls/ControlScheme.kt b/src/main/kotlin/frc/robot/controls/ControlScheme.kt index e4b00d1..f49771b 100644 --- a/src/main/kotlin/frc/robot/controls/ControlScheme.kt +++ b/src/main/kotlin/frc/robot/controls/ControlScheme.kt @@ -7,7 +7,7 @@ abstract class ControlScheme { abstract val xbox: CommandXboxController? open val speedMutiplier: Double - get() = 0.0 + get() = 1.0 open val rotation: Double get() = 0.0 @@ -62,5 +62,7 @@ abstract class ControlScheme { open val intakeEject: Trigger = Trigger { false } open val shootToLTwo: Trigger = Trigger { false } open val shootToLThree: Trigger = Trigger { false } + open val zeroIntake: Trigger = Trigger { false } +// open val balanceIOLevel: Trigger = Trigger { false } } \ No newline at end of file diff --git a/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt b/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt index a8b5d28..4c410d2 100644 --- a/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt +++ b/src/main/kotlin/frc/robot/subsystems/Drivetrain.kt @@ -282,6 +282,10 @@ class Drivetrain( SwerveDriveKinematics.desaturateWheelSpeeds( /* moduleStates = */ swerveModuleStates, /* currentChassisSpeed = */ currentChassisSpeeds, + /* + swerveModuleStates, + chassisSpeedsField, + */ /* attainableMaxModuleSpeedMetersPerSecond = */ 4.0, /* attainableMaxTranslationalSpeedMetersPerSecond = */ 4.0, /* attainableMaxRotationalVelocityRadiansPerSecond = */ PI, From 1d72e91088c1a7fadb8a79ed555ab44ca1f442b7 Mon Sep 17 00:00:00 2001 From: Kanishk-Pandey Date: Sun, 2 Apr 2023 22:29:34 -0400 Subject: [PATCH 4/8] Thats it folks! Signed-off-by: Kanishk-Pandey --- .glass/glass.json | 32 ++++++++++++++++++++++++-------- 1 file changed, 24 insertions(+), 8 deletions(-) diff --git a/.glass/glass.json b/.glass/glass.json index da2e636..6fdd498 100644 --- a/.glass/glass.json +++ b/.glass/glass.json @@ -22,10 +22,16 @@ }, "types": { "/FMSInfo": "FMSInfo", + "/Shuffleboard/Autonomous/Autonomous": "String Chooser", + "/SmartDashboard/Arm/sendable": "Subsystem", "/SmartDashboard/Drivetrain/sendable": "Subsystem", "/SmartDashboard/FIELD": "Field2d", "/SmartDashboard/Field": "Field2d", - "/SmartDashboard/elevcmd": "Subsystem" + "/SmartDashboard/Manipulator/sendable": "Subsystem", + "/SmartDashboard/elevator/sendable": "Subsystem", + "/SmartDashboard/elevcmd": "Subsystem", + "/SmartDashboard/field": "Field2d", + "/SmartDashboard/huhhh": "Subsystem" }, "windows": { "/FMSInfo": { @@ -33,11 +39,6 @@ "visible": true } }, - "/SmartDashboard/FIELD": { - "window": { - "visible": true - } - }, "/SmartDashboard/Field": { "Robot": { "length": 0.7620000243186951, @@ -54,6 +55,18 @@ "window": { "visible": true } + }, + "/SmartDashboard/field": { + "bottom": 544, + "height": 8.013679504394531, + "image": "C:\\Users\\frc\\IdeaProjects\\FRC2023\\field\\2023-field.png", + "left": 46, + "right": 1088, + "top": 36, + "width": 16.541748046875, + "window": { + "visible": true + } } } }, @@ -81,7 +94,7 @@ } }, "NetworkTables Settings": { - "mode": "Client (NT4)", + "mode": "Client (NT3)", "serverTeam": "6502" }, "Plots": { @@ -133,7 +146,10 @@ } ] } - ] + ], + "window": { + "visible": false + } } } } From 403ec785c73ce96ddda25bdf47a1a6a54233f888 Mon Sep 17 00:00:00 2001 From: Everett Wilber <71281043+a1cd@users.noreply.github.com> Date: Tue, 4 Apr 2023 08:32:35 -0400 Subject: [PATCH 5/8] Create dependency-checker.yml --- .github/workflows/dependency-checker.yml | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) create mode 100644 .github/workflows/dependency-checker.yml diff --git a/.github/workflows/dependency-checker.yml b/.github/workflows/dependency-checker.yml new file mode 100644 index 0000000..b15c76c --- /dev/null +++ b/.github/workflows/dependency-checker.yml @@ -0,0 +1,22 @@ +name: build +on: + push: + branches: + - main # run the action on your projects default branch + #- dev + +jobs: + build: + name: Dependencies + runs-on: ubuntu-latest + permissions: # The Dependency Submission API requires write permission + contents: write + steps: + - name: 'Checkout Repository' + uses: actions/checkout@v3 + + - name: Run snapshot action + uses: mikepenz/gradle-dependency-submission@{latest} + with: + gradle-project-path: "gradle-example" + gradle-build-module: ":app" From 9b7a899882e57af2fb90463be46057f181f27e34 Mon Sep 17 00:00:00 2001 From: Everett Wilber <71281043+a1cd@users.noreply.github.com> Date: Tue, 4 Apr 2023 08:37:20 -0400 Subject: [PATCH 6/8] Update dependency-checker.yml fixed a tiny bug in the checker --- .github/workflows/dependency-checker.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/dependency-checker.yml b/.github/workflows/dependency-checker.yml index b15c76c..09025d5 100644 --- a/.github/workflows/dependency-checker.yml +++ b/.github/workflows/dependency-checker.yml @@ -16,7 +16,7 @@ jobs: uses: actions/checkout@v3 - name: Run snapshot action - uses: mikepenz/gradle-dependency-submission@{latest} + uses: mikepenz/gradle-dependency-submission@v0.8.6 with: gradle-project-path: "gradle-example" gradle-build-module: ":app" From 1761f8303fd8f7fdcd4632fb98eb61d8ddd04e75 Mon Sep 17 00:00:00 2001 From: Everett Wilber <71281043+a1cd@users.noreply.github.com> Date: Tue, 4 Apr 2023 08:39:04 -0400 Subject: [PATCH 7/8] Update dependency-checker.yml --- .github/workflows/dependency-checker.yml | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/.github/workflows/dependency-checker.yml b/.github/workflows/dependency-checker.yml index 09025d5..64fa668 100644 --- a/.github/workflows/dependency-checker.yml +++ b/.github/workflows/dependency-checker.yml @@ -15,8 +15,11 @@ jobs: - name: 'Checkout Repository' uses: actions/checkout@v3 - - name: Run snapshot action - uses: mikepenz/gradle-dependency-submission@v0.8.6 + - name: Root Gradle Dependency Submission + uses: mikepenz/gradle-dependency-submission@{latest} with: - gradle-project-path: "gradle-example" - gradle-build-module: ":app" + use-gradlew: false + gradle-build-module: |- + : + gradle-build-configuration: |- + compileClasspath From 39fe39c0521671c698c110838de90eaf84ffb8fd Mon Sep 17 00:00:00 2001 From: Everett Wilber <71281043+a1cd@users.noreply.github.com> Date: Tue, 4 Apr 2023 08:42:44 -0400 Subject: [PATCH 8/8] Update dependency-checker.yml --- .github/workflows/dependency-checker.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/dependency-checker.yml b/.github/workflows/dependency-checker.yml index 64fa668..77fc76f 100644 --- a/.github/workflows/dependency-checker.yml +++ b/.github/workflows/dependency-checker.yml @@ -16,7 +16,7 @@ jobs: uses: actions/checkout@v3 - name: Root Gradle Dependency Submission - uses: mikepenz/gradle-dependency-submission@{latest} + uses: mikepenz/gradle-dependency-submission@v0.8.6 with: use-gradlew: false gradle-build-module: |-