From 3a9b2eca5b7e0a06f55864390899220ed41dcab8 Mon Sep 17 00:00:00 2001 From: Abhi Sachi Date: Mon, 6 Feb 2023 19:49:08 -0700 Subject: [PATCH 1/8] auto incomplete --- src/main/java/frc4388/robot/Constants.java | 5 +++-- src/main/java/frc4388/robot/RobotContainer.java | 2 ++ src/main/java/frc4388/utility/AprilTag.java | 13 +++++++++++++ 3 files changed, 18 insertions(+), 2 deletions(-) create mode 100644 src/main/java/frc4388/utility/AprilTag.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 1b94ed1..e063825 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -57,8 +57,8 @@ public final class Constants { new TrapezoidProfile.Constraints(0.0, 0.0) ); - public static final double PATH_MAX_VEL = -1; // TODO: find the actual value - public static final double PATH_MAX_ACC = -1; // TODO: find the actual value + public static final double PATH_MAX_VEL = SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND / 4; // TODO: find the actual value + public static final double PATH_MAX_ACC = 3; // TODO: find the actual value } public static final class Conversions { @@ -106,6 +106,7 @@ public final class Constants { } public static final double MAX_SPEED_FEET_PER_SECOND = 5; // TODO: find the actual value + public static final double MAX_ANGULAR_SPEED_FEET_PER_SECOND = 2 * 2 * Math.PI; // TODO: find the actual value // dimensions public static final double WIDTH = 18.5; // TODO: find the actual value diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 97f5db1..c6e01c8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,6 +7,7 @@ package frc4388.robot; +import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -91,6 +92,7 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { + TrajectoryConfig trajectoryConfig = new TrajectoryConfig(SwerveDriveConstants.AutoConstants.PATH_MAX_VEL, SwerveDriveConstants.AutoConstants.PATH_MAX_ACC).setKinematics(m_robotSwerveDrive.getKinematics()); return new InstantCommand(); } diff --git a/src/main/java/frc4388/utility/AprilTag.java b/src/main/java/frc4388/utility/AprilTag.java new file mode 100644 index 0000000..c2ad269 --- /dev/null +++ b/src/main/java/frc4388/utility/AprilTag.java @@ -0,0 +1,13 @@ +package frc4388.utility; + +// This is a seperate class in case I want to encode rotation or other +// information about the tag +public class AprilTag { + public final double x, y, z; + + public AprilTag(double _x, double _y, double _z) { + x = _x; + y = _y; + z = _z; + } +} From 4bfb26a50a0a5ddf610caf343d3f93548d843ab6 Mon Sep 17 00:00:00 2001 From: Abhi Sachi Date: Tue, 7 Feb 2023 18:50:33 -0700 Subject: [PATCH 2/8] auto (not looked over) --- src/main/java/frc4388/robot/Constants.java | 12 +++-- .../java/frc4388/robot/RobotContainer.java | 46 ++++++++++++++++++- .../frc4388/robot/subsystems/SwerveDrive.java | 9 +++- src/main/java/frc4388/utility/Gains.java | 6 +++ 4 files changed, 64 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index e063825..0420217 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -51,14 +51,16 @@ public final class Constants { } public static final class AutoConstants { - public static final PIDController X_CONTROLLER = new PIDController(0.0, 0.0, 0.0); - public static final PIDController Y_CONTROLLER = new PIDController(0.0, 0.0, 0.0); - public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(0.0, 0.0, 0.0, - new TrapezoidProfile.Constraints(0.0, 0.0) - ); + public static final Gains X_CONTROLLER = new Gains(0.0, 0.0, 0.0); + public static final Gains Y_CONTROLLER = new Gains(0.0, 0.0, 0.0); + public static final Gains THETA_CONTROLLER = new Gains(0.0, 0.0, 0.0); + public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(0.0, 0.0); public static final double PATH_MAX_VEL = SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND / 4; // TODO: find the actual value public static final double PATH_MAX_ACC = 3; // TODO: find the actual value + + public static final double kPX_CONTROLLER = 1.5; //TODO: find actual value + public static final double kPY_CONTROLLER = 1.5; //TODO: find actual value } public static final class Conversions { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c6e01c8..08f209f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,13 +7,26 @@ package frc4388.robot; +import java.util.List; + +import edu.wpi.first.math.controller.PIDController; +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.TrajectoryConfig; +import edu.wpi.first.math.trajectory.TrajectoryGenerator; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; 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.Constants.SwerveDriveConstants.AutoConstants; +import frc4388.robot.Constants.SwerveDriveConstants.PIDConstants; import frc4388.robot.commands.AutoBalance; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.controller.DeadbandedXboxController; @@ -92,9 +105,38 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - TrajectoryConfig trajectoryConfig = new TrajectoryConfig(SwerveDriveConstants.AutoConstants.PATH_MAX_VEL, SwerveDriveConstants.AutoConstants.PATH_MAX_ACC).setKinematics(m_robotSwerveDrive.getKinematics()); + //Create Trajectory Settings + TrajectoryConfig trajectoryConfig = new TrajectoryConfig(SwerveDriveConstants.AutoConstants.PATH_MAX_VEL, + SwerveDriveConstants.AutoConstants.PATH_MAX_ACC).setKinematics(m_robotSwerveDrive.getKinematics()); + //Generate Trajactory + Trajectory trajectory = TrajectoryGenerator.generateTrajectory(new Pose2d(0, 0, new Rotation2d(0)), + List.of(new Translation2d(1, 0), new Translation2d(1, -1)), + new Pose2d(0, 0, Rotation2d.fromDegrees(180)), + trajectoryConfig); - return new InstantCommand(); + //Defining PID Controller for tracking trajectory + PIDController xController = new PIDController(SwerveDriveConstants.AutoConstants.kPX_CONTROLLER, 0, 0); + PIDController yController = new PIDController(SwerveDriveConstants.AutoConstants.kPY_CONTROLLER, 0, 0); + ProfiledPIDController thetaController = new ProfiledPIDController(AutoConstants.THETA_CONTROLLER.kP, + AutoConstants.THETA_CONTROLLER.kI, AutoConstants.THETA_CONTROLLER.kD, AutoConstants.THETA_CONSTRAINTS, 0); + thetaController.enableContinuousInput(-Math.PI, Math.PI); + + //Command to follow trajectory + SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand( + trajectory, + m_robotSwerveDrive::getOdometry, + m_robotSwerveDrive.getKinematics(), + xController, + yController, + thetaController, + m_robotSwerveDrive::setModuleStates, + m_robotSwerveDrive); + + //Init and wrap-up, return everything + return new SequentialCommandGroup( + new InstantCommand(() -> m_robotSwerveDrive.setOdometry(trajectory.getInitialPose())), + swerveControllerCommand, + new InstantCommand(() -> m_robotSwerveDrive.stopModules())); } /** diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 38e7415..6cf7ba8 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -68,14 +68,12 @@ public class SwerveDrive extends SubsystemBase { public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { if (fieldRelative) { - if (rightStick.getNorm() > 0.1) { rotTarget = new Rotation2d(rightStick.getX(), -rightStick.getY()).minus(new Rotation2d(0, 1)); } double rot = rotTarget.minus(gyro.getRotation2d()).getRadians(); - // Use the left joystick to set speed. Apply a quadratic curve and the set max speed. Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); @@ -105,6 +103,13 @@ public class SwerveDrive extends SubsystemBase { } } + public void stopModules() { + leftFront.stop(); + rightFront.stop(); + leftBack.stop(); + rightBack.stop(); + } + public double getGyroAngle() { return gyro.getAngle(); } diff --git a/src/main/java/frc4388/utility/Gains.java b/src/main/java/frc4388/utility/Gains.java index 7cda1e0..7a3a026 100644 --- a/src/main/java/frc4388/utility/Gains.java +++ b/src/main/java/frc4388/utility/Gains.java @@ -54,6 +54,12 @@ public class Gains { this.kMinOutput = -1.0; } + public Gains(double kP, double kI, double kD) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + } + /** * Creates Gains object for PIDs * @param kP The P value. From 4900c397c4506f3dd3a603da0b8339b17306862f Mon Sep 17 00:00:00 2001 From: aarav18 Date: Tue, 7 Feb 2023 23:32:19 -0700 Subject: [PATCH 3/8] some auto work (not working at all) --- src/main/java/frc4388/robot/Constants.java | 16 +++++------ .../java/frc4388/robot/RobotContainer.java | 28 ++++++++++++++----- .../frc4388/robot/subsystems/SwerveDrive.java | 10 +++---- 3 files changed, 34 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 0420217..01f10e6 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -51,16 +51,16 @@ public final class Constants { } public static final class AutoConstants { - public static final Gains X_CONTROLLER = new Gains(0.0, 0.0, 0.0); - public static final Gains Y_CONTROLLER = new Gains(0.0, 0.0, 0.0); - public static final Gains THETA_CONTROLLER = new Gains(0.0, 0.0, 0.0); - public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(0.0, 0.0); + public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0); + public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0); + public static final Gains THETA_CONTROLLER = new Gains(0.8, 0.0, 0.0); + public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune - public static final double PATH_MAX_VEL = SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND / 4; // TODO: find the actual value - public static final double PATH_MAX_ACC = 3; // TODO: find the actual value + public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value + public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value - public static final double kPX_CONTROLLER = 1.5; //TODO: find actual value - public static final double kPY_CONTROLLER = 1.5; //TODO: find actual value + // public static final double kPX_CONTROLLER = 0.8; //TODO: tune + // public static final double kPY_CONTROLLER = 0.8; //TODO: tune } public static final class Conversions { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 08f209f..37158ad 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -7,6 +7,7 @@ package frc4388.robot; +import java.util.ArrayList; import java.util.List; import edu.wpi.first.math.controller.PIDController; @@ -17,6 +18,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.trajectory.Trajectory; import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; +import edu.wpi.first.math.trajectory.Trajectory.State; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -105,20 +107,32 @@ public class RobotContainer { * @return the command to run in autonomous */ public Command getAutonomousCommand() { + //Create Trajectory Settings TrajectoryConfig trajectoryConfig = new TrajectoryConfig(SwerveDriveConstants.AutoConstants.PATH_MAX_VEL, SwerveDriveConstants.AutoConstants.PATH_MAX_ACC).setKinematics(m_robotSwerveDrive.getKinematics()); - //Generate Trajactory - Trajectory trajectory = TrajectoryGenerator.generateTrajectory(new Pose2d(0, 0, new Rotation2d(0)), - List.of(new Translation2d(1, 0), new Translation2d(1, -1)), - new Pose2d(0, 0, Rotation2d.fromDegrees(180)), + + // generate trajectory + Trajectory trajectory = TrajectoryGenerator.generateTrajectory( + + new Pose2d(0, 0, new Rotation2d(0)), + List.of( + new Translation2d(0, 1)), + new Pose2d(0, 2, Rotation2d.fromDegrees(0)), + trajectoryConfig); + // ArrayList simplePath = new ArrayList(); + // simplePath.add(new Pose2d(0, 0, new Rotation2d(0))); + // simplePath.add(new Pose2d(0, 1, new Rotation2d(0))); + + // Trajectory trajectory = TrajectoryGenerator.generateTrajectory(simplePath, trajectoryConfig); + //Defining PID Controller for tracking trajectory - PIDController xController = new PIDController(SwerveDriveConstants.AutoConstants.kPX_CONTROLLER, 0, 0); - PIDController yController = new PIDController(SwerveDriveConstants.AutoConstants.kPY_CONTROLLER, 0, 0); + PIDController xController = new PIDController(SwerveDriveConstants.AutoConstants.X_CONTROLLER.kP, 0, 0); + PIDController yController = new PIDController(SwerveDriveConstants.AutoConstants.Y_CONTROLLER.kP, 0, 0); ProfiledPIDController thetaController = new ProfiledPIDController(AutoConstants.THETA_CONTROLLER.kP, - AutoConstants.THETA_CONTROLLER.kI, AutoConstants.THETA_CONTROLLER.kD, AutoConstants.THETA_CONSTRAINTS, 0); + AutoConstants.THETA_CONTROLLER.kI, AutoConstants.THETA_CONTROLLER.kD, AutoConstants.THETA_CONSTRAINTS); thetaController.enableContinuousInput(-Math.PI, Math.PI); //Command to follow trajectory diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 6cf7ba8..5790496 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -77,13 +77,10 @@ public class SwerveDrive extends SubsystemBase { // Use the left joystick to set speed. Apply a quadratic curve and the set max speed. Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); - if (rightStick.getNorm() < 0.1) { - rot = 0; - } - // Convert field-relative speeds to robot-relative speeds. chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1)); + } else { // Create robot-relative speeds. chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); } @@ -178,7 +175,10 @@ public class SwerveDrive extends SubsystemBase { updateOdometry(); // SmartDashboard.putNumberArray("Odometry", new double[] {getOdometry().getX(), getOdometry().getY(), getOdometry().getRotation().getDegrees()}); - + SmartDashboard.putNumber("Odo X", getOdometry().getX()); + SmartDashboard.putNumber("Odo Y", getOdometry().getY()); + SmartDashboard.putNumber("Odo Theta", getOdometry().getRotation().getDegrees()); + SmartDashboard.putNumber("Gyro Angle", getGyroAngle()); } From 5a8af1d071cd52b73d9b085fc698fdf260e5a93e Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 9 Feb 2023 18:25:57 -0700 Subject: [PATCH 4/8] cummit --- src/main/java/frc4388/robot/Constants.java | 7 ++---- .../java/frc4388/robot/RobotContainer.java | 24 ++++++++++--------- .../robot/subsystems/SwerveModule.java | 4 ++-- 3 files changed, 17 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 01f10e6..fbb267e 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -47,20 +47,17 @@ public final class Constants { public static final class PIDConstants { public static final int SWERVE_SLOT_IDX = 0; public static final int SWERVE_PID_LOOP_IDX = 1; - public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SWERVE_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 1.0); } public static final class AutoConstants { public static final Gains X_CONTROLLER = new Gains(0.8, 0.0, 0.0); public static final Gains Y_CONTROLLER = new Gains(0.8, 0.0, 0.0); - public static final Gains THETA_CONTROLLER = new Gains(0.8, 0.0, 0.0); + public static final Gains THETA_CONTROLLER = new Gains(-0.8, 0.0, 0.0); public static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints(Math.PI/2, Math.PI/2); // TODO: tune public static final double PATH_MAX_VEL = 0.3; // TODO: find the actual value public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value - - // public static final double kPX_CONTROLLER = 0.8; //TODO: tune - // public static final double kPY_CONTROLLER = 0.8; //TODO: tune } public static final class Conversions { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 37158ad..364b3cc 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -10,6 +10,7 @@ package frc4388.robot; import java.util.ArrayList; import java.util.List; +import edu.wpi.first.math.controller.HolonomicDriveController; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; @@ -113,20 +114,20 @@ public class RobotContainer { SwerveDriveConstants.AutoConstants.PATH_MAX_ACC).setKinematics(m_robotSwerveDrive.getKinematics()); // generate trajectory - Trajectory trajectory = TrajectoryGenerator.generateTrajectory( + // Trajectory trajectory = TrajectoryGenerator.generateTrajectory( - new Pose2d(0, 0, new Rotation2d(0)), - List.of( - new Translation2d(0, 1)), - new Pose2d(0, 2, Rotation2d.fromDegrees(0)), + // new Pose2d(0, 0, new Rotation2d(0)), + // List.of( + // new Translation2d(0, 1)), + // new Pose2d(0, 2, Rotation2d.fromDegrees(0)), - trajectoryConfig); + // trajectoryConfig); - // ArrayList simplePath = new ArrayList(); - // simplePath.add(new Pose2d(0, 0, new Rotation2d(0))); - // simplePath.add(new Pose2d(0, 1, new Rotation2d(0))); + ArrayList simplePath = new ArrayList(); + simplePath.add(new Pose2d(0, 0, new Rotation2d(0))); + simplePath.add(new Pose2d(0, 1, new Rotation2d(0))); - // Trajectory trajectory = TrajectoryGenerator.generateTrajectory(simplePath, trajectoryConfig); + Trajectory trajectory = TrajectoryGenerator.generateTrajectory(simplePath, trajectoryConfig); //Defining PID Controller for tracking trajectory PIDController xController = new PIDController(SwerveDriveConstants.AutoConstants.X_CONTROLLER.kP, 0, 0); @@ -148,7 +149,8 @@ public class RobotContainer { //Init and wrap-up, return everything return new SequentialCommandGroup( - new InstantCommand(() -> m_robotSwerveDrive.setOdometry(trajectory.getInitialPose())), + new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive), + // new InstantCommand(() -> m_robotSwerveDrive.setOdometry(trajectory.getInitialPose())), swerveControllerCommand, new InstantCommand(() -> m_robotSwerveDrive.stopModules())); } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 2616b71..a88fca7 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -132,13 +132,13 @@ public class SwerveModule extends SubsystemBase { SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation); // calculate the difference between our current rotational position and our new rotational position - Rotation2d rotationDelta = state.angle.minus(currentRotation); // ? might need to be negative + Rotation2d rotationDelta = state.angle.minus(currentRotation); // calculate the new absolute position of the SwerveModule based on the difference in rotation double deltaTicks = (rotationDelta.getDegrees() / 360.) * SwerveDriveConstants.Conversions.CANCODER_TICKS_PER_ROTATION; // convert the CANCoder from its position reading to ticks - double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); // ? why feedback coefficient + double currentTicks = encoder.getPosition() / encoder.configGetFeedbackCoefficient(); if (!ignoreAngle) { angleMotor.set(TalonFXControlMode.Position, currentTicks + deltaTicks); From d80b4faccbbd7d3d83ef34641fb87029044684e3 Mon Sep 17 00:00:00 2001 From: Abhi Sachi Date: Thu, 9 Feb 2023 18:48:43 -0700 Subject: [PATCH 5/8] auto incomplete --- .../java/frc4388/robot/RobotContainer.java | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 364b3cc..429f3f3 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -10,6 +10,8 @@ package frc4388.robot; import java.util.ArrayList; import java.util.List; +import org.opencv.objdetect.HOGDescriptor; + import edu.wpi.first.math.controller.HolonomicDriveController; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; @@ -21,6 +23,7 @@ import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; import edu.wpi.first.math.trajectory.Trajectory.State; import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; @@ -125,10 +128,17 @@ public class RobotContainer { ArrayList simplePath = new ArrayList(); simplePath.add(new Pose2d(0, 0, new Rotation2d(0))); - simplePath.add(new Pose2d(0, 1, new Rotation2d(0))); + simplePath.add(new Pose2d(0, -1, new Rotation2d(0))); Trajectory trajectory = TrajectoryGenerator.generateTrajectory(simplePath, trajectoryConfig); + ArrayList states = new ArrayList(); + + for(double i = 0; i <=5.0; i += 0.1) { + Trajectory.State state = trajectory.sample(i); + states.add(new double[] {state.velocityMetersPerSecond, state.curvatureRadPerMeter}); + } + //Defining PID Controller for tracking trajectory PIDController xController = new PIDController(SwerveDriveConstants.AutoConstants.X_CONTROLLER.kP, 0, 0); PIDController yController = new PIDController(SwerveDriveConstants.AutoConstants.Y_CONTROLLER.kP, 0, 0); @@ -136,14 +146,14 @@ public class RobotContainer { AutoConstants.THETA_CONTROLLER.kI, AutoConstants.THETA_CONTROLLER.kD, AutoConstants.THETA_CONSTRAINTS); thetaController.enableContinuousInput(-Math.PI, Math.PI); + HolonomicDriveController holoController = new HolonomicDriveController(xController, yController, thetaController); + //Command to follow trajectory SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand( trajectory, m_robotSwerveDrive::getOdometry, m_robotSwerveDrive.getKinematics(), - xController, - yController, - thetaController, + holoController, m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive); From e7af2c928190d4f5c27593ade3e67e4c68c1d2e9 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 9 Feb 2023 19:34:59 -0700 Subject: [PATCH 6/8] Update RobotContainer.java --- .../java/frc4388/robot/RobotContainer.java | 28 +++++++++++-------- 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 429f3f3..4058926 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -117,27 +117,31 @@ public class RobotContainer { SwerveDriveConstants.AutoConstants.PATH_MAX_ACC).setKinematics(m_robotSwerveDrive.getKinematics()); // generate trajectory - // Trajectory trajectory = TrajectoryGenerator.generateTrajectory( + Trajectory trajectory = TrajectoryGenerator.generateTrajectory( - // new Pose2d(0, 0, new Rotation2d(0)), - // List.of( - // new Translation2d(0, 1)), - // new Pose2d(0, 2, Rotation2d.fromDegrees(0)), + new Pose2d(0, 0, new Rotation2d(0)), + List.of( + new Translation2d(0, 1)), + new Pose2d(0, 2, Rotation2d.fromDegrees(0)), - // trajectoryConfig); + trajectoryConfig); ArrayList simplePath = new ArrayList(); simplePath.add(new Pose2d(0, 0, new Rotation2d(0))); simplePath.add(new Pose2d(0, -1, new Rotation2d(0))); - Trajectory trajectory = TrajectoryGenerator.generateTrajectory(simplePath, trajectoryConfig); + // Trajectory trajectory = TrajectoryGenerator.generateTrajectory(simplePath, trajectoryConfig); + // Trajectory trajectory = TrajectoryGenerator.generateTrajectory( + // new Pose2d(0, 0, new Rotation2d(0), + // List.of(new Translation2d(0, 1)), + // new Pose2d(0, 2, new Rotation2d(0)), + // trajectoryConfig + // ); + + + // ArrayList states = new ArrayList(); - ArrayList states = new ArrayList(); - for(double i = 0; i <=5.0; i += 0.1) { - Trajectory.State state = trajectory.sample(i); - states.add(new double[] {state.velocityMetersPerSecond, state.curvatureRadPerMeter}); - } //Defining PID Controller for tracking trajectory PIDController xController = new PIDController(SwerveDriveConstants.AutoConstants.X_CONTROLLER.kP, 0, 0); From 7c57dd53e8eceace2c400475e5a1ed91845e5fdc Mon Sep 17 00:00:00 2001 From: Abhi <90010729+Abhishrek05@users.noreply.github.com> Date: Fri, 10 Feb 2023 17:12:53 -0700 Subject: [PATCH 7/8] added pose estimator --- .../java/frc4388/robot/RobotContainer.java | 16 +++- .../frc4388/robot/subsystems/SwerveDrive.java | 89 ++++++++++++++----- 2 files changed, 81 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 4058926..14969a8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -92,8 +92,8 @@ public class RobotContainer { new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); - new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive)); + // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive)); // // .onFalse() new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) @@ -153,9 +153,17 @@ public class RobotContainer { HolonomicDriveController holoController = new HolonomicDriveController(xController, yController, thetaController); //Command to follow trajectory + // SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand( + // trajectory, + // m_robotSwerveDrive::getOdometry, + // m_robotSwerveDrive.getKinematics(), + // holoController, + // m_robotSwerveDrive::setModuleStates, + // m_robotSwerveDrive); + SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand( trajectory, - m_robotSwerveDrive::getOdometry, + m_robotSwerveDrive::getPoseEstimator, m_robotSwerveDrive.getKinematics(), holoController, m_robotSwerveDrive::setModuleStates, @@ -163,7 +171,7 @@ public class RobotContainer { //Init and wrap-up, return everything return new SequentialCommandGroup( - new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive), + new InstantCommand(() -> m_robotSwerveDrive.resetPoseEstimator(), m_robotSwerveDrive), // new InstantCommand(() -> m_robotSwerveDrive.setOdometry(trajectory.getInitialPose())), swerveControllerCommand, new InstantCommand(() -> m_robotSwerveDrive.stopModules())); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 5790496..c174c29 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -5,6 +5,7 @@ package frc4388.robot.subsystems; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -37,7 +38,9 @@ public class SwerveDrive extends SubsystemBase { private RobotGyro gyro; - private SwerveDriveOdometry odometry; + // private SwerveDriveOdometry odometry; + + private SwerveDrivePoseEstimator poseEstimator; public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default public Rotation2d rotTarget = new Rotation2d(); @@ -51,15 +54,28 @@ public class SwerveDrive extends SubsystemBase { this.rightBack = rightBack; this.gyro = gyro; - this.odometry = new SwerveDriveOdometry( + // this.odometry = new SwerveDriveOdometry( + // kinematics, + // gyro.getRotation2d(), + // new SwerveModulePosition[] { + // leftFront.getPosition(), + // rightFront.getPosition(), + // leftBack.getPosition(), + // rightBack.getPosition() + // }, + // getOdometry() + // ); + + this.poseEstimator = new SwerveDrivePoseEstimator( kinematics, - gyro.getRotation2d(), + gyro.getRotation2d(), new SwerveModulePosition[] { leftFront.getPosition(), rightFront.getPosition(), leftBack.getPosition(), rightBack.getPosition() - } + }, + new Pose2d(0,0, new Rotation2d(0)) ); this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; @@ -113,15 +129,27 @@ public class SwerveDrive extends SubsystemBase { public void resetGyro() { gyro.reset(); - setOdometry(getOdometry()); + // setOdometry(getOdometry()); rotTarget = new Rotation2d(0); } /** * Updates the odometry of the SwerveDrive. */ - public void updateOdometry() { - odometry.update( + // public void updateOdometry() { + // odometry.update( + // gyro.getRotation2d(), + // new SwerveModulePosition[] { + // leftFront.getPosition(), + // rightFront.getPosition(), + // leftBack.getPosition(), + // rightBack.getPosition() + // } + // ); + // } + + public void updatePoseEstimator() { + poseEstimator.update( gyro.getRotation2d(), new SwerveModulePosition[] { leftFront.getPosition(), @@ -136,16 +164,33 @@ public class SwerveDrive extends SubsystemBase { * Gets the odometry of the SwerveDrive. * @return The odometry of the SwerveDrive as a Pose2d object (xMeters, yMeters, theta). */ - public Pose2d getOdometry() { - return odometry.getPoseMeters(); + // public Pose2d getOdometry() { + // return odometry.getPoseMeters(); + // } + + public Pose2d getPoseEstimator() { + return poseEstimator.getEstimatedPosition(); } /** * Sets the odometry of the SwerveDrive. * @param pose Pose to set the odometry to. */ - public void setOdometry(Pose2d pose) { - odometry.resetPosition( + // public void setOdometry(Pose2d pose) { + // odometry.resetPosition( + // gyro.getRotation2d(), + // new SwerveModulePosition[] { + // leftFront.getPosition(), + // rightFront.getPosition(), + // leftBack.getPosition(), + // rightBack.getPosition() + // }, + // pose + // ); + // } + + public void setPoseEstimator(Pose2d pose) { + poseEstimator.resetPosition( gyro.getRotation2d(), new SwerveModulePosition[] { leftFront.getPosition(), @@ -157,13 +202,17 @@ public class SwerveDrive extends SubsystemBase { ); } + public void resetPoseEstimator() { + setPoseEstimator(new Pose2d()); + } + /** * Resets the odometry of the SwerveDrive to 0. * *NOTE: If you reset your gyro, this method MUST be called with the new gyro angle and wheel encoder positions. */ - public void resetOdometry() { - setOdometry(new Pose2d()); - } + // public void resetOdometry() { + // setOdometry(new Pose2d()); + // } public SwerveDriveKinematics getKinematics() { return this.kinematics; @@ -172,14 +221,14 @@ public class SwerveDrive extends SubsystemBase { @Override public void periodic() { // This method will be called once per scheduler run - updateOdometry(); + // updateOdometry(); + updatePoseEstimator(); - // SmartDashboard.putNumberArray("Odometry", new double[] {getOdometry().getX(), getOdometry().getY(), getOdometry().getRotation().getDegrees()}); - SmartDashboard.putNumber("Odo X", getOdometry().getX()); - SmartDashboard.putNumber("Odo Y", getOdometry().getY()); - SmartDashboard.putNumber("Odo Theta", getOdometry().getRotation().getDegrees()); + // SmartDashboard.putNumber("Odo X", getOdometry().getX()); + // SmartDashboard.putNumber("Odo Y", getOdometry().getY()); + // SmartDashboard.putNumber("Odo Theta", getOdometry().getRotation().getDegrees()); - SmartDashboard.putNumber("Gyro Angle", getGyroAngle()); + // SmartDashboard.putNumber("Gyro Angle", getGyroAngle()); } /** From 49107c6e143efd1fd40e0c543bd0011df71ed70f Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 17 Feb 2023 20:00:17 -0700 Subject: [PATCH 8/8] fix build issue --- .../frc4388/robot/subsystems/SwerveDrive.java | 17 +++++------------ 1 file changed, 5 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index ddd5300..0b13b6c 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -116,13 +116,6 @@ public class SwerveDrive extends SubsystemBase { } } - public void stopModules() { - leftFront.stop(); - rightFront.stop(); - leftBack.stop(); - rightBack.stop(); - } - public double getGyroAngle() { return gyro.getAngle(); } @@ -231,12 +224,12 @@ public class SwerveDrive extends SubsystemBase { // updateOdometry(); updatePoseEstimator(); - SmartDashboard.putNumber("Odo X (ft)", Units.metersToFeet(this.getOdometry().getX())); - SmartDashboard.putNumber("Odo Y (ft)", Units.metersToFeet(this.getOdometry().getY())); - SmartDashboard.putNumber("Odo Theta", this.getOdometry().getRotation().getDegrees()); + // SmartDashboard.putNumber("Odo X (ft)", Units.metersToFeet(this.getOdometry().getX())); + // SmartDashboard.putNumber("Odo Y (ft)", Units.metersToFeet(this.getOdometry().getY())); + // SmartDashboard.putNumber("Odo Theta", this.getOdometry().getRotation().getDegrees()); - SmartDashboard.putNumber("Gyro Angle", getGyroAngle()); - SmartDashboard.putNumber("rotTarget", this.rotTarget.getDegrees()); + // SmartDashboard.putNumber("Gyro Angle", getGyroAngle()); + // SmartDashboard.putNumber("rotTarget", this.rotTarget.getDegrees()); }