Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 00:38:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

Autonomous

Browse Source
This commit is contained in:
aarav18
2022-01-29 14:39:46 -07:00
parent f7c1519c91
commit ce6d43878d
5 changed files with 105 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+51 -5
View File
@@ -5,11 +5,24 @@
package frc4388.robot;
import com.pathplanner.lib.PathPlanner;
import com.pathplanner.lib.PathPlannerTrajectory;
import com.pathplanner.lib.commands.PPSwerveControllerCommand;
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.trajectory.Trajectory;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
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.RamseteCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.*;
@@ -83,7 +96,7 @@ public class RobotContainer {
new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON)
.whenPressed(() -> resetOdometry());
.whenPressed(() -> zeroOdometry(new Pose2d(0, 0, new Rotation2d(0))));
/* Operator Buttons */
// activates "Lit Mode"
@@ -99,8 +112,41 @@ public class RobotContainer {
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
// no auto
return new InstantCommand();
// https://github.com/mjansen4857/pathplanner/wiki <-- Pathplanner Wiki
TrajectoryConfig config = new TrajectoryConfig(1.0, 1.0);
// PATH PLANNER TRAJECTORY IMPLEMENTATION
// PathPlannerTrajectory testFirstPath = PathPlanner.loadPath("First Test Path", 1.0, 1.0);
// PPSwerveControllerCommand command = new PPSwerveControllerCommand(
// testFirstPath,
// pose,
// SwerveDrive.m_kinematics,
// new PIDController(0.5, 0, 0),
// new PIDController(0.5, 0, 0),
// new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(1.0, 1.0)),
// outputModuleStates,
// m_robotSwerveDrive
// );
// WPILIB TRAJECTORY IMPLEMENTATION
Trajectory testFirstPath = PathPlanner.loadPath("First Test Path", 1.0, 1.0);
SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(
testFirstPath,
m_robotSwerveDrive::getOdometry,
m_robotSwerveDrive.m_kinematics,
new PIDController(0.5, 1.0, 1.0),
new PIDController(0.5, 1.0, 1.0),
new ProfiledPIDController(0.5, 1.0, 1.0, new TrapezoidProfile.Constraints(1.0, 1.0)),
m_robotSwerveDrive::setModuleStates,
m_robotSwerveDrive
);
m_robotSwerveDrive.resetOdometry(testFirstPath.getInitialPose());
return swerveControllerCommand.andThen(() -> m_robotSwerveDrive.driveWithInput(new double[] {0, 0}, 0, true));
//return new InstantCommand();
}
/**
@@ -114,8 +160,8 @@ public class RobotContainer {
return m_robotSwerveDrive.getOdometry();
}
public void resetOdometry() {
m_robotSwerveDrive.resetOdometry();
public void zeroOdometry(Pose2d pose) {
m_robotSwerveDrive.resetOdometry(pose);
}
/**
* Add your docs here.
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+19 -10
View File
@@ -4,8 +4,10 @@
package frc4388.robot.subsystems;
import com.ctre.phoenix.sensors.PigeonIMU;
import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame;
import com.ctre.phoenix.sensors.WPI_PigeonIMU;
import com.ctre.phoenix.sensors.PigeonIMU.FusionStatus;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
@@ -38,14 +40,15 @@ public class SwerveDrive extends SubsystemBase {
Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(halfWidth));
Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(-halfWidth));
private SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
public SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
public SwerveModule[] modules;
public WPI_PigeonIMU m_gyro;
protected FusionStatus fstatus = new FusionStatus();
/* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. The numbers used
below are robot specific, and should be tuned. */
private SwerveDrivePoseEstimator m_poseEstimator;
public SwerveDrivePoseEstimator m_poseEstimator;
public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
public boolean ignoreAngles;
@@ -111,11 +114,15 @@ public class SwerveDrive extends SubsystemBase {
fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED));
SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));
for (int i = 0; i < states.length; i++) {
SwerveModule module = modules[i];
SwerveModuleState state = states[i];
module.setDesiredState(state, ignoreAngles);
setModuleStates(states);
}
public void setModuleStates(SwerveModuleState[] desiredStates) {
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));
for (int i = 0; i < desiredStates.length; i++) {
SwerveModule module = modules[i];
SwerveModuleState state = desiredStates[i];
module.setDesiredState(state, ignoreAngles);
}
}
@@ -123,9 +130,11 @@ public class SwerveDrive extends SubsystemBase {
public void periodic() {
//System.err.println(m_gyro.getFusedHeading() +" aaa");
updateOdometry();
SmartDashboard.putNumber("Pigeon Fused Heading", m_gyro.getFusedHeading());
SmartDashboard.putNumber("Pigeon Fused Heading", m_gyro.getFusedHeading(fstatus));
SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
SmartDashboard.putNumber("Pigeon Get Angle", m_gyro.getAngle());
SmartDashboard.putNumber("Pigeon Rotation 2D", m_gyro.getRotation2d().getDegrees());
SmartDashboard.putStringArray("Fusion Status", new String[] {"Is Fusing: "+fstatus.bIsFusing, "Is Valid: "+fstatus.bIsValid, "Heading: "+fstatus.heading});
// m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
// m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_6_SensorFusion, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
@@ -171,8 +180,8 @@ public class SwerveDrive extends SubsystemBase {
/**
* Resets the odometry of the robot to (x=0, y=0, theta=0).
*/
public void resetOdometry() {
m_poseEstimator.resetPosition(new Pose2d(0, 0, new Rotation2d(0)), m_gyro.getRotation2d());
public void resetOdometry(Pose2d pose) {
m_poseEstimator.resetPosition(pose, m_gyro.getRotation2d());
}
/** Updates the field relative position of the robot. */