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

Merge pull request #31 from Team4388/autonomous

Browse Source 
Autonomous --> Master
This commit is contained in:
Aarav Shah
2023-02-17 20:04:12 -07:00
committed by GitHub
parent 3dc311d225 768d345fa8
commit 84ed4c0e40
6 changed files with 189 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+8 -8
View File
@@ -47,18 +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 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.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 = -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 = 0.3; // TODO: find the actual value
public static final double PATH_MAX_ACC = 0.3; // TODO: find the actual value
}
public static final class Conversions {
@@ -106,6 +105,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;
src/main/java/frc4388/robot/RobotContainer.java
+88 -3
View File
@@ -7,11 +7,32 @@
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;
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.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;
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.commands.JoystickPlayback;
import frc4388.robot.commands.JoystickRecorder;
@@ -68,8 +89,9 @@ 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)
.onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
@@ -94,7 +116,70 @@ public class RobotContainer {
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
return new InstantCommand();
//Create Trajectory Settings
TrajectoryConfig trajectoryConfig = new TrajectoryConfig(SwerveDriveConstants.AutoConstants.PATH_MAX_VEL,
SwerveDriveConstants.AutoConstants.PATH_MAX_ACC).setKinematics(m_robotSwerveDrive.getKinematics());
// 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<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(
// new Pose2d(0, 0, new Rotation2d(0),
// List.of(new Translation2d(0, 1)),
// new Pose2d(0, 2, new Rotation2d(0)),
// trajectoryConfig
// );
// ArrayList<double[]> states = new ArrayList<double[]>();
//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);
ProfiledPIDController thetaController = new ProfiledPIDController(AutoConstants.THETA_CONTROLLER.kP,
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(),
// holoController,
// m_robotSwerveDrive::setModuleStates,
// m_robotSwerveDrive);
SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(
trajectory,
m_robotSwerveDrive::getPoseEstimator,
m_robotSwerveDrive.getKinematics(),
holoController,
m_robotSwerveDrive::setModuleStates,
m_robotSwerveDrive);
//Init and wrap-up, return everything
return new SequentialCommandGroup(
new InstantCommand(() -> m_robotSwerveDrive.resetPoseEstimator(), m_robotSwerveDrive),
// new InstantCommand(() -> m_robotSwerveDrive.setOdometry(trajectory.getInitialPose())),
swerveControllerCommand,
new InstantCommand(() -> m_robotSwerveDrive.stopModules()));
}
public DeadbandedXboxController getDeadbandedDriverController() {
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+72 -26
View File
@@ -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};
@@ -68,21 +84,15 @@ 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);
// if (rightStick.getNorm() < .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));
@@ -112,15 +122,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(),
@@ -135,16 +157,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(),
@@ -156,6 +195,10 @@ public class SwerveDrive extends SubsystemBase {
);
}
public void resetPoseEstimator() {
setPoseEstimator(new Pose2d());
}
public void stopModules() {
for (SwerveModule module : this.modules) {
module.stop();
@@ -166,9 +209,9 @@ public class SwerveDrive extends SubsystemBase {
* 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;
@@ -177,14 +220,17 @@ public class SwerveDrive extends SubsystemBase {
@Override
public void periodic() {
// This method will be called once per scheduler run
updateOdometry();
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());
// 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("Gyro Angle", getGyroAngle());
// SmartDashboard.putNumber("rotTarget", this.rotTarget.getDegrees());
SmartDashboard.putNumber("Gyro Angle", getGyroAngle());
SmartDashboard.putNumber("rotTarget", this.rotTarget.getDegrees());
}
/**
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);
src/main/java/frc4388/utility/AprilTag.java
+13
View File
@@ -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;
}
}
src/main/java/frc4388/utility/Gains.java
+6
View File
@@ -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.