Merge branch 'master' of https://github.com/Team4388/RiseOfRidgebotics2020

src/main/java/frc4388/robot/commands/DrivePositionMPAux.java

@@ -0,0 +1,90 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.DriveConstants;
+import frc4388.robot.subsystems.Drive;
+
+public class DrivePositionMPAux extends CommandBase {
+  Drive m_drive;
+  double m_cruiseVel;
+  double m_rampDist;
+  double m_targetPos;
+  double m_currentVel;
+  double m_currentPos;
+  double m_targetGyro;
+  double m_targetVel;
+  double m_rampAcc;
+  long m_startTime;
+  long m_rampRate;
+
+  /**
+   * Creates a new DrivePositionMPAux.
+   * 
+   * @param subsystem The drive subsystem
+   * @param cruiseVel The target velocity for the motors in units
+   * @param rampDist  The distance before cruise velocity is reached in inches
+   * @param rampRate  The time to reach the cruise velocity in seconds
+   * @param targetPos The target position
+   */
+  public DrivePositionMPAux(Drive subsystem, double cruiseVel, double rampDist, float rampRate, double targetPos, double targetGyro) {
+    // Use addRequirements() here to declare subsystem dependencies.
+    m_drive = subsystem;
+    m_cruiseVel = cruiseVel * DriveConstants.TICKS_PER_INCH_LOW / 10;
+    m_rampDist = rampDist * DriveConstants.TICKS_PER_INCH_LOW;
+    m_rampRate = (long) rampRate * 1000;
+    m_targetPos = targetPos * DriveConstants.TICKS_PER_INCH_LOW;
+    //m_targetGyro = targetGyro * DriveConstants.TICKS_PER_GYRO_REV / 360;
+    m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
+    addRequirements(m_drive);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_currentVel = m_drive.m_rightFrontMotorVel;
+    m_currentPos = m_drive.m_rightFrontMotorPos;
+    m_targetPos = m_targetPos + m_currentPos;
+    m_targetVel = m_currentVel;
+    m_startTime = System.currentTimeMillis();
+    m_rampAcc = (m_cruiseVel - m_currentVel) / m_rampRate;
+  }
+
+  // Called every m_isRamptime the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_currentVel = m_drive.m_rightFrontMotorVel;
+    m_currentPos = m_drive.m_rightFrontMotorPos;
+    if (System.currentTimeMillis() - m_startTime < m_rampRate) {
+      // Ramping
+      m_targetVel += m_rampAcc * m_drive.m_deltaTimeMs;
+      m_drive.runDriveVelocityPID(-m_targetVel, m_targetGyro);
+    } else if (m_targetPos - m_currentPos > m_rampDist) {
+      // Cruising
+      m_drive.runDriveVelocityPID(-m_cruiseVel, m_targetGyro);
+    } else {
+      // Deramp PID
+      m_drive.runDrivePositionPID(-m_targetPos, m_targetGyro);
+    }
+  }
+
+  // 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() {
+    if (m_currentPos - m_targetPos <= 0.5f * DriveConstants.TICKS_PER_INCH_LOW) {
+      return true;
+    }
+    return false;
+  }
+}

src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java

@@ -18,6 +18,7 @@ public class DriveStraightToPositionMM extends CommandBase {
   double m_targetPosOut;
   double m_targetGyro;
   boolean isGoneFast;
+  int i;
   
   /**
    * Creates a new DriveToDistancePID.
@@ -27,7 +28,7 @@ public class DriveStraightToPositionMM extends CommandBase {
   public DriveStraightToPositionMM(Drive subsystem, double targetPos) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_drive = subsystem;
-    m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH;
+    m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW;
     addRequirements(m_drive);
     //SmartDashboard.putNumber("Distance Target Inches", targetPos);
   }
@@ -39,6 +40,7 @@ public class DriveStraightToPositionMM extends CommandBase {
     m_targetGyro = m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN);
     m_targetPosOut = m_targetPosIn + m_drive.m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY);
     isGoneFast = false;
+    i = 0;
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -48,6 +50,8 @@ public class DriveStraightToPositionMM extends CommandBase {
     //System.err.println("Sensor Error \n" + m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
     //System.err.println("Sensor Target \n" + m_drive.m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN));
     m_drive.runMotionMagicPID(m_targetPosOut, m_targetGyro);
+    SmartDashboard.putBoolean("MM Run", true);
+    i++;
   }
 
   // Called once the command ends or is interrupted.
@@ -59,9 +63,10 @@ public class DriveStraightToPositionMM extends CommandBase {
   @Override
   public boolean isFinished() {
     if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && isGoneFast){
+      SmartDashboard.putBoolean("MM Run", false);
       return true;
     } else {
-      if (m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY) > 100) {
+      if ((m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY) > 100)) {
         isGoneFast = true;
       }
       return false;

src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java

@@ -27,7 +27,7 @@ public class DriveStraightToPositionPID extends CommandBase {
   public DriveStraightToPositionPID(Drive subsystem, double targetPos) {
     // Use addRequirements() here to declare subsystem dependencies.
     m_drive = subsystem;
-    m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH;
+    m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW;
     addRequirements(m_drive);
     //SmartDashboard.putNumber("Distance Target Inches", targetPos);
   }

src/main/java/frc4388/robot/commands/DriveWithJoystick.java

@@ -48,12 +48,14 @@ public class DriveWithJoystick extends CommandBase {
       moveOutput = Math.cos(1.571*moveInput)-1;
     }
 
-    double cosMultiplier = .45;
-    double deadzone = .2;
+    double cosMultiplier = .55;
+    double deadzone = .1;
     if (steerInput > 0){
-      steerOutput = -cosMultiplier*Math.cos(1.571*steerInput)+(cosMultiplier+deadzone);
+      steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steerInput) + cosMultiplier;
+    } else if (steerInput < 0) {
+      steerOutput = (cosMultiplier - deadzone) * Math.cos(1.571*steerInput) - cosMultiplier;
     } else {
-      steerOutput = cosMultiplier*Math.cos(1.571*steerInput)-(cosMultiplier+deadzone);
+      steerOutput = 0;
     }
 
     m_drive.driveWithInput(moveOutput, steerOutput);

src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java

@@ -7,6 +7,7 @@
 
 package frc4388.robot.commands;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import edu.wpi.first.wpiutil.math.MathUtil;
 import frc4388.robot.Constants.DriveConstants;
@@ -21,6 +22,23 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
   long m_deadTimeSteer, m_deadTimeMove;
   long m_deadTimeout = 100;
   IHandController m_controller;
+  boolean m_isInterrupted;
+
+  /* Deadassist Constants */
+  final float stopPosVelCoefLow = 1;
+  final float stopPosVelCoefHigh = 3;
+  final float cosMultiplierLow = 0.55f;
+  final float cosMultiplierHigh = 0.35f;
+  final float targetAngleCoefLow = 5;
+  final float targetAngleCoefHigh = 5;
+  final float gyroVelCoefLow = 1;
+  final float gyroVelCoefHigh = 3;
+
+  /* Deadassist Coeficients */
+  final float stopPosVelCoef = 1;
+  final float cosMultiplier = 0.55f;
+  final float targetAngleCoef = 5;
+  final float gyroVelCoef = 1;
 
   /**
    * Creates a new DriveWithJoystickUsingDeadAssistPID to control the drivetrain with an Xbox controller.
@@ -42,6 +60,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
   @Override
   public void initialize() {
     m_currTime = System.currentTimeMillis();
+    resetGyroTarget();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -54,6 +73,11 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
     m_deltaTime = System.currentTimeMillis() - m_currTime;
     m_currTime = System.currentTimeMillis();
 
+    if (m_isInterrupted) {
+      resetGyroTarget();
+      m_isInterrupted = false;
+    }
+
     /* If move stick is being used */
     if (moveInput != 0) {
       m_deadTimeMove = m_currTime;
@@ -65,15 +89,19 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
       m_deadTimeSteer = m_currTime;
     }
 
-    /* If move stick has been pressed within 1 sec */
-    if (m_currTime - m_deadTimeMove < m_deadTimeout) {
-      /* Curves the moveInput to be slightly more gradual at first */
-      if (moveInput >= 0) {
-        moveOutput = -Math.cos(1.571*moveInput)+1;
-      } else {
-        moveOutput = Math.cos(1.571*moveInput)-1;
-      }
+    /* Curves the moveInput to be slightly more gradual at first */
+    if (moveInput >= 0) {
+      moveOutput = -Math.cos(1.571*moveInput)+1;
+    } else {
+      moveOutput = Math.cos(1.571*moveInput)-1;
+    }
 
+    if (m_drive.m_isSpeedShiftHigh) {
+      runDriveWithInput(moveOutput, steerInput);
+      resetGyroTarget();
+    }
+    /* If move stick has been pressed within 1 sec */
+    else if (m_currTime - m_deadTimeMove < m_deadTimeout) {
       /* If steer stick has not been used for less than 1 sec */
       if (m_currTime - m_deadTimeSteer < m_deadTimeout) {
         runDriveWithInput(moveOutput, steerInput);
@@ -90,16 +118,17 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
     }
   }
 
-  private void runDriveWithInput(double move, double steer) {
-    double cosMultiplier = .45;
+  private void runDriveWithInput(double move, double steerInput) {
+    double cosMultiplier = .55;
     double steerOutput = 0;
     double deadzone = .2;
     /* Curves the steer output to be similarily gradual */
-    if (steer > 0){
-      steerOutput = -cosMultiplier*Math.cos(1.571*steer)+(cosMultiplier+deadzone);
-    } else {
-      steerOutput = cosMultiplier*Math.cos(1.571*steer)-(cosMultiplier+deadzone);
+    if (steerInput > 0){
+      steerOutput = -cosMultiplier*Math.cos(1.571*steerInput)+(cosMultiplier+deadzone);
+    } else if (steerInput < 0) {
+      steerOutput = cosMultiplier*Math.cos(1.571*steerInput)-(cosMultiplier+deadzone);
     }
+
     m_drive.driveWithInput(move, steerOutput);
     System.out.println("Driving With Input");
   }
@@ -110,8 +139,23 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
   }
 
   private void runStoppedTurn(double steer) {
-    updateGyroTarget(steer);
-    m_drive.runDrivePositionPID(m_stopPos, m_targetGyro);
+    double cosMultiplier = 0.55;
+    double steerOutput = 0;
+    double deadzone = .2;
+    /* Curves the steer output to be similarily gradual */
+    if (steer > 0) {
+      steerOutput = -cosMultiplier*Math.cos(1.571*steer)+(cosMultiplier+deadzone);
+    } else if (steer < 0) {
+      steerOutput = cosMultiplier*Math.cos(1.571*steer)-(cosMultiplier+deadzone);
+    }
+
+    updateGyroTarget(steerOutput);
+    double currentPos = m_drive.m_rightFrontMotorPos;
+    if (Math.abs(currentPos - m_stopPos) > 200) {
+      m_drive.runDrivePositionPID(m_stopPos, m_targetGyro);
+    } else {
+      m_drive.driveWithInputAux(0, m_targetGyro);
+    }
     System.out.println("Turning with Target: " + m_targetGyro);
   }
 
@@ -121,15 +165,15 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
   private void updateGyroTarget(double steerInput) {
     m_targetGyro -= 5 * steerInput * m_deltaTime;
     m_targetGyro = MathUtil.clamp(  m_targetGyro,
-                                    m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/8),
-                                    m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/8));
+                                    m_currentGyro-(DriveConstants.TICKS_PER_GYRO_REV/3),
+                                    m_currentGyro+(DriveConstants.TICKS_PER_GYRO_REV/3));
   }
 
   /**
    * set target angle to current angle (prevents buildup of gyro error).
    */
   private void resetGyroTarget() {
-    m_targetGyro = m_currentGyro;
+    //m_targetGyro = m_currentGyro;
     m_targetGyro =  m_currentGyro
                     + m_drive.getTurnRate();
   }
@@ -137,6 +181,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
+    m_isInterrupted = interrupted;
   }
 
   // Returns true when the command should end.

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

@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.DriveConstants;
+import frc4388.robot.subsystems.Drive;
+
+public class TurnDegrees extends CommandBase {
+
+  double m_targetAngle;
+  Drive m_drive;
+  double m_currentYawInTicks;
+  double m_targetAngleTicksIn;
+  double m_targetAngleTicksOut;
+  int i;
+
+  /**
+   * Creates a new TurnDeg.
+   */
+  public TurnDegrees(double targetAngle, Drive subsystem) {
+    // Use addRequirements() here to declare subsystem dependencies.
+
+    m_targetAngle = targetAngle;
+    m_drive = subsystem;
+
+    addRequirements(m_drive);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_targetAngleTicksIn = (m_targetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;
+    m_currentYawInTicks = (m_drive.getGyroYaw() / 360) * DriveConstants.TICKS_PER_GYRO_REV;
+    m_targetAngleTicksOut = m_targetAngleTicksIn + m_currentYawInTicks;
+
+    i = 0;
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_currentYawInTicks = (m_drive.getGyroYaw() / 360) * DriveConstants.TICKS_PER_GYRO_REV;
+
+    m_drive.runTurningPID(m_targetAngleTicksOut);
+
+    SmartDashboard.putNumber("Turning Error", Math.abs(m_currentYawInTicks - m_targetAngleTicksOut));
+    SmartDashboard.putNumber("Turning Target", m_targetAngleTicksOut);
+
+    i++;
+  }
+
+  // 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() {
+    if ((Math.abs(m_drive.getTurnRate()) < 1) && (i > 5)) {
+      return true;
+    }
+    return false;
+  }
+}
+

src/main/java/frc4388/robot/commands/Wait.java

@@ -0,0 +1,61 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Wait extends CommandBase {
+
+  long m_startTime;
+  long m_waitTime;
+  long m_currentTime;
+  SubsystemBase m_subsystem;
+
+  /**
+   * Creates a new WaitCommand.
+   */
+  public Wait(float seconds, SubsystemBase subsystem) {
+    // Use addRequirements() here to declare subsystem dependencies.
+
+    m_waitTime = (long) (seconds * 1000);
+    m_subsystem = subsystem;
+
+    addRequirements(m_subsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_currentTime = System.currentTimeMillis();
+    m_startTime = m_currentTime;
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_currentTime = System.currentTimeMillis();
+    SmartDashboard.putNumber("Time Difference for Wait", (m_currentTime - m_startTime));
+  }
+
+  // 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() {
+    if ((m_currentTime - m_startTime) >= m_waitTime) {
+      return true;
+    } else {
+      return false;
+    }
+  }
+}