Merge branch 'master' into covid-shooter-revert

src/main/java/frc4388/robot/commands/auto/Barrel.java

@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands.auto;
+
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc4388.robot.subsystems.Drive;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+public class Barrel extends SequentialCommandGroup {
+  /**
+   * Creates a new Barrel.
+   */
+  public Barrel(Drive drive, RamseteCommand[] paths) {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());
+   addCommands(
+     paths[0]
+   );
+  }
+}

src/main/java/frc4388/robot/commands/auto/BarrelMany.java

@@ -0,0 +1,27 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands.auto;
+
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc4388.robot.subsystems.Drive;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+public class BarrelMany extends SequentialCommandGroup {
+  /**
+   * Creates a new BarrelMany.
+   */
+  public BarrelMany(Drive drive, RamseteCommand[] paths) {
+    // Add your commands in the super() call, e.g.
+    addCommands(
+      paths[0]
+    );
+  }
+}

src/main/java/frc4388/robot/commands/auto/BarrelStart.java

@@ -0,0 +1,34 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands.auto;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.RobotContainer;
+import frc4388.robot.subsystems.Drive;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+public class BarrelStart extends SequentialCommandGroup {
+  /**
+   * Creates a new BarrelStart.
+   */
+  public BarrelStart(Drive drive, RamseteCommand[] paths) {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());
+    addCommands(
+      paths[0],
+      //new Wait(drive, 0.01, 1),
+      new TankDriveVelocity(drive, 5, 5, 1.2) //my life be like oooooo aaaaaa ooooo aaaa
+    );
+    
+  }
+}

src/main/java/frc4388/robot/commands/auto/Bounce.java

@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands.auto;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+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.SequentialCommandGroup;
+import frc4388.robot.RobotContainer;
+import frc4388.robot.subsystems.Drive;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+public class Bounce extends SequentialCommandGroup {
+  /**
+   * Creates a new Bounce.
+   */
+  public Bounce(Drive drive, RobotContainer robotContainer, RamseteCommand[] paths) {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());
+    addCommands(
+      paths[0],
+      new InstantCommand(() -> drive.switchReversed(true)),
+      new InstantCommand(() -> robotContainer.resetOdometry(new Pose2d())),
+      paths[1],
+      new InstantCommand(() -> drive.switchReversed(false)),
+      new InstantCommand(() -> robotContainer.resetOdometry(new Pose2d())),
+      paths[2],
+      new InstantCommand(() -> drive.switchReversed(true)),
+      new InstantCommand(() -> robotContainer.resetOdometry(new Pose2d())),
+      paths[3],
+      new InstantCommand(() -> drive.switchReversed(false)),
+      new InstantCommand(() -> robotContainer.resetOdometry(new Pose2d()))
+    );
+  }
+}

src/main/java/frc4388/robot/commands/auto/DriveOffLineForward.java

@@ -7,8 +7,11 @@
 
 package frc4388.robot.commands.auto;
 
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RamseteCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc4388.robot.RobotContainer;
 import frc4388.robot.subsystems.Drive;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
@@ -18,11 +21,13 @@ public class DriveOffLineForward extends SequentialCommandGroup {
   /**
    * Creates a new DriveOffLineForward.
    */
-  public DriveOffLineForward(Drive drive, RamseteCommand[] paths) {
+  public DriveOffLineForward(Drive drive, RobotContainer robotContainer, RamseteCommand[] paths) {
     // Add your commands in the super() call, e.g.
     // super(new FooCommand(), new BarCommand());
 
     addCommands(
+      new InstantCommand(() -> drive.switchReversed(true)),
+      new InstantCommand(() -> robotContainer.resetOdometry(new Pose2d())),
       paths[0]
     );
   }

src/main/java/frc4388/robot/commands/auto/SequentialTest.java

@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands.auto;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc4388.robot.RobotContainer;
+import frc4388.robot.subsystems.Drive;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+public class SequentialTest extends SequentialCommandGroup {
+  /**
+   * Creates a new SequentialTest.
+   */
+  public SequentialTest(RobotContainer m_robotContainer, RamseteCommand[] paths) {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());
+    addCommands(
+      paths[0],
+      new InstantCommand(() -> m_robotContainer.resetOdometry(new Pose2d())),
+      paths[1]
+    );
+  }
+}

src/main/java/frc4388/robot/commands/auto/Slalom.java

@@ -0,0 +1,28 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc4388.robot.commands.auto;
+
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc4388.robot.subsystems.Drive;
+
+// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
+// information, see:
+// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html
+public class Slalom extends SequentialCommandGroup {
+  /**
+   * Creates a new Slalom.
+   */
+  public Slalom(Drive drive, RamseteCommand[] paths) {
+    // Add your commands in the super() call, e.g.
+    // super(new FooCommand(), new BarCommand());
+    addCommands(
+      paths[0]
+    );
+  }
+}

src/main/java/frc4388/robot/commands/auto/TankDriveVelocity.java

@@ -15,10 +15,10 @@ public class TankDriveVelocity extends CommandBase {
   double m_leftTargetVel;
   double m_rightTargetVel;
 
-  double m_targetTime;
-  double m_firstTimeSec;
-  double m_currentTimeSec;
-  double m_diffSec;
+  long m_targetTime;
+  long m_firstTime;
+  long m_currentTime;
+  long m_diffTime;
 
   /**
    * Creates a new TankDriveVelocity.
@@ -28,24 +28,24 @@ public class TankDriveVelocity extends CommandBase {
     m_drive = subsystem;
     m_leftTargetVel = leftTargetVel;
     m_rightTargetVel = rightTargetVel;
-    m_targetTime = targetTime;
+    m_targetTime = (long) (targetTime * 1000);
     addRequirements(subsystem);
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    m_firstTimeSec = (System.currentTimeMillis() / 1000);
-    m_diffSec = 0;
+    m_firstTime = System.currentTimeMillis();
+    m_diffTime = 0;
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    m_currentTimeSec = (System.currentTimeMillis() / 1000);
-    m_diffSec = m_currentTimeSec - m_firstTimeSec;
+    m_currentTime = System.currentTimeMillis();
+    m_diffTime = m_currentTime - m_firstTime;
     
-    if (m_diffSec < m_targetTime) {
+    if (m_diffTime < m_targetTime) {
       m_drive.tankDriveVelocity(m_leftTargetVel, m_rightTargetVel);
     }
 
@@ -59,7 +59,7 @@ public class TankDriveVelocity extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (m_diffSec >= m_targetTime) {
+    if (m_diffTime >= m_targetTime) {
       return true;
     }
     return false;

src/main/java/frc4388/robot/commands/drive/DriveStraightAtVelocityPID.java

@@ -29,7 +29,7 @@ public class DriveStraightAtVelocityPID extends CommandBase {
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
+    m_targetGyro = (m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN));
   }
 
   // Called every time the scheduler runs while the command is scheduled.

src/main/java/frc4388/robot/commands/drive/GotoCoordinatesRobotRelative.java

@@ -46,7 +46,7 @@ public class GotoCoordinatesRobotRelative extends SequentialCommandGroup {
     addCommands(  new TurnDegrees(m_drive, m_currentAngle),
                   new Wait(m_drive, 0, 0),
                   new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist), 
-                  new TurnDegrees(m_drive, m_endAngle - m_currentAngle));
+                  new TurnDegrees(m_drive, (m_endAngle - m_currentAngle) % 360));
   }
 
   public GotoCoordinatesRobotRelative(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget) {

src/main/java/frc4388/robot/commands/drive/TurnDegrees.java

@@ -38,6 +38,7 @@ public class TurnDegrees extends CommandBase {
   public void initialize() {
     m_targetAngleTicksIn = (m_targetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;
     m_currentYawInTicks = (m_drive.getGyroYaw() / 360) * DriveConstants.TICKS_PER_GYRO_REV;
+    SmartDashboard.putNumber("Current Yaw Ticks", m_currentYawInTicks);
     m_targetAngleTicksOut = m_targetAngleTicksIn + m_currentYawInTicks;
 
     i = 0;
@@ -48,10 +49,10 @@ public class TurnDegrees extends CommandBase {
   public void execute() {
     m_currentYawInTicks = (m_drive.getGyroYaw() / 360) * DriveConstants.TICKS_PER_GYRO_REV;
 
-    m_drive.runTurningPID(m_targetAngleTicksOut);
+    m_drive.runTurningPID(m_targetAngleTicksIn);
 
-    //SmartDashboard.putNumber("Turning Error", Math.abs(m_currentYawInTicks - m_targetAngleTicksOut));
-    //SmartDashboard.putNumber("Turning Target", m_targetAngleTicksOut);
+    SmartDashboard.putNumber("Turning Error", Math.abs(m_currentYawInTicks - m_targetAngleTicksOut));
+    SmartDashboard.putNumber("Turning Target", m_targetAngleTicksOut);
 
     i++;
   }