From 4bfb26a50a0a5ddf610caf343d3f93548d843ab6 Mon Sep 17 00:00:00 2001 From: Abhi Sachi Date: Tue, 7 Feb 2023 18:50:33 -0700 Subject: [PATCH] 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.