Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-09 00:37:59 -06:00
Code Issues Packages Projects Releases Wiki Activity

cummit

Browse Source
This commit is contained in:
aarav18
2023-02-09 18:25:57 -07:00
parent 4900c397c4
commit 5a8af1d071
3 changed files with 17 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+2 -5
View File
@@ -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 {
src/main/java/frc4388/robot/RobotContainer.java
+13 -11
View File
@@ -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<Pose2d> simplePath = new ArrayList<Pose2d>();
// simplePath.add(new Pose2d(0, 0, new Rotation2d(0)));
// simplePath.add(new Pose2d(0, 1, new Rotation2d(0)));
ArrayList<Pose2d> simplePath = new ArrayList<Pose2d>();
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()));
}
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+2 -2
View File
@@ -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);