Skip to content

Commit

Permalink
a
Browse files Browse the repository at this point in the history
  • Loading branch information
@TheGamer1002
TheGamer1002 committed Nov 12, 2024
1 parent a213620 commit 050fbb2
Show file tree
Hide file tree
Showing 9 changed files with 447 additions and 241 deletions.
2 changes: 2 additions & 0 deletions .gitignore
View file
Original file line number Diff line number Diff line change
Expand Up @@ -176,3 +176,5 @@ logs/

# Folder that has CTRE Phoenix Sim device config storage
ctre_sim/

kls_database.db
9 changes: 9 additions & 0 deletions src/main/java/frc/robot/Robot.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,8 @@ class Robot : TimedRobot() {

private var m_robotContainer: RobotContainer? = null



init {
// if (isReal()) {
PowerDistribution(1, PowerDistribution.ModuleType.kRev) // Enables power distribution logging
Expand Down Expand Up @@ -48,6 +50,7 @@ class Robot : TimedRobot() {

override fun robotPeriodic() {
CommandScheduler.getInstance().run()
m_robotContainer!!.updatePoseEstimation()
}

override fun disabledInit() {}
Expand Down Expand Up @@ -87,4 +90,10 @@ class Robot : TimedRobot() {
override fun testExit() {}

override fun simulationPeriodic() {}

companion object {
fun sim(): Boolean {
return isSimulation()
}
}
}
29 changes: 24 additions & 5 deletions src/main/java/frc/robot/RobotContainer.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,13 +17,12 @@ 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.sysid.SysIdRoutine
import frc.robot.commands.GoToNoteCommand
import frc.robot.commands.PutArmDownCommand
import frc.robot.generated.TunerConstants
import frc.robot.generated.TunerConstants.createDrivetrain
import frc.robot.subsystems.ArmSubsystem
import frc.robot.subsystems.drive.CommandSwerveDrivetrain
import frc.robot.subsystems.vision.VisionSubsystem
import frc.robot.subsystems.vision.BetterVisionSubsystem

class RobotContainer {
private val MaxSpeed =
Expand All @@ -39,9 +38,8 @@ class RobotContainer {
val drivetrain: CommandSwerveDrivetrain = createDrivetrain()

val armSubsystem: ArmSubsystem = ArmSubsystem()
val visionSubsystem: VisionSubsystem = VisionSubsystem.instance
val visionSubsystem: BetterVisionSubsystem = BetterVisionSubsystem()

val goToNoteCommand = GoToNoteCommand(drivetrain, visionSubsystem)
val putArmDownCommand = PutArmDownCommand(armSubsystem)

/* Setting up bindings for necessary control of the swerve drive platform */
Expand Down Expand Up @@ -115,7 +113,6 @@ class RobotContainer {
// reset the field-centric heading on left bumper press
joystick.leftBumper().onTrue(drivetrain.runOnce { drivetrain::resetPose })

joystick.rightBumper().whileTrue(goToNoteCommand.andThen(putArmDownCommand))

drivetrain.registerTelemetry { state: SwerveDriveState? ->
if (state != null) {
Expand All @@ -127,4 +124,26 @@ class RobotContainer {
val autonomousCommand: Command
get() = /* First put the drivetrain into auto run mode, then run the auto */
autoChooser.selected


fun updatePoseEstimation() {
var visionEst = visionSubsystem.leftEstimatedGlobalPose
visionEst.ifPresent {
est ->
run {
var leftEstStdDevs = visionSubsystem.leftEstimationStdDevs

drivetrain.addVisionMeasurement(est.estimatedPose.toPose2d(), est.timestampSeconds, leftEstStdDevs)
}
}
visionEst = visionSubsystem.rightEstimatedGlobalPose
visionEst.ifPresent {
est ->
run {
var rightEstStdDevs = visionSubsystem.rightEstimationStdDevs

drivetrain.addVisionMeasurement(est.estimatedPose.toPose2d(), est.timestampSeconds, rightEstStdDevs)
}
}
}
}
64 changes: 0 additions & 64 deletions src/main/java/frc/robot/commands/GoToNoteCommand.kt
View file

This file was deleted.

68 changes: 50 additions & 18 deletions src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,6 @@ import edu.wpi.first.math.geometry.Rotation2d
import edu.wpi.first.math.geometry.Translation2d
import edu.wpi.first.math.geometry.Translation3d
import edu.wpi.first.math.kinematics.ChassisSpeeds
import edu.wpi.first.math.kinematics.SwerveDriveOdometry
import edu.wpi.first.math.numbers.N1
import edu.wpi.first.math.numbers.N3
import edu.wpi.first.units.Units
Expand All @@ -29,13 +28,15 @@ 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.wpilibj.shuffleboard.Shuffleboard
import edu.wpi.first.wpilibj.smartdashboard.Field2d
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard
import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog
import edu.wpi.first.wpilibj2.command.Command
import edu.wpi.first.wpilibj2.command.Subsystem
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism
import frc.robot.util.RectanglePoseArea
import frc.robot.subsystems.vision.VisionSubsystem
import java.util.function.DoubleSupplier
import java.util.function.Supplier
import kotlin.math.PI
Expand Down Expand Up @@ -248,8 +249,19 @@ class CommandSwerveDrivetrain : SwerveDrivetrain, Subsystem {
return m_sysIdRoutineToApply.dynamic(direction)
}

// var rightConfidence = SmartDashboard.getEntry("Right Confidence")
//
// var leftConfidence = SmartDashboard.getEntry("Left Confidence")
//
// var rightField = Field2d()
// var leftField = Field2d()
init {
OdometryThread().start() // shrug
SmartDashboard.putNumber("Left Confidence", 0.0)
SmartDashboard.putNumber("Right Confidence", 0.0)

// Shuffleboard.getTab("stuff").add("left field", {leftField})
// Shuffleboard.getTab("stuff").add("right field", {rightField})
}

val field: RectanglePoseArea = RectanglePoseArea(Translation2d(0.0, 0.0), Translation2d(16.54, 8.02))
Expand All @@ -271,27 +283,47 @@ class CommandSwerveDrivetrain : SwerveDrivetrain, Subsystem {
}
}

var left = VisionSubsystem.instance.updateLeft()
var right = VisionSubsystem.instance.updateRight()

if (left !== null) {
var dist = VisionSubsystem.instance.getLeftResult()?.bestTarget?.bestCameraToTarget?.translation?.getDistance(Translation3d())
var confidence = 1 - ((dist!! - 1) / 6) // 1 - ((dist - min) / range)
if(field.isPoseWithinArea(left.estimatedPose.toPose2d())) {
addVisionMeasurement(left.estimatedPose.toPose2d(), left.timestampSeconds, VecBuilder.fill(confidence, confidence, 0.01)) // [x, y, theta]
}
}
if (right !== null) {
var dist = VisionSubsystem.instance.getRightResult()?.bestTarget?.bestCameraToTarget?.translation?.getDistance(Translation3d())
var confidence = 1 - ((dist!! - 1) / 6) // 1 - ((dist - min) / range)
if(field.isPoseWithinArea(right.estimatedPose.toPose2d())) {
addVisionMeasurement(right.estimatedPose.toPose2d(), right.timestampSeconds, VecBuilder.fill(confidence, confidence, 0.01)) // [x, y, theta]
}
}
// if (VisionSubsystem.instance.getLeftResult().hasTargets()) {
// var left = VisionSubsystem.instance.updateLeft()
// var dist = VisionSubsystem.instance.getLeftResult().bestTarget.bestCameraToTarget.translation.getDistance(Translation3d())
//
// var confidence = 1 - ((dist!! - 1) / 6) // 1 - ((dist - min) / range)
// leftConfidence.setNumber(confidence)
// if (left != null) {
// if(field.isPoseWithinArea(left.estimatedPose.toPose2d())) {
// // addVisionMeasurement(left.estimatedPose.toPose2d(), left.timestampSeconds, VecBuilder.fill(confidence, confidence, 0.01)) // [x, y, theta]
// addVisionMeasurement(left.estimatedPose.toPose2d(), left.timestampSeconds)
// leftField.robotPose = left.estimatedPose.toPose2d()
// println("left: " + left.estimatedPose.toPose2d())
//
//
// }
// }
// }
// if (VisionSubsystem.instance.getRightResult().hasTargets()) {
// var right = VisionSubsystem.instance.updateRight()
// var dist = VisionSubsystem.instance.getRightResult()?.bestTarget?.bestCameraToTarget?.translation?.getDistance(Translation3d())
// var confidence = 1 - ((dist!! - 1) / 6) // 1 - ((dist - min) / range)
// rightConfidence.setNumber(confidence)
// if (right != null) {
// if(field.isPoseWithinArea(right.estimatedPose.toPose2d())) {
// //addVisionMeasurement(right.estimatedPose.toPose2d(), right.timestampSeconds, VecBuilder.fill(confidence, confidence, 0.01)) // [x, y, theta]
// addVisionMeasurement(right.estimatedPose.toPose2d(), right.timestampSeconds)
//
// rightField.robotPose = right.estimatedPose.toPose2d()
// println("right: " + right.estimatedPose.toPose2d())
// }
// }
// }


}

// fun addVisionMeasurement(pose: Pose2d, timestamp: Double, stdDevs: Unit) {
// addVisionMeasurement(pose, timestamp)
// }

private fun startSimThread() {
m_lastSimTime = Utils.getCurrentTimeSeconds()

Expand Down
Loading

0 comments on commit 050fbb2

Please sign in to comment.