Skip to content

Commit

Permalink
finished the logic for retrieving game piece data from the neural net…
Browse files Browse the repository at this point in the history 
…work
  • Loading branch information
@Shom770
Shom770 committed Dec 1, 2023
1 parent f5f3b24 commit 818083a
Show file tree
Hide file tree
Showing 23 changed files with 251 additions and 326 deletions.
4 changes: 0 additions & 4 deletions src/main/kotlin/com/team4099/robot2023/Robot.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,11 +11,9 @@ import com.team4099.robot2023.util.Alert.AlertType
import com.team4099.robot2023.util.FMSData
import com.team4099.robot2023.util.NTSafePublisher
import edu.wpi.first.hal.AllianceStationID
import edu.wpi.first.wpilibj.AnalogInput
import edu.wpi.first.wpilibj.DriverStation
import edu.wpi.first.wpilibj.PowerDistribution
import edu.wpi.first.wpilibj.RobotBase
import edu.wpi.first.wpilibj.RobotController
import edu.wpi.first.wpilibj.livewindow.LiveWindow
import edu.wpi.first.wpilibj.simulation.DriverStationSim
import edu.wpi.first.wpilibj2.command.Command
Expand Down Expand Up @@ -135,8 +133,6 @@ object Robot : LoggedRobot() {
CommandScheduler.getInstance().onCommandInterrupt { command: Command ->
Logger.getInstance().recordOutput("/ActiveCommands/${command.name}", false)
}


}

override fun disabledExit() {
Expand Down
41 changes: 16 additions & 25 deletions src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
View file
Original file line number Diff line number Diff line change
@@ -1,33 +1,24 @@
package com.team4099.robot2023

import com.ctre.phoenix.motorcontrol.can.TalonFX
import com.team4099.robot2023.auto.AutonomousSelector
import com.team4099.robot2023.commands.AutoIntakeCommand
import com.team4099.robot2023.commands.AutoScoreCommand
import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand
import com.team4099.robot2023.commands.drivetrain.SwerveModuleTuningCommand
import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand
import com.team4099.robot2023.config.ControlBoard
import com.team4099.robot2023.config.constants.Constants
import com.team4099.robot2023.config.constants.DrivetrainConstants
import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIO
import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOReal
import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim
import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO
import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIOPigeon2
import com.team4099.robot2023.subsystems.drivetrain.swervemodule.SwerveModule
import com.team4099.robot2023.subsystems.drivetrain.swervemodule.SwerveModuleIO
import com.team4099.robot2023.subsystems.drivetrain.swervemodule.SwerveModuleIOFalcon
import com.team4099.robot2023.subsystems.elevator.Elevator
import com.team4099.robot2023.subsystems.elevator.ElevatorIO
import com.team4099.robot2023.subsystems.elevator.ElevatorIONeo
import com.team4099.robot2023.subsystems.elevator.ElevatorIOSim
import com.team4099.robot2023.subsystems.gameboy.GameBoy
import com.team4099.robot2023.subsystems.gameboy.GameboyIOServer
import com.team4099.robot2023.subsystems.gameboy.objective.isConeNode
import com.team4099.robot2023.subsystems.groundintake.GroundIntake
import com.team4099.robot2023.subsystems.groundintake.GroundIntakeIO
import com.team4099.robot2023.subsystems.groundintake.GroundIntakeIONeo
import com.team4099.robot2023.subsystems.groundintake.GroundIntakeIOSim
import com.team4099.robot2023.subsystems.led.Led
Expand All @@ -36,15 +27,13 @@ import com.team4099.robot2023.subsystems.led.LedIOSim
import com.team4099.robot2023.subsystems.limelight.LimelightVision
import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO
import com.team4099.robot2023.subsystems.manipulator.Manipulator
import com.team4099.robot2023.subsystems.manipulator.ManipulatorIO
import com.team4099.robot2023.subsystems.manipulator.ManipulatorIONeo
import com.team4099.robot2023.subsystems.manipulator.ManipulatorIOSim
import com.team4099.robot2023.subsystems.superstructure.Request
import com.team4099.robot2023.subsystems.superstructure.Superstructure
import com.team4099.robot2023.subsystems.vision.Vision
import com.team4099.robot2023.subsystems.vision.camera.CameraIONorthstar
import com.team4099.robot2023.util.driver.Ryan
import edu.wpi.first.wpilibj.AnalogInput
import edu.wpi.first.wpilibj.RobotBase
import org.littletonrobotics.junction.Logger
import org.team4099.lib.smoothDeadband
Expand All @@ -64,7 +53,7 @@ object RobotContainer {
init {
if (RobotBase.isReal()) {
// Real Hardware Implementations
//drivetrain = Drivetrain(object: GyroIO {},object: DrivetrainIO {}
// drivetrain = Drivetrain(object: GyroIO {},object: DrivetrainIO {}
drivetrain = Drivetrain(GyroIOPigeon2, DrivetrainIOReal)
vision =
Vision(
Expand Down Expand Up @@ -124,16 +113,14 @@ object RobotContainer {
drivetrain
)

/*
/*
drivetrain.defaultCommand =
SwerveModuleTuningCommand(
drivetrain,
{ (ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) * 180).degrees },
)
*/

drivetrain.defaultCommand =
SwerveModuleTuningCommand(
drivetrain,
{ (ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) * 180).degrees },
)
*/
}

fun requestSuperstructureIdle() {
Expand Down Expand Up @@ -233,11 +220,15 @@ object RobotContainer {
)

ControlBoard.groundIntakeCone.whileTrue(superstructure.groundIntakeConeCommand())
ControlBoard.autoScore.whileTrue(AutoScoreCommand(drivetrain, superstructure)
.finallyDo { Logger.getInstance().recordOutput("Auto/isAutoDriving", false) }
ControlBoard.autoScore.whileTrue(
AutoScoreCommand(drivetrain, superstructure).finallyDo {
Logger.getInstance().recordOutput("Auto/isAutoDriving", false)
}
)
ControlBoard.autoIntake.whileTrue(AutoIntakeCommand(drivetrain, superstructure)
.finallyDo { Logger.getInstance().recordOutput("Auto/isAutoDriving", false) }
ControlBoard.autoIntake.whileTrue(
AutoIntakeCommand(drivetrain, superstructure).finallyDo {
Logger.getInstance().recordOutput("Auto/isAutoDriving", false)
}
)

ControlBoard.ejectGamePiece.whileTrue(superstructure.ejectGamePieceCommand())
Expand Down
2 changes: 0 additions & 2 deletions src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -90,8 +90,6 @@ object AutonomousSelector {

autonomousModeChooser.addOption("Score Preload Cone", AutonomousMode.PRELOAD_SCORE_AUTO)



autoTab.add("Mode", autonomousModeChooser.sendableChooser).withSize(4, 2).withPosition(2, 0)

autoEngageWidgit =
Expand Down
1 change: 0 additions & 1 deletion src/main/kotlin/com/team4099/robot2023/commands/AutoIntakeCommand.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -88,5 +88,4 @@ class AutoIntakeCommand(val drivetrain: Drivetrain, val superstructure: Superstr
superstructure.groundIntakeConeCommand()
)
}

}
33 changes: 11 additions & 22 deletions src/main/kotlin/com/team4099/robot2023/commands/AutoScoreCommand.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,8 +2,6 @@ package com.team4099.robot2023.commands

import com.team4099.lib.math.Zone2d
import com.team4099.lib.trajectory.Waypoint
import com.team4099.robot2023.RobotContainer
import com.team4099.robot2023.commands.drivetrain.DriveBrakeModeCommand
import com.team4099.robot2023.commands.drivetrain.DrivePathCommand
import com.team4099.robot2023.config.constants.Constants
import com.team4099.robot2023.config.constants.FieldConstants
Expand All @@ -16,17 +14,14 @@ import com.team4099.robot2023.subsystems.superstructure.Superstructure
import com.team4099.robot2023.util.AllianceFlipUtil
import com.team4099.robot2023.util.FMSData
import com.team4099.robot2023.util.Velocity2d
import edu.wpi.first.math.geometry.Rotation2d
import edu.wpi.first.wpilibj2.command.Commands.runOnce
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup
import org.littletonrobotics.junction.Logger
import org.team4099.lib.geometry.Pose2d
import org.team4099.lib.geometry.Pose3d
import org.team4099.lib.units.base.inches
import org.team4099.lib.units.base.meters
import org.team4099.lib.units.derived.Angle
import org.team4099.lib.units.derived.degrees
import org.team4099.lib.units.derived.inDegrees
import org.team4099.lib.units.derived.inRotation2ds
import org.team4099.lib.units.perSecond

Expand All @@ -46,7 +41,6 @@ class AutoScoreCommand(val drivetrain: Drivetrain, val superstructure: Superstru

val setupCommand =
runOnce({

Logger.getInstance().recordOutput("Auto/isAutoDriving", true)
drivePose = drivetrain.odometryPose
postAlignPose =
Expand All @@ -61,15 +55,16 @@ class AutoScoreCommand(val drivetrain: Drivetrain, val superstructure: Superstru
)
)


finalPose =
AllianceFlipUtil.apply(
Pose2d(
2.00.meters, // slightly offset in the x
FieldConstants.Grids.nodeFirstY +
FieldConstants.Grids.nodeSeparationY *
(if (FMSData.isBlue) superstructure.objective.nodeColumn
else 8 - superstructure.objective.nodeColumn),
(
if (FMSData.isBlue) superstructure.objective.nodeColumn
else 8 - superstructure.objective.nodeColumn
),
180.degrees
)
)
Expand All @@ -94,18 +89,12 @@ class AutoScoreCommand(val drivetrain: Drivetrain, val superstructure: Superstru
})

val breakCommand =
runOnce({
drivetrain.swerveModules.forEach() { it.setDriveBrakeMode(true) }
})
runOnce({ drivetrain.swerveModules.forEach() { it.setDriveBrakeMode(true) } })

val coastCommand =
runOnce({
drivetrain.swerveModules.forEach() { it.setDriveBrakeMode(false) }
})
runOnce({ drivetrain.swerveModules.forEach() { it.setDriveBrakeMode(false) } })

val updatedDrivePose = runOnce({
finalDrivePose = drivetrain.odometryPose
})
val updatedDrivePose = runOnce({ finalDrivePose = drivetrain.odometryPose })

addCommands(
setupCommand,
Expand All @@ -121,7 +110,7 @@ class AutoScoreCommand(val drivetrain: Drivetrain, val superstructure: Superstru
drivetrain.odometryPose.rotation.inRotation2ds
)
) +
//intermediaryWaypoints +
// intermediaryWaypoints +
listOf(
Waypoint(
finalPose.translation.translation2d,
Expand All @@ -132,9 +121,9 @@ class AutoScoreCommand(val drivetrain: Drivetrain, val superstructure: Superstru
},
keepTrapping = false,
flipForAlliances = false,
endVelocity= Velocity2d(0.meters.perSecond, 0.0.meters.perSecond),
endVelocity = Velocity2d(0.meters.perSecond, 0.0.meters.perSecond),
tolerance = Pose2d(5.inches, 3.inches, 3.degrees),
forceRobotVelocityCheck=true
forceRobotVelocityCheck = true
),
updatedDrivePose,
DrivePathCommand(
Expand All @@ -147,7 +136,7 @@ class AutoScoreCommand(val drivetrain: Drivetrain, val superstructure: Superstru
finalDrivePose.rotation.inRotation2ds
)
) +
//intermediaryWaypoints +
// intermediaryWaypoints +
listOf(
Waypoint(
postAlignPose.translation.translation2d,
Expand Down
3 changes: 2 additions & 1 deletion src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,8 @@ import org.team4099.lib.units.base.meters
import org.team4099.lib.units.derived.radians
import org.team4099.lib.units.perSecond

class DriveBrakeModeCommand(val drivetrain: Drivetrain, val isBreakMode: Boolean = true) : CommandBase() {
class DriveBrakeModeCommand(val drivetrain: Drivetrain, val isBreakMode: Boolean = true) :
CommandBase() {
init {
addRequirements(drivetrain)
}
Expand Down
5 changes: 2 additions & 3 deletions src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -333,9 +333,8 @@ class DrivePathCommand(
trajCurTime = Clock.fpgaTime - trajStartTime
return endPathOnceAtReference &&
(!keepTrapping || swerveDriveController.atReference()) &&
trajCurTime > trajectoryGenerator.driveTrajectory.totalTimeSeconds.seconds && (
drivetrain.fieldVelocity.magnitude < 0.1.meters.perSecond || !forceRobotVelocityCheck
)
trajCurTime > trajectoryGenerator.driveTrajectory.totalTimeSeconds.seconds &&
(drivetrain.fieldVelocity.magnitude < 0.1.meters.perSecond || !forceRobotVelocityCheck)
}

override fun end(interrupted: Boolean) {
Expand Down
2 changes: 0 additions & 2 deletions src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,8 +4,6 @@ import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
import com.team4099.robot2023.util.driver.DriverProfile
import edu.wpi.first.wpilibj2.command.CommandBase
import org.littletonrobotics.junction.Logger
import java.util.function.DoubleSupplier
import java.util.function.Supplier

class TeleopDriveCommand(
val driver: DriverProfile,
Expand Down
4 changes: 0 additions & 4 deletions src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,5 @@
package com.team4099.robot2023.config.constants

import edu.wpi.first.wpilibj.AnalogInput
import org.team4099.lib.units.base.seconds
import org.team4099.lib.units.milli

Expand All @@ -25,9 +24,6 @@ object Constants {
val LOOP_PERIOD_TIME = 20.milli.seconds
val POWER_DISTRIBUTION_HUB_ID = 1




enum class GamePiece {
CUBE,
CONE,
Expand Down
14 changes: 8 additions & 6 deletions src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -77,10 +77,10 @@ object DrivetrainConstants {
val DRIVE_STATOR_THRESHOLD_CURRENT_LIMIT = 80.0.amps
val DRIVE_STATOR_TRIGGER_THRESHOLD_TIME = 1.0.seconds

val FRONT_LEFT_MODULE_ZERO = 1.3.radians + 180.degrees + 180.degrees
val FRONT_RIGHT_MODULE_ZERO = 4.49.radians + 180.degrees + 180.degrees
val BACK_LEFT_MODULE_ZERO = 3.22.radians + 180.degrees + 180.degrees
val BACK_RIGHT_MODULE_ZERO = 0.99.radians - 180.degrees + 180.degrees
val FRONT_LEFT_MODULE_ZERO = 1.3.radians + 180.degrees + 180.degrees
val FRONT_RIGHT_MODULE_ZERO = 4.49.radians + 180.degrees + 180.degrees
val BACK_LEFT_MODULE_ZERO = 3.22.radians + 180.degrees + 180.degrees
val BACK_RIGHT_MODULE_ZERO = 0.99.radians - 180.degrees + 180.degrees

val STEERING_COMPENSATION_VOLTAGE = 10.volts
val DRIVE_COMPENSATION_VOLTAGE = 12.volts
Expand Down Expand Up @@ -109,7 +109,8 @@ object DrivetrainConstants {
val AUTO_POS_KDX: DerivativeGain<Meter, Velocity<Meter>>
get() {
if (RobotBase.isReal()) {
return (0.1.meters.perSecond / (1.0.meters.perSecond)).metersPerSecondPerMetersPerSecond // todo: 0.25
return (0.1.meters.perSecond / (1.0.meters.perSecond))
.metersPerSecondPerMetersPerSecond // todo: 0.25
} else {
return (0.0.meters.perSecond / (1.0.meters.perSecond)).metersPerSecondPerMetersPerSecond
}
Expand All @@ -135,7 +136,8 @@ object DrivetrainConstants {
val AUTO_POS_KDY: DerivativeGain<Meter, Velocity<Meter>>
get() {
if (RobotBase.isReal()) {
return (0.025.meters.perSecond / (1.0.meters.perSecond)).metersPerSecondPerMetersPerSecond // todo: 0.25
return (0.025.meters.perSecond / (1.0.meters.perSecond))
.metersPerSecondPerMetersPerSecond // todo: 0.25
} else {
return (0.0.meters.perSecond / (1.0.meters.perSecond)).metersPerSecondPerMetersPerSecond
}
Expand Down
Loading

0 comments on commit 818083a

Please sign in to comment.