diff --git a/src/main/deploy/pathplanner/L Path.path b/src/main/deploy/pathplanner/L Path.path index 15b82fa..de23fbf 100644 --- a/src/main/deploy/pathplanner/L Path.path +++ b/src/main/deploy/pathplanner/L Path.path @@ -1,55 +1 @@ -{ - "waypoints": [ - { - "anchorPoint": { - "x": 2.0, - "y": 2.0 - }, - "prevControl": null, - "nextControl": { - "x": 3.0, - "y": 2.0 - }, - "holonomicAngle": 0.0, - "isReversal": false, - "velOverride": null, - "isLocked": false - }, - { - "anchorPoint": { - "x": 5.0, - "y": 2.0 - }, - "prevControl": { - "x": 4.000000000000002, - "y": 2.0 - }, - "nextControl": { - "x": 5.004218182347978, - "y": 2.0 - }, - "holonomicAngle": 0.0, - "isReversal": false, - "velOverride": null, - "isLocked": false - }, - { - "anchorPoint": { - "x": 5.0, - "y": 5.0 - }, - "prevControl": { - "x": 4.991875448297241, - "y": 5.0 - }, - "nextControl": null, - "holonomicAngle": 0.0, - "isReversal": false, - "velOverride": null, - "isLocked": false - } - ], - "maxVelocity": 1.0, - "maxAcceleration": 1.0, - "isReversed": null -} \ No newline at end of file +{"waypoints":[{"anchorPoint":{"x":4.0,"y":2.0},"prevControl":null,"nextControl":{"x":5.0,"y":2.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":2.0},"prevControl":{"x":4.000000000000002,"y":2.0},"nextControl":{"x":5.004218182347978,"y":2.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":3.0},"prevControl":{"x":4.991875448297241,"y":3.0},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Move Forward.path b/src/main/deploy/pathplanner/Move Forward.path index b63994d..1e2c78c 100644 --- a/src/main/deploy/pathplanner/Move Forward.path +++ b/src/main/deploy/pathplanner/Move Forward.path @@ -1 +1 @@ -{"waypoints":[{"anchorPoint":{"x":3.8761638155965237,"y":2.097178183811552},"prevControl":null,"nextControl":{"x":3.891027717524718,"y":2.097178183811552},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.989837747689554,"y":2.097178183811552},"prevControl":{"x":4.984427304969435,"y":2.097178183811552},"nextControl":{"x":4.984427304969435,"y":2.097178183811552},"holonomicAngle":91.52752544221289,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.8761638155965237,"y":2.140568077269722},"prevControl":{"x":3.3359301436282043,"y":2.140568077269722},"nextControl":null,"holonomicAngle":178.2100893917539,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null} \ No newline at end of file +{"waypoints":[{"anchorPoint":{"x":3.0,"y":2.0},"prevControl":null,"nextControl":{"x":3.0148639019281944,"y":2.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":2.0},"prevControl":{"x":4.45976632803168,"y":2.0},"nextControl":null,"holonomicAngle":180.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Rotate.path b/src/main/deploy/pathplanner/Rotate.path index 0707701..bc807e0 100644 --- a/src/main/deploy/pathplanner/Rotate.path +++ b/src/main/deploy/pathplanner/Rotate.path @@ -1 +1 @@ -{"waypoints":[{"anchorPoint":{"x":3.0,"y":5.0},"prevControl":null,"nextControl":{"x":3.018133524616885,"y":5.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.0,"y":5.0},"prevControl":{"x":2.9945895572798564,"y":5.0},"nextControl":null,"holonomicAngle":-180.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":false} \ No newline at end of file +{"waypoints":[{"anchorPoint":{"x":3.0,"y":5.0},"prevControl":null,"nextControl":{"x":3.018133524616885,"y":5.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.0,"y":5.0},"prevControl":{"x":2.9945895572798564,"y":5.0},"nextControl":null,"holonomicAngle":-180.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":0.05,"maxAcceleration":0.05,"isReversed":false} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Test Forward.path b/src/main/deploy/pathplanner/Test Forward.path new file mode 100644 index 0000000..428cb4a --- /dev/null +++ b/src/main/deploy/pathplanner/Test Forward.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":0.0,"y":0.0},"prevControl":null,"nextControl":{"x":1.0,"y":0.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.0,"y":0.0},"prevControl":{"x":3.0,"y":0.0},"nextControl":null,"holonomicAngle":-180.0,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index d129c34..7101859 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -46,18 +46,18 @@ public final class Constants { public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20; // TODO: redundant constant? // IDs - public static final int LEFT_FRONT_STEER_CAN_ID = 2; // * - public static final int LEFT_FRONT_WHEEL_CAN_ID = 3; // * - public static final int RIGHT_FRONT_STEER_CAN_ID = 4; // * - public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5; // * - public static final int LEFT_BACK_STEER_CAN_ID = 6; // * - public static final int LEFT_BACK_WHEEL_CAN_ID = 7; // * - public static final int RIGHT_BACK_STEER_CAN_ID = 8; // * - public static final int RIGHT_BACK_WHEEL_CAN_ID = 9; // * - public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10; // * - public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11; // * - public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12; // * - public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13; // * + public static final int LEFT_FRONT_STEER_CAN_ID = 2; + public static final int LEFT_FRONT_WHEEL_CAN_ID = 3; + public static final int RIGHT_FRONT_STEER_CAN_ID = 4; + public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5; + public static final int LEFT_BACK_STEER_CAN_ID = 6; + public static final int LEFT_BACK_WHEEL_CAN_ID = 7; + public static final int RIGHT_BACK_STEER_CAN_ID = 8; + public static final int RIGHT_BACK_WHEEL_CAN_ID = 9; + public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10; + public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11; + public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12; + public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13; public static final int GYRO_ID = 14; // offsets are in degrees @@ -85,8 +85,8 @@ public final class Constants { // swerve auto constants public static final PIDController X_CONTROLLER = new PIDController(0.5, 0.0, 0.0); public static final PIDController Y_CONTROLLER = new PIDController(2.0, 0.0, 0.0); - public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(15.0, 0.1, 0.3, - new TrapezoidProfile.Constraints(Math.PI, Math.PI)); + public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(2.0, 0, 0.01,//0.1, 0.3, + new TrapezoidProfile.Constraints(Math.PI, Math.PI/2)); public static final boolean PATH_RECORD_VELOCITY = true; public static final double PATH_MAX_VELOCITY = 5.0; public static final double PATH_MAX_ACCELERATION = 5.0; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 9929191..80d5759 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -213,8 +213,7 @@ public class Robot extends TimedRobot { * This function is called periodically during autonomous. */ @Override - public void autonomousPeriodic() { - } + public void autonomousPeriodic() {} @Override public void teleopInit() { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 165550a..55bbf1f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -46,6 +46,8 @@ import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.math.trajectory.Trajectory.State; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.Filesystem; @@ -59,6 +61,7 @@ import edu.wpi.first.wpilibj2.command.NotifierCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.subsystems.Claws; @@ -71,7 +74,8 @@ import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch; import frc4388.robot.commands.ClimberCommands.RunClaw; import frc4388.robot.commands.ClimberCommands.RunClimberPath; -// import frc4388.robot.commands.ButtonBoxCommands.TurretManual; +import frc4388.robot.commands.DriveCommands.DriveWithInputForTime; +import frc4388.robot.commands.DriveCommands.RotateUntilTarget; import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup; import frc4388.robot.commands.ShooterCommands.AimToCenter; import frc4388.robot.commands.ShooterCommands.Seek; @@ -97,6 +101,7 @@ import frc4388.utility.PathPlannerUtil; import frc4388.utility.Vector2D; import frc4388.utility.PathPlannerUtil.Path.Waypoint; import frc4388.utility.controller.ButtonBox; +import frc4388.utility.controller.DeadbandedRawXboxController; import frc4388.utility.controller.DeadbandedXboxController; /** @@ -128,8 +133,8 @@ public class RobotContainer { public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); // Controllers - private final static XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); - private final static XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); + private final static XboxController m_driverXbox = new DeadbandedRawXboxController(OIConstants.XBOX_DRIVER_ID); + private final static XboxController m_operatorXbox = new DeadbandedRawXboxController(OIConstants.XBOX_OPERATOR_ID); private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTON_BOX_ID); // Autonomous @@ -371,8 +376,11 @@ public class RobotContainer { if (inputs[i] instanceof String) { PathPlannerTrajectory traj = PathPlanner.loadPath(inputs[i].toString(), maxVel, maxAccel); - PathPlannerState initState = (PathPlannerState) traj.sample(0); - commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation)))); + PathPlannerState initState = traj.getInitialState(); + + Pose2d initPose = new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation); + + commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initPose), m_robotSwerveDrive)); commands.add(new PPSwerveControllerCommand( traj, m_robotSwerveDrive::getOdometry, @@ -383,12 +391,13 @@ public class RobotContainer { m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive)); } + if (inputs[i] instanceof Command) { commands.add((Command) inputs[i]); } } - commands.add(new InstantCommand(() -> m_robotSwerveDrive.stopModules())); + commands.add(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0, 0, 0, 0, true), m_robotSwerveDrive)); Command[] ret = new Command[commands.size()]; ret = commands.toArray(ret); return ret; @@ -421,6 +430,13 @@ public class RobotContainer { // }).withName("No Autonomous Path"); // } + PIDController xController = SwerveDriveConstants.X_CONTROLLER; + PIDController yController = SwerveDriveConstants.Y_CONTROLLER; + ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER; + // thetaController.enableContinuousInput(-Math.PI, Math.PI); + + loadPath("Move Forward"); + // ! this will run each of the specified PathPlanner paths in sequence. // * return new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3")); @@ -429,8 +445,16 @@ public class RobotContainer { // * null, // * new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3")), // * new RunCommand(() -> m_robotIntake.runAtOutput(0.5)))); + + // return new SequentialCommandGroup(buildAuto(1.0, 1.0, new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset(), m_robotSwerveDrive), + // // new InstantCommand(() -> this.resetOdometry(new Pose2d())), + // new InstantCommand(() -> m_robotSwerveDrive.setModuleRotationsToAngle(0.0), m_robotSwerveDrive), + // "Diamond")); - return new SequentialCommandGroup(buildAuto(1.0, 1.0, new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()), "Move Forward")); // test command + return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), + new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0), + new RotateUntilTarget(m_robotSwerveDrive, m_robotVisionOdometry, 0.5), + new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); } public static XboxController getDriverController() { @@ -563,7 +587,7 @@ public class RobotContainer { } private void saveRecording() { - // IMPORTANT: Had to chown the pathplanner folder in order to save autos. + // ! IMPORTANT: Had to chown the pathplanner folder in order to save autos. File outputFile = PATHPLANNER_DIRECTORY .resolve(ZonedDateTime.now(SYSTEM_CLOCK).format(RECORDING_FILE_NAME_FORMATTER)).toFile(); LOGGER.log(Level.WARNING, "Creating path {0}.", outputFile.getPath()); @@ -584,8 +608,7 @@ public class RobotContainer { public void recordPeriodic() { Translation2d position = m_robotSwerveDrive.m_poseEstimator.getEstimatedPosition().getTranslation(); Rotation2d rotation = m_robotSwerveDrive.m_gyro.getRotation2d(); - // FIXME: Chassis speeds are created from joystick inputs and do not reflect - // actual robot velocity. + // * FIXME: Chassis speeds are created from joystick inputs and do not reflect actual robot velocity Translation2d velocity = new Translation2d(m_robotSwerveDrive.getChassisSpeeds()[0], m_robotSwerveDrive.getChassisSpeeds()[1]); Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false, diff --git a/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java b/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java new file mode 100644 index 0000000..4a04a22 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java @@ -0,0 +1,59 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands.DriveCommands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.SwerveDrive; + +public class DriveWithInputForTime extends CommandBase { + + // subsystems + SwerveDrive swerve; + + double[] inputs; + + // timing + long start; + long elapsed; + double duration; + + /** + * DriveWithInput for a specified amount of time. + * @param inputs Inputs used in DriveWithInput (xspeed, yspeed, xrot, yrot). + * @param time Time to DriveWithInput for, in seconds. + */ + public DriveWithInputForTime(SwerveDrive swerve, double[] inputs, double duration) { + // Use addRequirements() here to declare subsystem dependencies. + + this.swerve = swerve; + this.inputs = inputs; + this.duration = duration; + + addRequirements(this.swerve); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + start = System.currentTimeMillis(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + elapsed = System.currentTimeMillis() - start; + this.swerve.driveWithInput(inputs[0], inputs[1], inputs[2], inputs[3], true); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return (((long) duration) >= (start - elapsed)); + } +} diff --git a/src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java b/src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java new file mode 100644 index 0000000..76ec283 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java @@ -0,0 +1,52 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands.DriveCommands; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.VisionOdometry; + +public class RotateUntilTarget extends CommandBase { + + // subsystems + SwerveDrive swerve; + VisionOdometry visionOdometry; + + double rotateSpeed; + + /** Creates a new RotateUntilTarget. */ + public RotateUntilTarget(SwerveDrive swerve, VisionOdometry visionOdometry, double rotateSpeed) { + // Use addRequirements() here to declare subsystem dependencies. + + this.swerve = swerve; + this.visionOdometry = visionOdometry; + + this.rotateSpeed = rotateSpeed; + + addRequirements(this.swerve, this.visionOdometry); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + this.visionOdometry.setLEDs(true); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + this.swerve.driveWithInput(0.0, 0.0, rotateSpeed, true); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return this.visionOdometry.m_camera.getLatestResult().hasTargets(); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index ed9c193..f913873 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -158,6 +158,13 @@ public class SwerveDrive extends SubsystemBase { // modules[0].setDesiredState(desiredStates[0], false); } + public void setModuleRotationsToAngle(double angle) { + for (int i = 0; i < modules.length; i++) { + SwerveModule module = modules[i]; + module.rotateToAngle(angle); + } + } + @Override public void periodic() { @@ -228,10 +235,16 @@ public class SwerveDrive extends SubsystemBase { * Updates the field relative position of the robot. */ public void updateOdometry() { - m_poseEstimator.update( getRegGyro(), - modules[0].getState(), - modules[1].getState(), - modules[2].getState(), + Rotation2d actualDWI = new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI/2)); + Rotation2d actual = new Rotation2d(-1 * m_gyro.getRotation2d().getRadians()); + + SmartDashboard.putNumber("AUTO ACTUAL GYRO", actual.getDegrees()); + SmartDashboard.putNumber("AUTO DWI GYRO", actual.getDegrees()); + + m_poseEstimator.update( actualDWI,//new Rotation2d((2 * Math.PI) - getRegGyro().getRadians()), + modules[0].getState(), + modules[1].getState(), + modules[2].getState(), modules[3].getState()); } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index d4905af..59e9e13 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -182,6 +182,10 @@ public class SwerveModule extends SubsystemBase { angleMotor.set(0); } + public void rotateToAngle(double angle) { + this.angleMotor.set(TalonFXControlMode.Position, angle); + } + @Override public void periodic() {