From 2c3684c948e565a9266dc08e87c9939cb63bbaf4 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 11 Feb 2022 17:42:48 -0700 Subject: [PATCH] refactored auto build function --- build.gradle | 2 +- .../java/frc4388/robot/RobotContainer.java | 62 +++++-------------- .../frc4388/robot/subsystems/SwerveDrive.java | 12 ++-- 3 files changed, 23 insertions(+), 53 deletions(-) diff --git a/build.gradle b/build.gradle index 996bdec..283effa 100644 --- a/build.gradle +++ b/build.gradle @@ -28,7 +28,7 @@ deploy { frcJava(getArtifactTypeClass('FRCJavaArtifact')) { // Enable visualvm - // jvmArgs << "-Dcom.sun.management.jmxremote=true -Dcom.sun.management.jmxremote.port=1099 -Dcom.sun.management.jmxremote.local.only=false -Dcom.sun.management.jmxremote.ssl=false -Dcom.sun.management.jmxremote.authenticate=false -Djava.rmi.server.hostname=10.43.88.2" + jvmArgs << "-Dcom.sun.management.jmxremote=true -Dcom.sun.management.jmxremote.port=1099 -Dcom.sun.management.jmxremote.local.only=false -Dcom.sun.management.jmxremote.ssl=false -Dcom.sun.management.jmxremote.authenticate=false -Djava.rmi.server.hostname=10.43.88.2" } // Static files artifact diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1c98c77..58fc9e6 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -14,6 +14,8 @@ import java.util.List; import javax.swing.plaf.nimbus.State; +import com.ctre.phoenix.motorcontrol.TalonFXControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonFX; import com.pathplanner.lib.PathPlanner; import com.pathplanner.lib.PathPlannerTrajectory; import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState; @@ -34,6 +36,7 @@ import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.XboxController; 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.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; @@ -60,6 +63,8 @@ public class RobotContainer { private final SwerveDrive m_robotSwerveDrive = new SwerveDrive( m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); + private final TalonFX m_testMotor = new TalonFX(23); + private final LED m_robotLED = new LED(m_robotMap.LEDController); /* Controllers */ @@ -84,6 +89,8 @@ public class RobotContainer { // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED)); + + m_testMotor.set(TalonFXControlMode.PercentOutput, 0); } /** @@ -120,29 +127,21 @@ public class RobotContainer { .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); } - - - public Command[] buildAuto(double maxVel, double maxAccel, ArrayList inputs) { + public Command[] buildAuto(double maxVel, double maxAccel, Object... inputs) { ArrayList commands = new ArrayList(); commands.add(new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset())); - // ArrayList trajectories = new ArrayList(); - // ArrayList trajCommands = new ArrayList(); - // ArrayList initStates = new ArrayList(); - // ArrayList fullCommand = new ArrayList(); - PIDController xController = SwerveDriveConstants.X_CONTROLLER; PIDController yController = SwerveDriveConstants.Y_CONTROLLER; ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER; thetaController.enableContinuousInput(-Math.PI, Math.PI); // parse input - for (int i=0; i m_robotSwerveDrive.resetOdometry(new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation)))); @@ -156,41 +155,11 @@ public class RobotContainer { m_robotSwerveDrive::setModuleStates, m_robotSwerveDrive)); } - if (inputs.get(i) instanceof Command) { - commands.add((Command) inputs.get(i)); + if (inputs[i] instanceof Command) { + commands.add((Command) inputs[i]); } } - - // fullCommand.add(new SequentialCommandGroup(new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()))); - - // for (int i = 0; i < len; i++) { - // String path = paths[i]; - - // trajectories.add(PathPlanner.loadPath(path, maxVel, maxAccel)); - - // trajCommands.add(new PPSwerveControllerCommand( - // trajectories.get(i), - // m_robotSwerveDrive::getOdometry, - // m_robotSwerveDrive.m_kinematics, - // xController, - // yController, - // thetaController, - // m_robotSwerveDrive::setModuleStates, - // m_robotSwerveDrive)); - - // initStates.add((PathPlannerState) trajectories.get(i).sample(0)); - // } - - // for (int i=1; i < (1 + len); i++) { - - // PathPlannerState initState = initStates.get(i - 1); - // fullCommand.add(new SequentialCommandGroup(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation))), trajCommands.get(i - 1))); - // } - - // fullCommand.add(new SequentialCommandGroup(new InstantCommand(() -> m_robotSwerveDrive.stopModules()))); - // SequentialCommandGroup[] ret = new SequentialCommandGroup[fullCommand.size()]; - // ret = fullCommand.toArray(ret); commands.add(new InstantCommand(() -> m_robotSwerveDrive.stopModules())); Command[] ret = new Command[commands.size()]; @@ -206,8 +175,9 @@ public class RobotContainer { public Command getAutonomousCommand() { // https://github.com/mjansen4857/pathplanner/wiki <-- Pathplanner Wiki - Command[] trajCommands = buildAuto(0.5, 0.5, new ArrayList(Arrays.asList("Move Forward", "Move Down"))); - return new SequentialCommandGroup(trajCommands); + Command[] trajCommands = buildAuto(0.5, 0.5, "Move Forward", "Move Down"); + SequentialCommandGroup ret = new SequentialCommandGroup(trajCommands); + return (new ParallelCommandGroup(buildAuto(0.1, 0.1, ret, new InstantCommand(() -> m_testMotor.set(TalonFXControlMode.PercentOutput, 0.2))))); } /** diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index b29f57e..1bd3fb9 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -157,17 +157,17 @@ public class SwerveDrive extends SubsystemBase { public void periodic() { // System.err.println(m_gyro.getFusedHeading() +" aaa"); updateOdometry(); - 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}); + // 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); // m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_1_General, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - m_field.setRobotPose(m_poseEstimator.getEstimatedPosition()); + // m_field.setRobotPose(m_poseEstimator.getEstimatedPosition()); super.periodic(); }