From 8d28579ea4261e2d38ec2215d8c6174c68754540 Mon Sep 17 00:00:00 2001 From: AlphaPranav9102 <49816864+AlphaPranav9102@users.noreply.github.com> Date: Sun, 18 Feb 2024 15:40:43 -0500 Subject: [PATCH 1/2] dt fixes --- simgui-ds.json | 5 + .../deploy/pathplanner/autos/New Auto.auto | 18 ++ .../pathplanner/paths/5NotePureMove.path | 182 ++++++++++++++++++ .../pathplanner/paths/Example Path.path | 50 +---- .../com/team4099/robot2023/RobotContainer.kt | 33 ++-- .../com/team4099/robot2023/auto/PathStore.kt | 2 +- .../robot2023/auto/mode/ExamplePathAuto.kt | 1 - .../FollowPathPlannerPathCommand.kt | 3 +- .../team4099/robot2023/config/ControlBoard.kt | 4 +- .../robot2023/config/constants/Constants.kt | 4 +- .../config/constants/DrivetrainConstants.kt | 10 +- .../config/constants/GyroConstants.kt | 2 +- .../swervemodule/SwerveModuleIOTalon.kt | 87 +-------- 13 files changed, 249 insertions(+), 152 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/New Auto.auto create mode 100644 src/main/deploy/pathplanner/paths/5NotePureMove.path diff --git a/simgui-ds.json b/simgui-ds.json index 97f4895e..b4f2141f 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,9 @@ { + "Keyboard 0 Settings": { + "window": { + "visible": true + } + }, "System Joysticks": { "window": { "visible": false diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto new file mode 100644 index 00000000..07d4e332 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/New Auto.auto @@ -0,0 +1,18 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2, + "y": 2 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/5NotePureMove.path b/src/main/deploy/pathplanner/paths/5NotePureMove.path new file mode 100644 index 00000000..e79dcbbe --- /dev/null +++ b/src/main/deploy/pathplanner/paths/5NotePureMove.path @@ -0,0 +1,182 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.2289055048108568, + "y": 5.4387004560018815 + }, + "prevControl": null, + "nextControl": { + "x": 1.7231721740590975, + "y": 3.908136928915673 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.22637826150855, + "y": 4.12372243302023 + }, + "prevControl": { + "x": 2.172429912120765, + "y": 4.09804759236747 + }, + "nextControl": { + "x": 2.280326610896335, + "y": 4.1493972736729905 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.371676329252173, + "y": 5.4387004560018815 + }, + "prevControl": { + "x": 1.1350935870429284, + "y": 5.125820818897388 + }, + "nextControl": { + "x": 1.6082590714614176, + "y": 5.751580093106375 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.6622268722657387, + "y": 5.462296362194021 + }, + "prevControl": { + "x": 2.07483477608705, + "y": 5.411457747949859 + }, + "nextControl": { + "x": 2.7066640964687334, + "y": 5.466142390960894 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.1878985224745173, + "y": 5.696696223214165 + }, + "prevControl": { + "x": 2.129863497405182, + "y": 5.953154353689664 + }, + "nextControl": { + "x": 0.966076214972528, + "y": 5.636303176982367 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.4021521705928346, + "y": 6.677901317715116 + }, + "prevControl": { + "x": 1.8381221065820927, + "y": 5.508856306149623 + }, + "nextControl": { + "x": 2.417172472108316, + "y": 6.709033361340628 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.0688795243068614, + "y": 5.985668113961836 + }, + "prevControl": { + "x": 2.1442753447778573, + "y": 6.4137426928772 + }, + "nextControl": { + "x": 0.9454947637610032, + "y": 5.936553287207062 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.766426772723887, + "y": 7.256520752911847 + }, + "prevControl": { + "x": 3.670312735497631, + "y": 6.7083552256964465 + }, + "nextControl": { + "x": 8.420823369195706, + "y": 7.344095867289241 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.654099576432541, + "y": 6.019814612562775 + }, + "prevControl": { + "x": 5.070880344350176, + "y": 6.220795602050137 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0, + "rotationDegrees": 0, + "rotateFast": false + }, + { + "waypointRelativePos": 5, + "rotationDegrees": 28.75499731851916, + "rotateFast": false + }, + { + "waypointRelativePos": 7, + "rotationDegrees": 14.598436530771252, + "rotateFast": false + }, + { + "waypointRelativePos": 3, + "rotationDegrees": 0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 1.0, + "maxAngularVelocity": 270.0, + "maxAngularAcceleration": 500.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 22.463820093818477, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": null, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path index 4da23a2a..4f49b8c1 100644 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ b/src/main/deploy/pathplanner/paths/Example Path.path @@ -3,57 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.0, - "y": 7.0 + "x": 1.2289055048108568, + "y": 5.4387004560018815 }, "prevControl": null, "nextControl": { - "x": 3.0, - "y": 6.5 + "x": 1.2526592560220629, + "y": 3.8988635337354443 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 6.061558522422601, - "y": 6.1060047436211775 + "x": 4.682015652524012, + "y": 6.1630012238954786 }, "prevControl": { - "x": 5.061558522422601, - "y": 7.1060047436211775 - }, - "nextControl": { - "x": 7.061558522422601, - "y": 5.1060047436211775 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.9885342675054565, - "y": 3.354442578025781 - }, - "prevControl": { - "x": 7.7385342675054565, - "y": 4.8544425780257825 - }, - "nextControl": { - "x": 8.238534267505454, - "y": 1.8544425780257794 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.3963635116216997, - "y": 1.641386992337064 - }, - "prevControl": { - "x": 5.380848911075716, - "y": 2.0910493199386275 + "x": 2.7753614271785247, + "y": 6.054149658970127 }, "nextControl": null, "isLocked": false, @@ -71,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 173.99592794454193, + "rotation": -1.6558108046829694, "rotateFast": false }, "reversed": false, diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index c638a280..eedbff47 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -1,6 +1,7 @@ package com.team4099.robot2023 import com.team4099.robot2023.auto.AutonomousSelector +import com.team4099.robot2023.auto.mode.ExamplePathAuto import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand import com.team4099.robot2023.config.ControlBoard @@ -11,6 +12,7 @@ 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.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.feeder.Feeder @@ -68,7 +70,7 @@ object RobotContainer { limelight = LimelightVision(object : LimelightVisionIO {}) intake = Intake(IntakeIONEO) feeder = Feeder(FeederIONeo) - elevator = Elevator(ElevatorIONEO) + elevator = Elevator(object: ElevatorIO {}) flywheel = Flywheel(FlywheelIOTalon) wrist = Wrist(object : WristIO {}) } else { @@ -98,9 +100,9 @@ object RobotContainer { drivetrain.defaultCommand = TeleopDriveCommand( driver = Ryan(), - { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, - { ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, - { -1 * ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, + { -ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + { -ControlBoard.strafe.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) }, + { ControlBoard.turn.smoothDeadband(Constants.Joysticks.TURN_DEADBAND) }, { ControlBoard.slowMode }, drivetrain ) @@ -150,17 +152,18 @@ object RobotContainer { fun mapTeleopControls() { - ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain, toAngle = 180.degrees)) - ControlBoard.runGroundIntake.whileTrue(superstructure.groundIntakeCommand()) - ControlBoard.ejectGamePiece.whileTrue(superstructure.ejectGamePieceCommand()) - ControlBoard.prepAmpScore.whileTrue(superstructure.prepAmpCommand()) - ControlBoard.ampScore.whileTrue(superstructure.scoreAmpCommand()) - ControlBoard.scoreSpeakerLow.whileTrue(superstructure.scoreSpeakerLowCommand()) - ControlBoard.scoreSpeakerMid.whileTrue(superstructure.scoreSpeakerMidCommand()) - ControlBoard.scoreSpeakerHigh.whileTrue(superstructure.scoreSpeakerHighCommand()) - ControlBoard.climbExtend.whileTrue(superstructure.climbExtendCommand()) - ControlBoard.climbRetract.whileTrue(superstructure.climbRetractCommand()) - ControlBoard.requestIdle.whileTrue(superstructure.requestIdleCommand()) + ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain, toAngle = 0.degrees)) + //ControlBoard.runGroundIntake.whileTrue(superstructure.groundIntakeCommand()) +// ControlBoard.ejectGamePiece.whileTrue(superstructure.ejectGamePieceCommand()) +// ControlBoard.prepAmpScore.whileTrue(superstructure.prepAmpCommand()) +// ControlBoard.ampScore.whileTrue(superstructure.scoreAmpCommand()) +// ControlBoard.scoreSpeakerLow.whileTrue(superstructure.scoreSpeakerLowCommand()) +// ControlBoard.scoreSpeakerMid.whileTrue(superstructure.scoreSpeakerMidCommand()) +// ControlBoard.scoreSpeakerHigh.whileTrue(superstructure.scoreSpeakerHighCommand()) +// ControlBoard.climbExtend.whileTrue(superstructure.climbExtendCommand()) +// ControlBoard.climbRetract.whileTrue(superstructure.climbRetractCommand()) +// ControlBoard.requestIdle.whileTrue(superstructure.requestIdleCommand()) + ControlBoard.runGroundIntake.whileTrue(ExamplePathAuto(drivetrain)) /* TUNING COMMANDS diff --git a/src/main/kotlin/com/team4099/robot2023/auto/PathStore.kt b/src/main/kotlin/com/team4099/robot2023/auto/PathStore.kt index 44e951f4..6271d9a4 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/PathStore.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/PathStore.kt @@ -3,5 +3,5 @@ package com.team4099.robot2023.auto import com.pathplanner.lib.path.PathPlannerPath object PathStore { - val examplePath: PathPlannerPath = PathPlannerPath.fromPathFile("Example Path") + val examplePath: PathPlannerPath = PathPlannerPath.fromPathFile("5NotePureMove") } diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/ExamplePathAuto.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/ExamplePathAuto.kt index d1e4b419..e3481c77 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/ExamplePathAuto.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/ExamplePathAuto.kt @@ -10,7 +10,6 @@ import org.team4099.lib.geometry.Pose2d class ExamplePathAuto(val drivetrain: Drivetrain) : SequentialCommandGroup() { init { addRequirements(drivetrain) - addCommands( ResetPoseCommand(drivetrain, Pose2d(PathStore.examplePath.previewStartingHolonomicPose)), FollowPathPlannerPathCommand(drivetrain, PathStore.examplePath) diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt index e9c14ed4..1ffaeec2 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt @@ -182,6 +182,8 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla // Sampling the trajectory for a state that we're trying to target val stateFromTrajectory = generatedTrajectory.sample(trajCurTime.inSeconds) + lastSampledPose = Pose2d(stateFromTrajectory.targetHolonomicPose) + // Retrieves the last sampled pose, so we can keep our `atReference` variable updated Logger.recordOutput( "Odometry/targetPose", @@ -196,7 +198,6 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla ) Logger.recordOutput("Pathfollow/currentTheta", drivetrain.odometryPose.rotation.inDegrees) - lastSampledPose = Pose2d(stateFromTrajectory.targetHolonomicPose) val targetedChassisSpeeds = swerveDriveController.calculateRobotRelativeSpeeds( diff --git a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt index 85d8e866..3a0019ff 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt @@ -21,10 +21,10 @@ object ControlBoard { } val strafe: Double - get() = -driver.leftXAxis + get() = driver.leftXAxis val forward: Double - get() = -driver.leftYAxis + get() = driver.leftYAxis val turn: Double get() = driver.rightXAxis diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt index e5c2f98d..7b786e57 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt @@ -67,8 +67,8 @@ object Constants { const val SHOTGUN_PORT = 1 const val TECHNICIAN_PORT = 2 - const val THROTTLE_DEADBAND = 0.05 - const val TURN_DEADBAND = 0.05 + const val THROTTLE_DEADBAND = 0.1 + const val TURN_DEADBAND = 0.1 } object Drivetrain { 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 daf59558..9cb5ed7a 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -58,8 +58,8 @@ object DrivetrainConstants { val SLOW_AUTO_VEL = 2.meters.perSecond val SLOW_AUTO_ACCEL = 2.0.meters.perSecond.perSecond - val MAX_AUTO_VEL = 3.meters.perSecond // 4 - val MAX_AUTO_ACCEL = 3.meters.perSecond.perSecond // 3 + val MAX_AUTO_VEL = 1.meters.perSecond // 4 + val MAX_AUTO_ACCEL = 1.meters.perSecond.perSecond // 3 val MAX_AUTO_BRAKE_VEL = 0.5.meters.perSecond // 4 val MAX_AUTO_BRAKE_ACCEL = 0.5.meters.perSecond.perSecond // 3 @@ -101,7 +101,7 @@ object DrivetrainConstants { val AUTO_POS_KP: ProportionalGain> get() { if (RobotBase.isReal()) { - return 8.0.meters.perSecond / 1.0.meters + return 0.0.meters.perSecond / 1.0.meters } else { return 7.0.meters.perSecond / 1.0.meters } @@ -118,7 +118,7 @@ object DrivetrainConstants { val AUTO_POS_KD: DerivativeGain> get() { if (RobotBase.isReal()) { - return (0.05.meters.perSecond / (1.0.meters.perSecond)).metersPerSecondPerMetersPerSecond + return (0.00.meters.perSecond / (1.0.meters.perSecond)).metersPerSecondPerMetersPerSecond } else { return (0.0.meters.perSecond / (1.0.meters.perSecond)).metersPerSecondPerMetersPerSecond } @@ -126,7 +126,7 @@ object DrivetrainConstants { val AUTO_THETA_ALLOWED_ERROR = 3.degrees - val AUTO_THETA_PID_KP = 1.degrees.perSecond / 1.degrees + val AUTO_THETA_PID_KP = 0.degrees.perSecond / 1.degrees val AUTO_THETA_PID_KI = 0.0.degrees.perSecond / (1.degrees * 1.seconds) val AUTO_THETA_PID_KD = (0.1.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/GyroConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/GyroConstants.kt index a6ac9b3c..0ef5db0b 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/GyroConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/GyroConstants.kt @@ -3,7 +3,7 @@ package com.team4099.robot2023.config.constants import org.team4099.lib.units.derived.degrees object GyroConstants { - val mountPitch = 0.0.degrees + val mountPitch = 180.0.degrees val mountYaw = 0.0.degrees val mountRoll = 0.0.degrees } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt index 032c87a2..cd0a915e 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt @@ -10,6 +10,7 @@ import com.ctre.phoenix6.controls.DutyCycleOut import com.ctre.phoenix6.controls.PositionDutyCycle import com.ctre.phoenix6.controls.VelocityDutyCycle import com.ctre.phoenix6.hardware.TalonFX +import com.ctre.phoenix6.signals.InvertedValue import com.ctre.phoenix6.signals.NeutralModeValue import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.DrivetrainConstants @@ -82,87 +83,6 @@ class SwerveModuleIOTalon( } } - init { - driveFalcon.configurator.apply(TalonFXConfiguration()) - steeringFalcon.configurator.apply(TalonFXConfiguration()) - - driveFalcon.clearStickyFaults() - steeringFalcon.clearStickyFaults() - - steeringConfiguration.Slot0.kP = - steeringSensor.proportionalPositionGainToRawUnits(DrivetrainConstants.PID.STEERING_KP) - steeringConfiguration.Slot0.kI = - steeringSensor.integralPositionGainToRawUnits(DrivetrainConstants.PID.STEERING_KI) - steeringConfiguration.Slot0.kD = - steeringSensor.derivativePositionGainToRawUnits(DrivetrainConstants.PID.STEERING_KD) - steeringConfiguration.Slot0.kV = - steeringSensor.velocityFeedforwardToRawUnits(DrivetrainConstants.PID.STEERING_KFF) - steeringConfiguration.MotionMagic.MotionMagicCruiseVelocity = - steeringSensor.velocityToRawUnits(DrivetrainConstants.STEERING_VEL_MAX) - steeringConfiguration.MotionMagic.MotionMagicAcceleration = - steeringSensor.accelerationToRawUnits(DrivetrainConstants.STEERING_ACCEL_MAX) - steeringConfiguration.CurrentLimits.SupplyCurrentLimit = - DrivetrainConstants.STEERING_SUPPLY_CURRENT_LIMIT.inAmperes - steeringConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true - - /* - steeringConfiguration.ClosedLoopGeneral.ContinuousWrap = true - steeringConfiguration.Feedback.SensorToMechanismRatio = - 1 / DrivetrainConstants.STEERING_SENSOR_GEAR_RATIO - steeringConfiguration.Feedback.SensorToMechanismRatio = 1 / DrivetrainConstants.STEERING_SENSOR_GEAR_RATIO - */ - - steeringConfiguration.MotorOutput.NeutralMode = - NeutralModeValue.Brake // change back to coast maybe? - steeringFalcon.inverted = true - steeringFalcon.configurator.apply(steeringConfiguration) - - driveConfiguration.Slot0.kP = - driveSensor.proportionalVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KP) - driveConfiguration.Slot0.kI = - driveSensor.integralVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KI) - driveConfiguration.Slot0.kD = - driveSensor.derivativeVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KD) - driveConfiguration.Slot0.kV = 0.05425 - // driveSensor.velocityFeedforwardToRawUnits(DrivetrainConstants.PID.DRIVE_KFF) - driveConfiguration.CurrentLimits.SupplyCurrentLimit = - DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT.inAmperes - driveConfiguration.CurrentLimits.SupplyCurrentThreshold = - DrivetrainConstants.DRIVE_THRESHOLD_CURRENT_LIMIT.inAmperes - driveConfiguration.CurrentLimits.SupplyTimeThreshold = - DrivetrainConstants.DRIVE_TRIGGER_THRESHOLD_TIME.inSeconds - driveConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true - driveConfiguration.CurrentLimits.StatorCurrentLimit = - DrivetrainConstants.DRIVE_STATOR_CURRENT_LIMIT.inAmperes - driveConfiguration.CurrentLimits.StatorCurrentLimitEnable = false // TODO tune - - driveConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake - driveFalcon.configurator.apply(driveConfiguration) - - MotorChecker.add( - "Drivetrain", - "Drive", - MotorCollection( - mutableListOf(Falcon500(driveFalcon, "$label Drive Motor")), - DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT, - 90.celsius, - DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT - 30.amps, - 110.celsius - ) - ) - - MotorChecker.add( - "Drivetrain", - "Steering", - MotorCollection( - mutableListOf(Falcon500(steeringFalcon, "$label Steering Motor")), - DrivetrainConstants.STEERING_SUPPLY_CURRENT_LIMIT, - 90.celsius, - DrivetrainConstants.STEERING_SUPPLY_CURRENT_LIMIT - 10.amps, - 110.celsius - ) - ) - } val driveStatorCurrentSignal: StatusSignal val driveSupplyCurrentSignal: StatusSignal val steeringStatorCurrentSignal: StatusSignal @@ -212,7 +132,7 @@ class SwerveModuleIOTalon( driveSensor.integralVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KI) driveConfiguration.Slot0.kD = driveSensor.derivativeVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KD) - driveConfiguration.Slot0.kV = 0.05425 + driveConfiguration.Slot0.kV = 0.02 // driveSensor.velocityFeedforwardToRawUnits(DrivetrainConstants.PID.DRIVE_KFF) driveConfiguration.CurrentLimits.SupplyCurrentLimit = DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT.inAmperes @@ -224,8 +144,8 @@ class SwerveModuleIOTalon( driveConfiguration.CurrentLimits.StatorCurrentLimit = DrivetrainConstants.DRIVE_STATOR_CURRENT_LIMIT.inAmperes driveConfiguration.CurrentLimits.StatorCurrentLimitEnable = false // TODO tune - driveConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Coast + driveConfiguration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive driveFalcon.configurator.apply(driveConfiguration) driveStatorCurrentSignal = driveFalcon.statorCurrent @@ -482,6 +402,7 @@ class SwerveModuleIOTalon( } else { motorOutputConfig.NeutralMode = NeutralModeValue.Coast } + motorOutputConfig.Inverted = InvertedValue.Clockwise_Positive driveFalcon.configurator.apply(motorOutputConfig) } From b581e3773eb3198e1072eb66e5043c348bb09696 Mon Sep 17 00:00:00 2001 From: Saraansh Wadkar Date: Sun, 18 Feb 2024 17:20:51 -0500 Subject: [PATCH 2/2] work --- .../com/team4099/robot2023/RobotContainer.kt | 23 ++++++++-------- .../robot2023/auto/AutonomousSelector.kt | 6 ++--- .../robot2023/auto/mode/ExamplePathAuto.kt | 5 ++-- .../FollowPathPlannerPathCommand.kt | 27 +++++++++++++------ .../commands/drivetrain/ResetPoseCommand.kt | 1 + .../config/constants/DrivetrainConstants.kt | 2 +- .../swervemodule/SwerveModuleIOTalon.kt | 2 +- .../team4099/robot2023/util/PoseEstimator.kt | 2 ++ 8 files changed, 40 insertions(+), 28 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index eedbff47..f4bbc2fd 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -13,7 +13,6 @@ import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIOPigeon2 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.feeder.Feeder import com.team4099.robot2023.subsystems.feeder.FeederIONeo @@ -70,7 +69,7 @@ object RobotContainer { limelight = LimelightVision(object : LimelightVisionIO {}) intake = Intake(IntakeIONEO) feeder = Feeder(FeederIONeo) - elevator = Elevator(object: ElevatorIO {}) + elevator = Elevator(object : ElevatorIO {}) flywheel = Flywheel(FlywheelIOTalon) wrist = Wrist(object : WristIO {}) } else { @@ -153,16 +152,16 @@ object RobotContainer { fun mapTeleopControls() { ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain, toAngle = 0.degrees)) - //ControlBoard.runGroundIntake.whileTrue(superstructure.groundIntakeCommand()) -// ControlBoard.ejectGamePiece.whileTrue(superstructure.ejectGamePieceCommand()) -// ControlBoard.prepAmpScore.whileTrue(superstructure.prepAmpCommand()) -// ControlBoard.ampScore.whileTrue(superstructure.scoreAmpCommand()) -// ControlBoard.scoreSpeakerLow.whileTrue(superstructure.scoreSpeakerLowCommand()) -// ControlBoard.scoreSpeakerMid.whileTrue(superstructure.scoreSpeakerMidCommand()) -// ControlBoard.scoreSpeakerHigh.whileTrue(superstructure.scoreSpeakerHighCommand()) -// ControlBoard.climbExtend.whileTrue(superstructure.climbExtendCommand()) -// ControlBoard.climbRetract.whileTrue(superstructure.climbRetractCommand()) -// ControlBoard.requestIdle.whileTrue(superstructure.requestIdleCommand()) + // ControlBoard.runGroundIntake.whileTrue(superstructure.groundIntakeCommand()) + // ControlBoard.ejectGamePiece.whileTrue(superstructure.ejectGamePieceCommand()) + // ControlBoard.prepAmpScore.whileTrue(superstructure.prepAmpCommand()) + // ControlBoard.ampScore.whileTrue(superstructure.scoreAmpCommand()) + // ControlBoard.scoreSpeakerLow.whileTrue(superstructure.scoreSpeakerLowCommand()) + // ControlBoard.scoreSpeakerMid.whileTrue(superstructure.scoreSpeakerMidCommand()) + // ControlBoard.scoreSpeakerHigh.whileTrue(superstructure.scoreSpeakerHighCommand()) + // ControlBoard.climbExtend.whileTrue(superstructure.climbExtendCommand()) + // ControlBoard.climbRetract.whileTrue(superstructure.climbRetractCommand()) + // ControlBoard.requestIdle.whileTrue(superstructure.requestIdleCommand()) ControlBoard.runGroundIntake.whileTrue(ExamplePathAuto(drivetrain)) /* diff --git a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt index bb6f6847..5839f717 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/AutonomousSelector.kt @@ -1,6 +1,6 @@ package com.team4099.robot2023.auto -import com.team4099.robot2023.auto.mode.TestAutoPath +import com.team4099.robot2023.auto.mode.ExamplePathAuto import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import edu.wpi.first.networktables.GenericEntry import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets @@ -56,11 +56,11 @@ object AutonomousSelector { get() = secondaryWaitInAuto.getDouble(0.0).seconds fun getCommand(drivetrain: Drivetrain): Command { - val mode = autonomousModeChooser.get() + val mode = AutonomousMode.TEST_AUTO_PATH // println("${waitTime().inSeconds} wait command") when (mode) { AutonomousMode.TEST_AUTO_PATH -> - return WaitCommand(waitTime.inSeconds).andThen(TestAutoPath(drivetrain)) + return WaitCommand(waitTime.inSeconds).andThen(ExamplePathAuto(drivetrain)) else -> println("ERROR: unexpected auto mode: $mode") } return InstantCommand() diff --git a/src/main/kotlin/com/team4099/robot2023/auto/mode/ExamplePathAuto.kt b/src/main/kotlin/com/team4099/robot2023/auto/mode/ExamplePathAuto.kt index e3481c77..86cc0343 100644 --- a/src/main/kotlin/com/team4099/robot2023/auto/mode/ExamplePathAuto.kt +++ b/src/main/kotlin/com/team4099/robot2023/auto/mode/ExamplePathAuto.kt @@ -2,16 +2,15 @@ package com.team4099.robot2023.auto.mode import com.team4099.robot2023.auto.PathStore import com.team4099.robot2023.commands.drivetrain.FollowPathPlannerPathCommand -import com.team4099.robot2023.commands.drivetrain.ResetPoseCommand import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import edu.wpi.first.wpilibj2.command.SequentialCommandGroup -import org.team4099.lib.geometry.Pose2d class ExamplePathAuto(val drivetrain: Drivetrain) : SequentialCommandGroup() { init { addRequirements(drivetrain) addCommands( - ResetPoseCommand(drivetrain, Pose2d(PathStore.examplePath.previewStartingHolonomicPose)), + // ResetPoseCommand(drivetrain, + // Pose2d(PathStore.examplePath.previewStartingHolonomicPose)), FollowPathPlannerPathCommand(drivetrain, PathStore.examplePath) ) } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt index 1ffaeec2..ed2b140b 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt @@ -1,10 +1,12 @@ package com.team4099.robot2023.commands.drivetrain import com.pathplanner.lib.path.PathPlannerPath +import com.pathplanner.lib.path.PathPlannerTrajectory import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.superstructure.Request +import edu.wpi.first.math.geometry.Rotation2d import edu.wpi.first.math.kinematics.ChassisSpeeds import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger @@ -38,8 +40,11 @@ import org.team4099.lib.units.derived.perMeterSeconds import org.team4099.lib.units.derived.radians import org.team4099.lib.units.perSecond -class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPlannerPath) : - Command() { +class FollowPathPlannerPathCommand( + val drivetrain: Drivetrain, + val path: PathPlannerPath, + val resetPose: Boolean = true +) : Command() { private val translationToleranceAtEnd = 1.inches private val thetaToleranceAtEnd = 2.5.degrees @@ -50,6 +55,11 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla private var pathFollowRequest = Request.DrivetrainRequest.ClosedLoop(ChassisSpeeds(0.0, 0.0, 0.0)) private var lastSampledPose = Pose2d() + + private lateinit var currentSpeeds: ChassisSpeeds + private lateinit var poseRotation: Rotation2d + private lateinit var generatedTrajectory: PathPlannerTrajectory + private val atReference: Boolean get() = ( @@ -140,6 +150,12 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla override fun initialize() { trajStartTime = Clock.fpgaTime + + drivetrain.odometryPose = Pose2d(path.previewStartingHolonomicPose) + + currentSpeeds = drivetrain.targetedChassisSpeeds + poseRotation = drivetrain.odometryPose.rotation.inRotation2ds + generatedTrajectory = path.getTrajectory(currentSpeeds, poseRotation) } override fun execute() { @@ -175,10 +191,6 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla ) } - val currentSpeeds = drivetrain.targetedChassisSpeeds - val poseRotation = drivetrain.odometryPose.rotation.inRotation2ds - val generatedTrajectory = path.getTrajectory(currentSpeeds, poseRotation) - // Sampling the trajectory for a state that we're trying to target val stateFromTrajectory = generatedTrajectory.sample(trajCurTime.inSeconds) @@ -198,7 +210,6 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla ) Logger.recordOutput("Pathfollow/currentTheta", drivetrain.odometryPose.rotation.inDegrees) - val targetedChassisSpeeds = swerveDriveController.calculateRobotRelativeSpeeds( drivetrain.odometryPose, stateFromTrajectory @@ -219,7 +230,7 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla // trajCurTime > trajectoryGenerator.driveTrajectory.totalTimeSeconds.seconds // } override fun isFinished(): Boolean { - return atReference + return atReference && trajCurTime.inSeconds >= generatedTrajectory.totalTimeSeconds } override fun end(interrupted: Boolean) { diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt index 305c8f00..fbabd978 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt @@ -21,6 +21,7 @@ class ResetPoseCommand(val drivetrain: Drivetrain, val pose: Pose2d) : Command() } override fun isFinished(): Boolean { + Logger.recordOutput("ActiveCommands/ResetPoseCommand", false) return true } } 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 9cb5ed7a..4677264f 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -151,7 +151,7 @@ object DrivetrainConstants { val STEERING_KFF = 0.0.volts / 1.0.radians.perSecond // 0.0375 - val DRIVE_KP = 2.6829.volts / 1.meters.perSecond + val DRIVE_KP = 0.0001.volts / 1.meters.perSecond val DRIVE_KI = 0.0.volts / (1.meters.perSecond * 1.seconds) val DRIVE_KD = 0.0.volts / 1.meters.perSecond.perSecond diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt index cd0a915e..9d895724 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOTalon.kt @@ -132,7 +132,7 @@ class SwerveModuleIOTalon( driveSensor.integralVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KI) driveConfiguration.Slot0.kD = driveSensor.derivativeVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KD) - driveConfiguration.Slot0.kV = 0.02 + driveConfiguration.Slot0.kV = 0.1267939375649165 / 15 // kv too high rn // driveSensor.velocityFeedforwardToRawUnits(DrivetrainConstants.PID.DRIVE_KFF) driveConfiguration.CurrentLimits.SupplyCurrentLimit = DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT.inAmperes diff --git a/src/main/kotlin/com/team4099/robot2023/util/PoseEstimator.kt b/src/main/kotlin/com/team4099/robot2023/util/PoseEstimator.kt index 16a00a7c..5f811d0e 100644 --- a/src/main/kotlin/com/team4099/robot2023/util/PoseEstimator.kt +++ b/src/main/kotlin/com/team4099/robot2023/util/PoseEstimator.kt @@ -1,5 +1,6 @@ package com.team4099.robot2023.util +import com.team4099.lib.hal.Clock import com.team4099.lib.vision.TimestampedVisionUpdate import edu.wpi.first.math.Matrix import edu.wpi.first.math.Nat @@ -32,6 +33,7 @@ class PoseEstimator(stateStdDevs: Matrix) { /** Resets the odometry to a known pose. */ fun resetPose(pose: Pose2d) { + Logger.recordOutput("Odometry/resetPoseTime", Clock.fpgaTime.inSeconds) basePose = pose updates.clear() update()