diff --git a/src/main/java/frc/robot/RobotContainer.kt b/src/main/java/frc/robot/RobotContainer.kt index 1c0f3f4..4eb6af0 100755 --- a/src/main/java/frc/robot/RobotContainer.kt +++ b/src/main/java/frc/robot/RobotContainer.kt @@ -112,8 +112,6 @@ class RobotContainer { .andThen({ AngularRate = MaxAngularRate }) ) - drv.leftBumper().onTrue(drivetrain.rotateToAngle(visionSubsystem.getNoteCamAngle())) - /* TODO make sure this works and probably also see if we can't get it to also move the robot forward a bit to center it w/ the arm */ if (Utils.isSimulation()) { drivetrain.seedFieldRelative(Pose2d(Translation2d(), Rotation2d.fromDegrees(0.0))) @@ -142,7 +140,9 @@ class RobotContainer { drv.back().and(drv.pov(0)).whileTrue(drivetrain.runDriveSlipTest()) - drv.rightBumper().whileTrue(drivetrain.rotateToAngle(visionSubsystem.getNoteCamAngle())) + drv.rightBumper().whileTrue(drivetrain.rotateToAngleDiff(visionSubsystem.getNoteCamAngle())) + /* TODO make sure this works and probably also see if we can't get it to also move the robot forward a bit to center it w/ the arm */ + } init { @@ -209,6 +209,7 @@ class RobotContainer { try { drivetrain.defaultCommand.cancel() } catch (_: Exception) { + DriverStation.reportWarning("RC: Failed to cancel default command", false) } drivetrain.defaultCommand = drivetrain.applyRequest(controlStyle!!).ignoringDisable(true) } @@ -222,7 +223,5 @@ class RobotContainer { return xLimiter.calculate(MathUtil.applyDeadband(joystick, deadband)) } - fun getAutonomousCommand(): Command? { - return null - } + } diff --git a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.kt b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.kt index 1d45078..ec0e1b6 100755 --- a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.kt +++ b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.kt @@ -3,34 +3,37 @@ package frc.robot.subsystems.drive import com.ctre.phoenix6.Utils import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrainConstants +import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType import com.ctre.phoenix6.mechanisms.swerve.SwerveModuleConstants import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest import com.ctre.phoenix6.signals.NeutralModeValue import com.pathplanner.lib.auto.AutoBuilder import com.pathplanner.lib.commands.PathPlannerAuto import com.pathplanner.lib.controllers.PPHolonomicDriveController -import com.pathplanner.lib.path.GoalEndState import com.pathplanner.lib.path.PathConstraints import com.pathplanner.lib.path.PathPlannerPath import com.pathplanner.lib.util.HolonomicPathFollowerConfig import com.pathplanner.lib.util.PIDConstants import com.pathplanner.lib.util.ReplanningConfig +import edu.wpi.first.math.controller.PIDController import edu.wpi.first.math.geometry.Pose2d import edu.wpi.first.math.geometry.Rotation2d import edu.wpi.first.math.kinematics.ChassisSpeeds import edu.wpi.first.units.Measure +import edu.wpi.first.units.Units import edu.wpi.first.units.Units.* import edu.wpi.first.units.Voltage 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.smartdashboard.SmartDashboard import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Subsystem import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine +import frc.robot.generated.TunerConstants import frc.robot.util.ModifiedSignalLogger import frc.robot.util.SwerveVoltageRequest -import frc.robot.generated.TunerConstants import org.littletonrobotics.junction.AutoLogOutput import java.util.* import java.util.function.Supplier @@ -46,6 +49,8 @@ class CommandSwerveDrivetrain : SwerveDrivetrain, Subsystem { private var driveBaseRadius = 0.0 private var simNotifier: Notifier? = null private var lastSimTime = 0.0 + private var thetaController: PIDController = PIDController(1.1, 0.0, 0.125) // TODO tune this + private var angleJoystickDeadbandRadius = 0.5 /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ private val blueAlliancePerspectiveRotation: Rotation2d = Rotation2d.fromDegrees(0.0) @@ -69,6 +74,8 @@ class CommandSwerveDrivetrain : SwerveDrivetrain, Subsystem { } } configurePathPlanner() + + SmartDashboard.putData(thetaController) // for tuning } constructor(driveTrainConstants: SwerveDrivetrainConstants, vararg modules: SwerveModuleConstants?) : super( @@ -141,20 +148,18 @@ class CommandSwerveDrivetrain : SwerveDrivetrain, Subsystem { return AutoBuilder.pathfindThenFollowPath(path, constraints, 0.0) } - fun rotateToAngle(angle: Double): Command { - // this takes a lot of building up - // first get the current angle - val currentAngle = this.state.Pose.rotation.degrees - // then get the difference between the current angle and the target angle - val angleDifference = angle - currentAngle - // create a Pose2d - val targetPose = Pose2d(0.0, 0.0, Rotation2d.fromDegrees(angleDifference)) - // and now the full thing - val path = PathPlannerPath( - PathPlannerPath.bezierFromPoses(targetPose), constraints, GoalEndState(0.0, Rotation2d(angleDifference)) - ) - // follow path command - return AutoBuilder.pathfindToPose(targetPose, constraints, 0.0, 0.0) + fun rotateToAngleDiff(angle: Double): Command { + return run { + // first get the current angle + val currentAngle = state.Pose.rotation.degrees + // then get the difference between the current angle and the target angle + val angleDifference = Units.Degree.of(angle - currentAngle).`in`(Units.Radian) // convert to radians + applyRequest { + SwerveRequest.FieldCentric() + .withDriveRequestType(DriveRequestType.OpenLoopVoltage) + .withRotationalRate(thetaController.calculate(angleDifference, state.Pose.rotation.radians) * 2 * PI) + } + } } var constraints = PathConstraints(9.46, 2.0, 2 * PI, 4 * PI)