From f7d8d41445df968b95f56a69a7f4568baf979e12 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Sat, 29 Feb 2020 11:08:38 -0700 Subject: [PATCH] Replaced GotoCoordinate with Distance PID instead Motion Magic --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/RobotContainer.java | 6 ++++-- src/main/java/frc4388/robot/commands/GotoCoordinates.java | 5 +++-- src/main/java/frc4388/robot/commands/Wait.java | 2 +- 4 files changed, 9 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index dc283e4..3f1d249 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -54,7 +54,7 @@ public final class Constants { public static final Gains DRIVE_DISTANCE_GAINS_LOW = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.5); public static final Gains DRIVE_VELOCITY_GAINS_LOW = new Gains(0.1, 0.0, 0.2, 0.025, 0, 1.0); public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 0.0, 0.0, 0, 0.55); - public static final Gains DRIVE_MOTION_MAGIC_GAINS_LOW = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains DRIVE_MOTION_MAGIC_GAINS_LOW = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0); public static final int DRIVE_CRUISE_VELOCITY = 25000; public static final int DRIVE_ACCELERATION = 21000; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 2fda40a..78bfc4c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -192,7 +192,7 @@ public class RobotContainer { //Runs an Autonomous command group that would shoot our preloaded balls, pick up 3 more from the trench, and shoot those //This assumes that we are positioned against the right wall with our shooter facing the target. - return new SequentialCommandGroup(new Wait(0, m_robotDrive), + return new SequentialCommandGroup(new Wait(m_robotDrive, 0), //add aim command //add shooter command //new DriveStraightToPositionMM(m_robotDrive, 48.0), @@ -208,7 +208,9 @@ public class RobotContainer { //Below this would be the picking up additional balls outside of those in the trench directly behind us //new GotoCoordinates(m_robotDrive, 36, 36), - new GotoCoordinates(m_robotDrive, 36, 36, -90));//, + //new DriveStraightToPositionPID(m_robotDrive, 160) + //new DriveStraightToPositionMM(m_robotDrive, 160) + new GotoCoordinates(m_robotDrive, 87, 47, -90));//, //new StorageIntakeGroup(m_robotIntake, m_robotStorage), //new TurnDegrees(m_robotDrive, 75), //new DriveStraightToPositionMM(m_robotDrive, 18.0), diff --git a/src/main/java/frc4388/robot/commands/GotoCoordinates.java b/src/main/java/frc4388/robot/commands/GotoCoordinates.java index 5580cfc..d267cbe 100644 --- a/src/main/java/frc4388/robot/commands/GotoCoordinates.java +++ b/src/main/java/frc4388/robot/commands/GotoCoordinates.java @@ -43,8 +43,9 @@ public class GotoCoordinates extends SequentialCommandGroup { m_endAngle = endAngle; - addCommands( new TurnDegrees(m_drive, m_currentAngle), - new DriveStraightToPositionMM(m_drive, m_hypotDist), + addCommands( new TurnDegrees(m_drive, m_currentAngle), + new Wait(m_drive, 1), + new DriveStraightToPositionPID(m_drive, m_hypotDist), new TurnDegrees(m_drive, m_endAngle - m_currentAngle)); } diff --git a/src/main/java/frc4388/robot/commands/Wait.java b/src/main/java/frc4388/robot/commands/Wait.java index 934b508..ee7f63f 100644 --- a/src/main/java/frc4388/robot/commands/Wait.java +++ b/src/main/java/frc4388/robot/commands/Wait.java @@ -21,7 +21,7 @@ public class Wait extends CommandBase { /** * Creates a new WaitCommand. */ - public Wait(float seconds, SubsystemBase subsystem) { + public Wait(SubsystemBase subsystem, float seconds) { // Use addRequirements() here to declare subsystem dependencies. m_waitTime = (long) (seconds * 1000);