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

auto shtuff

Browse Source
This commit is contained in:
Ryan Manley
2022-04-04 17:22:52 -06:00
parent a3602b2004
commit c94aa83919
7 changed files with 53 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+1 -1
View File
@@ -85,7 +85,7 @@ public final class Constants {
// swerve auto constants
public static final PIDController X_CONTROLLER = new PIDController(0.5, 0.0, 0.0);
public static final PIDController Y_CONTROLLER = new PIDController(2.0, 0.0, 0.0);
public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(2.0, 0, 0.01,//0.1, 0.3,
public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(15.0, 0, 0.0,//0.1, 0.3,
new TrapezoidProfile.Constraints(Math.PI, Math.PI/2));
public static final boolean PATH_RECORD_VELOCITY = true;
public static final double PATH_MAX_VELOCITY = 5.0;
src/main/java/frc4388/robot/RobotContainer.java
+31 -7
View File
@@ -34,6 +34,8 @@ 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.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
@@ -347,8 +349,10 @@ public class RobotContainer {
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
// .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false));
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
// .whenPressed(new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - 10, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0));
new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
.whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry));
.whileHeld(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret)); // * aim with turret to target);
// new JoystickButton(getOperatorController(), XboxController.Button.kY.value)
// .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true)));
@@ -447,12 +451,13 @@ public class RobotContainer {
* @param inputs strings (paths) or commands you want to run (in order)
* @return array of commands
*/
public Command[] buildAuto(Double maxVel, Double maxAccel, Object... inputs) {
public SequentialCommandGroup buildAuto(Double maxVel, Double maxAccel, Object... inputs) {
maxVel = Objects.requireNonNullElse(maxVel, SwerveDriveConstants.PATH_MAX_VELOCITY);
maxAccel = Objects.requireNonNullElse(maxAccel, SwerveDriveConstants.PATH_MAX_ACCELERATION);
ArrayList<Command> commands = new ArrayList<Command>();
commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
// commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
// commands.add(new InstantCommand(() -> m_robotSwerveDrive.m, m_robotSwerveDrive));
PIDController xController = SwerveDriveConstants.X_CONTROLLER;
PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
@@ -485,10 +490,11 @@ public class RobotContainer {
}
}
commands.add(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0, 0, 0, 0, true), m_robotSwerveDrive));
commands.add(new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive));
Command[] ret = new Command[commands.size()];
ret = commands.toArray(ret);
return ret;
SequentialCommandGroup seqCG = new SequentialCommandGroup(ret);
return seqCG;
}
/**
@@ -522,7 +528,13 @@ public class RobotContainer {
SequentialCommandGroup weirdAutoShootingGroup = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
new ParallelCommandGroup(
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 5.0)
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 2.0, true)
)); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead.
SequentialCommandGroup weirdAutoShootingGroup2 = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
new ParallelCommandGroup(
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 2.0, true)
)); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead.
// ! DRIVE BACKWARDS AND SHOOT (HOPEFULLY)
@@ -605,7 +617,19 @@ public class RobotContainer {
// );
// ! PathPlanner Testing
return new SequentialCommandGroup(buildAuto(1.0, 1.0, "Move Forward"));
ParallelDeadlineGroup intakeWithPath = new ParallelDeadlineGroup(new RunCommandForTime(new RunCommand(() -> m_robotIntake.runAtOutput(-1.0), m_robotIntake), 3.0, true),
new RunCommand(() -> m_robotSerializer.setSerializer(0.8), m_robotSerializer),
buildAuto(1.0, 1.0, "JMove"));
// return new SequentialCommandGroup(buildAuto(1.0, 1.0, "Move Forward"));
return new SequentialCommandGroup(new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 1.0, true), // * aim with turret to target
weirdAutoShootingGroup, // * shoot
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), 0.5, true),
new ExtenderIntakeGroup(m_robotIntake, m_robotExtender),
intakeWithPath,
weirdAutoShootingGroup2,
// new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 1.0, true)); // * aim with turret to target); // * shoot
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), 0.5, true));
// return new SequentialCommandGroup(buildAuto(1.0, 1.0, "Diamond"));
}
src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
+8 -8
View File
@@ -196,14 +196,14 @@ public class TrackTarget extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
// if (this.isAuto) {
// if (targetLocked && !timerStarted) {
// timerStarted = true;
// startTime = System.currentTimeMillis();
// }
// return (targetLocked && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance));
// } else {
if (this.isAuto) {
if (targetLocked && !timerStarted) {
timerStarted = true;
startTime = System.currentTimeMillis();
}
return (targetLocked && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance));
} else {
return false;
// }
}
}
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+10 -5
View File
@@ -177,14 +177,14 @@ public class SwerveDrive extends SubsystemBase {
public void periodic() {
updateOdometry();
// updateSmartDash();
updateSmartDash();
// SmartDashboard.putNumber("Pigeon getRotation2d", m_gyro.getRotation2d().getDegrees());
// SmartDashboard.putNumber("Pigeon getAngle", m_gyro.getAngle());
// SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
// SmartDashboard.putNumber("Pigeon Yaw (0 to 360)", m_gyro.getYaw() % 360);
m_field.setRobotPose(m_odometry.getPoseMeters());
m_field.setRobotPose(getOdometry());
super.periodic();
}
@@ -220,6 +220,11 @@ public class SwerveDrive extends SubsystemBase {
// return m_poseEstimator.getEstimatedPosition();
}
public Pose2d getAutoOdo() {
Pose2d workingPose = getOdometry();
return new Pose2d(-workingPose.getX(), workingPose.getY(), workingPose.getRotation());
}
/**
* Gets the current gyro using regression formula.
*
@@ -244,13 +249,13 @@ public class SwerveDrive extends SubsystemBase {
* Updates the field relative position of the robot.
*/
public void updateOdometry() {
Rotation2d actualDWI = new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI/2));
Rotation2d actual = new Rotation2d(-1 * m_gyro.getRotation2d().getRadians());
Rotation2d actualDWI = new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2)); //+ (Math.PI/2));
Rotation2d actual = new Rotation2d(m_gyro.getRotation2d().getRadians());
SmartDashboard.putNumber("AUTO ACTUAL GYRO", actual.getDegrees());
SmartDashboard.putNumber("AUTO DWI GYRO", actual.getDegrees());
m_odometry.update( m_gyro.getRotation2d(),//new Rotation2d((2 * Math.PI) - getRegGyro().getRadians()),
m_odometry.update( actual,//m_gyro.getRotation2d(),//new Rotation2d((2 * Math.PI) - getRegGyro().getRadians()),
modules[0].getState(),
modules[1].getState(),
modules[2].getState(),