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

autos better + drive for time + rotate until target

Browse Source
This commit is contained in:
aarav18
2022-03-21 22:49:54 -06:00
parent 4c5106d14f
commit e83c1e5cf8
11 changed files with 184 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/deploy/pathplanner/L Path.path
+1 -55
View File
@@ -1,55 +1 @@
{
"waypoints": [
{
"anchorPoint": {
"x": 2.0,
"y": 2.0
},
"prevControl": null,
"nextControl": {
"x": 3.0,
"y": 2.0
},
"holonomicAngle": 0.0,
"isReversal": false,
"velOverride": null,
"isLocked": false
},
{
"anchorPoint": {
"x": 5.0,
"y": 2.0
},
"prevControl": {
"x": 4.000000000000002,
"y": 2.0
},
"nextControl": {
"x": 5.004218182347978,
"y": 2.0
},
"holonomicAngle": 0.0,
"isReversal": false,
"velOverride": null,
"isLocked": false
},
{
"anchorPoint": {
"x": 5.0,
"y": 5.0
},
"prevControl": {
"x": 4.991875448297241,
"y": 5.0
},
"nextControl": null,
"holonomicAngle": 0.0,
"isReversal": false,
"velOverride": null,
"isLocked": false
}
],
"maxVelocity": 1.0,
"maxAcceleration": 1.0,
"isReversed": null
}
{"waypoints":[{"anchorPoint":{"x":4.0,"y":2.0},"prevControl":null,"nextControl":{"x":5.0,"y":2.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":2.0},"prevControl":{"x":4.000000000000002,"y":2.0},"nextControl":{"x":5.004218182347978,"y":2.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":3.0},"prevControl":{"x":4.991875448297241,"y":3.0},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null}
src/main/deploy/pathplanner/Move Forward.path
+1 -1
View File
@@ -1 +1 @@
{"waypoints":[{"anchorPoint":{"x":3.8761638155965237,"y":2.097178183811552},"prevControl":null,"nextControl":{"x":3.891027717524718,"y":2.097178183811552},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.989837747689554,"y":2.097178183811552},"prevControl":{"x":4.984427304969435,"y":2.097178183811552},"nextControl":{"x":4.984427304969435,"y":2.097178183811552},"holonomicAngle":91.52752544221289,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.8761638155965237,"y":2.140568077269722},"prevControl":{"x":3.3359301436282043,"y":2.140568077269722},"nextControl":null,"holonomicAngle":178.2100893917539,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null}
{"waypoints":[{"anchorPoint":{"x":3.0,"y":2.0},"prevControl":null,"nextControl":{"x":3.0148639019281944,"y":2.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":2.0},"prevControl":{"x":4.45976632803168,"y":2.0},"nextControl":null,"holonomicAngle":180.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null}
src/main/deploy/pathplanner/Rotate.path
+1 -1
View File
@@ -1 +1 @@
{"waypoints":[{"anchorPoint":{"x":3.0,"y":5.0},"prevControl":null,"nextControl":{"x":3.018133524616885,"y":5.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.0,"y":5.0},"prevControl":{"x":2.9945895572798564,"y":5.0},"nextControl":null,"holonomicAngle":-180.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":false}
{"waypoints":[{"anchorPoint":{"x":3.0,"y":5.0},"prevControl":null,"nextControl":{"x":3.018133524616885,"y":5.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.0,"y":5.0},"prevControl":{"x":2.9945895572798564,"y":5.0},"nextControl":null,"holonomicAngle":-180.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":0.05,"maxAcceleration":0.05,"isReversed":false}
src/main/deploy/pathplanner/Test Forward.path
+1
View File
@@ -0,0 +1 @@
{"waypoints":[{"anchorPoint":{"x":0.0,"y":0.0},"prevControl":null,"nextControl":{"x":1.0,"y":0.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.0,"y":0.0},"prevControl":{"x":3.0,"y":0.0},"nextControl":null,"holonomicAngle":-180.0,"isReversal":false,"velOverride":null,"isLocked":false}]}
src/main/java/frc4388/robot/Constants.java
+14 -14
View File
@@ -46,18 +46,18 @@ public final class Constants {
public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20; // TODO: redundant constant?
// IDs
public static final int LEFT_FRONT_STEER_CAN_ID = 2; // *
public static final int LEFT_FRONT_WHEEL_CAN_ID = 3; // *
public static final int RIGHT_FRONT_STEER_CAN_ID = 4; // *
public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5; // *
public static final int LEFT_BACK_STEER_CAN_ID = 6; // *
public static final int LEFT_BACK_WHEEL_CAN_ID = 7; // *
public static final int RIGHT_BACK_STEER_CAN_ID = 8; // *
public static final int RIGHT_BACK_WHEEL_CAN_ID = 9; // *
public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10; // *
public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11; // *
public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12; // *
public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13; // *
public static final int LEFT_FRONT_STEER_CAN_ID = 2;
public static final int LEFT_FRONT_WHEEL_CAN_ID = 3;
public static final int RIGHT_FRONT_STEER_CAN_ID = 4;
public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5;
public static final int LEFT_BACK_STEER_CAN_ID = 6;
public static final int LEFT_BACK_WHEEL_CAN_ID = 7;
public static final int RIGHT_BACK_STEER_CAN_ID = 8;
public static final int RIGHT_BACK_WHEEL_CAN_ID = 9;
public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10;
public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11;
public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;
public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13;
public static final int GYRO_ID = 14;
// offsets are in degrees
@@ -85,8 +85,8 @@ 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(15.0, 0.1, 0.3,
new TrapezoidProfile.Constraints(Math.PI, Math.PI));
public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(2.0, 0, 0.01,//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;
public static final double PATH_MAX_ACCELERATION = 5.0;
src/main/java/frc4388/robot/Robot.java
+1 -2
View File
@@ -213,8 +213,7 @@ public class Robot extends TimedRobot {
* This function is called periodically during autonomous.
*/
@Override
public void autonomousPeriodic() {
}
public void autonomousPeriodic() {}
@Override
public void teleopInit() {
src/main/java/frc4388/robot/RobotContainer.java
+33 -10
View File
@@ -46,6 +46,8 @@ 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.Trajectory.State;
import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.Filesystem;
@@ -59,6 +61,7 @@ import edu.wpi.first.wpilibj2.command.NotifierCommand;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
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.subsystems.Claws;
@@ -71,7 +74,8 @@ import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.commands.ButtonBoxCommands.RunMiddleSwitch;
import frc4388.robot.commands.ClimberCommands.RunClaw;
import frc4388.robot.commands.ClimberCommands.RunClimberPath;
// import frc4388.robot.commands.ButtonBoxCommands.TurretManual;
import frc4388.robot.commands.DriveCommands.DriveWithInputForTime;
import frc4388.robot.commands.DriveCommands.RotateUntilTarget;
import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup;
import frc4388.robot.commands.ShooterCommands.AimToCenter;
import frc4388.robot.commands.ShooterCommands.Seek;
@@ -97,6 +101,7 @@ import frc4388.utility.PathPlannerUtil;
import frc4388.utility.Vector2D;
import frc4388.utility.PathPlannerUtil.Path.Waypoint;
import frc4388.utility.controller.ButtonBox;
import frc4388.utility.controller.DeadbandedRawXboxController;
import frc4388.utility.controller.DeadbandedXboxController;
/**
@@ -128,8 +133,8 @@ public class RobotContainer {
public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret);
// Controllers
private final static XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
private final static XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
private final static XboxController m_driverXbox = new DeadbandedRawXboxController(OIConstants.XBOX_DRIVER_ID);
private final static XboxController m_operatorXbox = new DeadbandedRawXboxController(OIConstants.XBOX_OPERATOR_ID);
private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTON_BOX_ID);
// Autonomous
@@ -371,8 +376,11 @@ public class RobotContainer {
if (inputs[i] instanceof String) {
PathPlannerTrajectory traj = PathPlanner.loadPath(inputs[i].toString(), maxVel, maxAccel);
PathPlannerState initState = (PathPlannerState) traj.sample(0);
commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation))));
PathPlannerState initState = traj.getInitialState();
Pose2d initPose = new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation);
commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(initPose), m_robotSwerveDrive));
commands.add(new PPSwerveControllerCommand(
traj,
m_robotSwerveDrive::getOdometry,
@@ -383,12 +391,13 @@ public class RobotContainer {
m_robotSwerveDrive::setModuleStates,
m_robotSwerveDrive));
}
if (inputs[i] instanceof Command) {
commands.add((Command) inputs[i]);
}
}
commands.add(new InstantCommand(() -> m_robotSwerveDrive.stopModules()));
commands.add(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0, 0, 0, 0, true), m_robotSwerveDrive));
Command[] ret = new Command[commands.size()];
ret = commands.toArray(ret);
return ret;
@@ -421,6 +430,13 @@ public class RobotContainer {
// }).withName("No Autonomous Path");
// }
PIDController xController = SwerveDriveConstants.X_CONTROLLER;
PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
// thetaController.enableContinuousInput(-Math.PI, Math.PI);
loadPath("Move Forward");
// ! this will run each of the specified PathPlanner paths in sequence.
// * return new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3"));
@@ -429,8 +445,16 @@ public class RobotContainer {
// * null,
// * new SequentialCommandGroup(buildAuto(5.0, 5.0, "Path1", "Path2", "Path3")),
// * new RunCommand(() -> m_robotIntake.runAtOutput(0.5))));
// return new SequentialCommandGroup(buildAuto(1.0, 1.0, new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset(), m_robotSwerveDrive),
// // new InstantCommand(() -> this.resetOdometry(new Pose2d())),
// new InstantCommand(() -> m_robotSwerveDrive.setModuleRotationsToAngle(0.0), m_robotSwerveDrive),
// "Diamond"));
return new SequentialCommandGroup(buildAuto(1.0, 1.0, new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()), "Move Forward")); // test command
return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive),
new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.5, 0.5, 0.0, 0.0}, 1.0),
new RotateUntilTarget(m_robotSwerveDrive, m_robotVisionOdometry, 0.5),
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry));
}
public static XboxController getDriverController() {
@@ -563,7 +587,7 @@ public class RobotContainer {
}
private void saveRecording() {
// IMPORTANT: Had to chown the pathplanner folder in order to save autos.
// ! IMPORTANT: Had to chown the pathplanner folder in order to save autos.
File outputFile = PATHPLANNER_DIRECTORY
.resolve(ZonedDateTime.now(SYSTEM_CLOCK).format(RECORDING_FILE_NAME_FORMATTER)).toFile();
LOGGER.log(Level.WARNING, "Creating path {0}.", outputFile.getPath());
@@ -584,8 +608,7 @@ public class RobotContainer {
public void recordPeriodic() {
Translation2d position = m_robotSwerveDrive.m_poseEstimator.getEstimatedPosition().getTranslation();
Rotation2d rotation = m_robotSwerveDrive.m_gyro.getRotation2d();
// FIXME: Chassis speeds are created from joystick inputs and do not reflect
// actual robot velocity.
// * FIXME: Chassis speeds are created from joystick inputs and do not reflect actual robot velocity
Translation2d velocity = new Translation2d(m_robotSwerveDrive.getChassisSpeeds()[0],
m_robotSwerveDrive.getChassisSpeeds()[1]);
Waypoint waypoint = new Waypoint(position, position, position, rotation.getDegrees(), false,
src/main/java/frc4388/robot/commands/DriveCommands/DriveWithInputForTime.java
+59
View File
@@ -0,0 +1,59 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.commands.DriveCommands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.SwerveDrive;
public class DriveWithInputForTime extends CommandBase {
// subsystems
SwerveDrive swerve;
double[] inputs;
// timing
long start;
long elapsed;
double duration;
/**
* DriveWithInput for a specified amount of time.
* @param inputs Inputs used in DriveWithInput (xspeed, yspeed, xrot, yrot).
* @param time Time to DriveWithInput for, in seconds.
*/
public DriveWithInputForTime(SwerveDrive swerve, double[] inputs, double duration) {
// Use addRequirements() here to declare subsystem dependencies.
this.swerve = swerve;
this.inputs = inputs;
this.duration = duration;
addRequirements(this.swerve);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
start = System.currentTimeMillis();
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
elapsed = System.currentTimeMillis() - start;
this.swerve.driveWithInput(inputs[0], inputs[1], inputs[2], inputs[3], true);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return (((long) duration) >= (start - elapsed));
}
}
src/main/java/frc4388/robot/commands/DriveCommands/RotateUntilTarget.java
+52
View File
@@ -0,0 +1,52 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.commands.DriveCommands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.VisionOdometry;
public class RotateUntilTarget extends CommandBase {
// subsystems
SwerveDrive swerve;
VisionOdometry visionOdometry;
double rotateSpeed;
/** Creates a new RotateUntilTarget. */
public RotateUntilTarget(SwerveDrive swerve, VisionOdometry visionOdometry, double rotateSpeed) {
// Use addRequirements() here to declare subsystem dependencies.
this.swerve = swerve;
this.visionOdometry = visionOdometry;
this.rotateSpeed = rotateSpeed;
addRequirements(this.swerve, this.visionOdometry);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
this.visionOdometry.setLEDs(true);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
this.swerve.driveWithInput(0.0, 0.0, rotateSpeed, true);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return this.visionOdometry.m_camera.getLatestResult().hasTargets();
}
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+17 -4
View File
@@ -158,6 +158,13 @@ public class SwerveDrive extends SubsystemBase {
// modules[0].setDesiredState(desiredStates[0], false);
}
public void setModuleRotationsToAngle(double angle) {
for (int i = 0; i < modules.length; i++) {
SwerveModule module = modules[i];
module.rotateToAngle(angle);
}
}
@Override
public void periodic() {
@@ -228,10 +235,16 @@ public class SwerveDrive extends SubsystemBase {
* Updates the field relative position of the robot.
*/
public void updateOdometry() {
m_poseEstimator.update( getRegGyro(),
modules[0].getState(),
modules[1].getState(),
modules[2].getState(),
Rotation2d actualDWI = new Rotation2d(-m_gyro.getRotation2d().getRadians() + (Math.PI*2) + (Math.PI/2));
Rotation2d actual = new Rotation2d(-1 * m_gyro.getRotation2d().getRadians());
SmartDashboard.putNumber("AUTO ACTUAL GYRO", actual.getDegrees());
SmartDashboard.putNumber("AUTO DWI GYRO", actual.getDegrees());
m_poseEstimator.update( actualDWI,//new Rotation2d((2 * Math.PI) - getRegGyro().getRadians()),
modules[0].getState(),
modules[1].getState(),
modules[2].getState(),
modules[3].getState());
}
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+4
View File
@@ -182,6 +182,10 @@ public class SwerveModule extends SubsystemBase {
angleMotor.set(0);
}
public void rotateToAngle(double angle) {
this.angleMotor.set(TalonFXControlMode.Position, angle);
}
@Override
public void periodic() {