Merge branch 'master' into autonomous-commands

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

@@ -49,11 +49,11 @@ public class DriveWithJoystick extends CommandBase {
     }
 
     double cosMultiplier = .55;
-    double deadzone = .2;
+    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*Math.cos(1.571*steerInput)-(cosMultiplier+deadzone);
+      steerOutput = (cosMultiplier - deadzone) * Math.cos(1.571*steerInput) - cosMultiplier;
     } else {
       steerOutput = 0;
     }

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

@@ -7,14 +7,25 @@
 
 package frc4388.robot.commands;
 
+import com.revrobotics.CANDigitalInput;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANDigitalInput.LimitSwitchPolarity;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+
 import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.IntakeConstants;
 import frc4388.robot.subsystems.Intake;
 import frc4388.utility.controller.IHandController;
 
 public class RunExtenderOutIn extends CommandBase {
   private Intake m_intake;
-  private boolean isOut = false;
-  private long startTime;
+
+  CANSparkMax m_intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_SPARK_ID, MotorType.kBrushless);
+  CANSparkMax m_extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_SPARK_ID, MotorType.kBrushless);
+  CANDigitalInput m_extenderForwardLimit;
+  CANDigitalInput m_extenderReverseLimit;
+
+  
 
   /**
    * Uses input from opperator to run the extender motor.
@@ -25,19 +36,23 @@ public class RunExtenderOutIn extends CommandBase {
     // Use addRequirements() here to declare subsystem dependencies.
     m_intake = subsystem;
     addRequirements(m_intake);
+
+    m_extenderForwardLimit = m_extenderMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
+    m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed);
+    m_extenderForwardLimit.enableLimitSwitch(false);
+    m_extenderReverseLimit.enableLimitSwitch(false);
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
-    isOut = !isOut;
-    startTime = System.currentTimeMillis();
+    m_intake.isExtended = !m_intake.isExtended;
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    if (isOut){
+    if (m_intake.isExtended){
       m_intake.runExtender(0.3);
     } else {
       m_intake.runExtender(-0.3);
@@ -54,9 +69,16 @@ public class RunExtenderOutIn extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    if (startTime + 3000 < System.currentTimeMillis()) {
+    if (m_intake.isExtended && m_extenderForwardLimit.get() == true){
       return true;
     }
-    return false;
+    
+    else if(m_intake.isExtended && m_extenderReverseLimit.get() == true){
+      return true;
+    }
+    
+    else{
+      return false;
+    }
   }
 }

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

@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* 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.subsystems.Storage;
+
+public class StorageIntakeFinal extends CommandBase {
+  Storage m_storage;
+  /**
+   * Creates a new StorageIntakeFinal.
+   */
+  public StorageIntakeFinal(Storage subsystem) {
+    m_storage = subsystem;
+    addRequirements(m_storage);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if (m_storage.getBeam(1)){
+      m_storage.setStoragePID(m_storage.getEncoderPos() + 5);
+    }
+  }
+
+  // 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 false;
+  }
+}

src/main/java/frc4388/robot/commands/StorageIntakeGroup.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;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import frc4388.robot.subsystems.Intake;
+import frc4388.robot.subsystems.Storage;
+
+// 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 StorageIntakeGroup extends SequentialCommandGroup {
+  /**
+   * Creates a new StorageIntakeGroup.
+   */
+  public StorageIntakeGroup(Intake m_intake, Storage m_storage) {
+    addCommands(
+      new storagePrepIntake(m_intake, m_storage), 
+      new storageIntake(m_intake, m_storage), 
+      new StorageIntakeFinal(m_storage));
+  }
+}

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

@@ -7,6 +7,7 @@
 
 package frc4388.robot.commands;
 
+import frc4388.robot.Constants.ShooterConstants;
 import frc4388.robot.Constants.VisionConstants;
 import frc4388.robot.subsystems.Drive;
 import frc4388.robot.subsystems.Shooter;
@@ -28,6 +29,8 @@ public class TrackTarget extends CommandBase {
   double yAngle = 0;
   double target = 0;
   public double distance;
+  public static double fireVel;
+  public static double fireAngle;
 
   /**
    * Uses the Limelight to track the target
@@ -46,6 +49,8 @@ public class TrackTarget extends CommandBase {
     NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3);
   }
 
+
+
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
@@ -65,6 +70,14 @@ public class TrackTarget extends CommandBase {
         //Finding Distance
       distance = VisionConstants.TARGET_HEIGHT/Math.tan((VisionConstants.LIME_ANGLE + yAngle)*(Math.PI/180));
       SmartDashboard.putNumber("Distance to Target", distance);
+
+      double yVel = Math.sqrt(2*VisionConstants.GRAV*VisionConstants.TARGET_HEIGHT);
+      double xVel = (distance*VisionConstants.GRAV)/(yVel);
+
+      fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2)));
+      fireAngle = Math.atan(yVel/xVel);
+      m_shooter.m_fireVel = fireVel;
+      m_shooter.m_fireAngle = fireAngle;
     }
   }
 

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

@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* 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.subsystems.Intake;
+import frc4388.robot.subsystems.Storage;
+
+public class storageIntake extends CommandBase {
+  public Intake m_intake;
+  public Storage m_storage;
+  /**
+   * Creates a new storageIntake.
+   */
+  public storageIntake(Intake inSub, Storage storeSub) {
+    m_intake = inSub;
+    m_storage = storeSub;
+    addRequirements(m_intake);
+    addRequirements(m_storage);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+
+    if (m_storage.getBeam(0)){
+      m_storage.setStoragePID(m_storage.getEncoderPos() + 2);
+      m_intake.runExtender(-0.3); 
+    }
+    else{
+      m_intake.runExtender(0.3); 
+    }
+  }
+
+  // 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_storage.getBeam(0)){
+      return true;
+    }
+    return false;
+  }
+}

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

@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* 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.subsystems.Storage;
+
+public class storageOutake extends CommandBase {
+  Storage m_storage;
+  /**
+   * Creates a new storageOutake.
+   */
+  public storageOutake(Storage storeSub) {
+    m_storage = storeSub;
+    addRequirements(m_storage);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+      m_storage.runStorage(-0.5);
+  }
+
+  // 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 false;
+  }
+}

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

@@ -0,0 +1,52 @@
+/*----------------------------------------------------------------------------*/
+/* 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.subsystems.Storage;
+
+public class storagePrepAim extends CommandBase {
+  Storage m_storage;
+  /**
+   * Creates a new storagePrepAim.
+   */
+  public storagePrepAim(Storage storeSub) {
+    m_storage = storeSub;
+    addRequirements(m_storage);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if (m_storage.getBeam(2) == false){
+      m_storage.runStorage(0.5);
+    }
+    else{
+      m_storage.runStorage(0);
+    }
+  }
+
+  // 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_storage.getBeam(2)){
+      return true;
+    }
+    return false;
+  }
+}

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

@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* 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.subsystems.Intake;
+import frc4388.robot.subsystems.Storage;
+
+public class storagePrepIntake extends CommandBase {
+  public Intake m_intake;
+  public Storage m_storage;
+  /**
+   * Creates a new storagePrepIntake.
+   */
+  public storagePrepIntake(Intake inSub, Storage storeSub) {
+    m_intake = inSub;
+    m_storage = storeSub;
+    addRequirements(m_intake);
+    addRequirements(m_storage);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if (m_storage.getBeam(1) == false){
+      m_storage.runStorage(-0.5);
+    }
+    else{
+      m_storage.runStorage(0);
+    }
+  }
+
+  // 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_storage.getBeam(1)){
+      return true;
+    }
+    return false;
+  }
+}