From 74e56655bf539e0c41555eb9f0fd8b6ae8054f3b Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 9 Feb 2023 18:25:57 -0700 Subject: [PATCH] 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);