Merge branch 'master' into arm-command

src/main/java/frc4388/robot/Constants.java

@@ -23,6 +23,7 @@ import frc4388.utility.LEDPatterns;
  */
 public final class Constants {
   public static final class SwerveDriveConstants {
+
     public static final double MAX_ROT_SPEED        = 1.5;
     public static final double MIN_ROT_SPEED        = 0.8;
     public static       double ROTATION_SPEED       = MAX_ROT_SPEED;

src/main/java/frc4388/robot/RobotContainer.java

@@ -14,10 +14,7 @@ import edu.wpi.first.wpilibj.Servo;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 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.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import frc4388.robot.Constants.*;
 import frc4388.robot.commands.AutoBalance;
@@ -28,7 +25,6 @@ import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.robot.commands.JoystickRecorder;
 import frc4388.robot.commands.PivotCommand;
 import frc4388.robot.commands.PlaybackChooser;
-import frc4388.robot.commands.RunArmIn;
 import frc4388.utility.controller.DeadbandedXboxController;
 import frc4388.utility.controller.XboxController; 
 
@@ -89,8 +85,8 @@ public class RobotContainer {
             .withName("SwerveDrive DefaultCommand"));
 
         m_robotArm.setDefaultCommand(new RunCommand(() -> {
-            m_robotArm.setRotVel(getDeadbandedOperatorController().getLeftY(), false);
+            m_robotArm.setRotVel(getDeadbandedOperatorController().getLeftY());
-            m_robotArm.setTeleVel(getDeadbandedOperatorController().getRightY(), false);
+            m_robotArm.setTeleVel(getDeadbandedOperatorController().getRightY());
         }, m_robotArm)
         .withName("Arm DefaultCommand"));
         
@@ -161,23 +157,13 @@ public class RobotContainer {
             .onTrue(new PivotCommand(m_robotArm, 135));
         
         new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
-            .onTrue(new PivotCommand(m_robotArm, 225));
+            .onTrue(new PivotCommand(m_robotArm, 210));
         
         new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
-            .onTrue(new InstantCommand(() -> m_robotClaw.toggle(), m_robotClaw));
+            .onTrue(new InstantCommand(() -> m_robotClaw.toggle()));
         
         new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits()));
-
-        // TODO: put this into a variable
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
-            .onTrue(new ParallelCommandGroup(
-                new InstantCommand(() -> m_robotClaw.toggle()),
-                new SequentialCommandGroup(
-                    new RunArmIn(m_robotArm),
-                    new PivotCommand(m_robotArm, 135)
-                )
-            ));
         
         // new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
         //     .onTrue(new InstantCommand(() -> m_robotArm.resetTeleSoftLimit(), m_robotArm));

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

@@ -1,36 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc4388.robot.commands;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc4388.robot.subsystems.Arm;
-
-public class ExtendArmTo extends CommandBase {
-    private final Arm     m_arm;
-    private final double  m_target;
-    private final boolean m_direction;
-
-    public ExtendArmTo(Arm arm, double units) {
-        m_arm       = arm;
-        m_target    = units;
-        m_direction = m_target > 1;
-        addRequirements(m_arm);
-    }
-
-    // Called every time the scheduler runs while the command is scheduled.
-    @Override
-    public void execute() {
-        m_arm.setTeleVel(m_direction ? 1 : -1, false);
-    }
-
-    // Returns true when the command should end.
-    @Override
-    public boolean isFinished() {
-        if (m_direction)
-            return m_arm.getTeleUnit() > m_target;
-        else
-            return m_arm.getTeleUnit() < m_target;
-    }
-}

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

@@ -5,25 +5,22 @@
 package frc4388.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
-import frc4388.robot.subsystems.Arm;
+import frc4388.robot.subsystems.SwerveDrive;
 
-public class RunArmIn extends CommandBase {
+public class LimeAlign extends CommandBase {
-  private final Arm m_arm;
+  public LimeAlign(SwerveDrive drive) {
-
+    addRequirements(drive);
-  public RunArmIn(Arm arm) {
-    m_arm = arm;
-    addRequirements();
   }
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {}
+  public void initialize() {
+    
+  }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {
+  public void execute() {}
-    m_arm.setTeleVel(1, true);
-  }
 
   // Called once the command ends or is interrupted.
   @Override
@@ -32,6 +29,6 @@ public class RunArmIn extends CommandBase {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return m_arm.isTeleIn();
+    return false;
   }
 }

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

@@ -27,6 +27,6 @@ public class PivotCommand extends PelvicInflammatoryDisease {
   @Override
   public void runWithOutput(double output) {
     SmartDashboard.putNumber("pivot output", output);
-    arm.setRotVel(output, true);
+    arm.setRotVel(output);
   }
 }

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

@@ -27,6 +27,6 @@ public class TeleCommand extends PelvicInflammatoryDisease {
 
   @Override
   public void runWithOutput(double output) {
-    arm.setTeleVel(output, true);
+    arm.setTeleVel(output);
   }
 }

src/main/java/frc4388/robot/subsystems/Arm.java

@@ -47,7 +47,7 @@ public class Arm extends SubsystemBase {
         this(pivot, tele, encoder, false);
     }
 
-    public void setRotVel(double vel, boolean fast) {
+    public void setRotVel(double vel) {
         var degrees = Math.abs(getArmRotation()) - 135;
         SmartDashboard.putNumber("arm degrees", degrees);
         SmartDashboard.putNumber("arm rot vel", vel);
@@ -55,23 +55,14 @@ public class Arm extends SubsystemBase {
         if ((degrees < 2 && vel < 0) || (degrees > 110 && vel > 0)) {
             m_pivot.set(ControlMode.PercentOutput, 0);
         } else if (degrees > 90 && vel > 0) {
-            m_pivot.set(ControlMode.PercentOutput, .2 * vel);
+            m_pivot.set(ControlMode.PercentOutput, .15 * vel);
         } else {
-            m_pivot.set(ControlMode.PercentOutput, (fast ? .8 : .3) * vel);
+            m_pivot.set(ControlMode.PercentOutput, .3 * vel);
         }
     }
 
-    public void setTeleVel(double vel, boolean speed) {
+    public void setTeleVel(double vel) {
-        m_tele.set(ControlMode.PercentOutput, (speed ? -.9 : .7) * vel);
+        m_tele.set(ControlMode.PercentOutput, -0.5 * vel);
-    }
-
-    public boolean isTeleIn() {
-        return m_tele.isRevLimitSwitchClosed() == 1 ||
-            m_tele.getSelectedSensorPosition() < reverse_tele;
-    }
-
-    public double getTeleUnit() {
-        return m_tele.getSelectedSensorPosition() - reverse_tele;
     }
 
     public void armSetRotation(double rot) {
@@ -108,8 +99,8 @@ public class Arm extends SubsystemBase {
                            (ArmConstants.TELE_FORWARD_SOFT_LIMIT - ArmConstants.TELE_REVERSE_SOFT_LIMIT);
 
         if (pivot > 0 || tele < 0 || checkLimits(abs_tele, abs_pivot)) {
-            setRotVel(pivot, false);
+            setRotVel(pivot);
-            setTeleVel(tele, false);
+            setTeleVel(tele);
         }
     }
 
@@ -136,8 +127,6 @@ public class Arm extends SubsystemBase {
             m_tele.configReverseSoftLimitThreshold(tele_soft);
             m_tele.configForwardSoftLimitEnable(true);
             m_tele.configReverseSoftLimitEnable(true);
-
-            reverse_tele = tele_soft;
         } else {
             m_tele.configForwardSoftLimitEnable(false);
             m_tele.configReverseSoftLimitEnable(false);
@@ -146,9 +135,8 @@ public class Arm extends SubsystemBase {
         tele_softLimit = !tele_softLimit;
     }
 
-    boolean resetable    = true;
+    boolean resetable  = true;
-    boolean tele_reset   = true;
+    boolean tele_reset = true;
-    double  reverse_tele = 0;
 
     @Override
     public void periodic() {
@@ -160,8 +148,6 @@ public class Arm extends SubsystemBase {
             m_tele.configReverseSoftLimitThreshold(1000  - tele_soft);
             m_tele.configForwardSoftLimitEnable(true);
             m_tele.configReverseSoftLimitEnable(true);
-            
-            reverse_tele = 1000 - tele_soft;
             tele_reset = false;
         } else if (m_tele.isFwdLimitSwitchClosed() == 0) {
             tele_reset = true;
@@ -173,6 +159,12 @@ public class Arm extends SubsystemBase {
     boolean soft_limits = true;
     public void killSoftLimits() {
         resetTeleSoftLimit();
+        var pivot_soft = m_pivot.getSelectedSensorPosition();
+        var tele_soft  = m_tele.getSelectedSensorPosition();
+        
+        m_pivot.configForwardSoftLimitEnable(!soft_limits);
+        m_pivot.configReverseSoftLimitEnable(!soft_limits);
+
         soft_limits = !soft_limits;
     }
 }

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -12,7 +12,6 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import frc4388.robot.Constants.SwerveDriveConstants;
@@ -38,7 +37,7 @@ public class SwerveDrive extends SubsystemBase {
 
   public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
   
-  public double rotTarget = 0;
+  public Rotation2d rotTarget = new Rotation2d();
   public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
 
   /** Creates a new SwerveDrive. */
@@ -58,6 +57,7 @@ public class SwerveDrive extends SubsystemBase {
     if (fieldRelative) {
 
       double rot = 0;
+      
       if (rightStick.getNorm() > 0.05) {
         rotTarget = gyro.getAngle();
         rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
@@ -71,15 +71,15 @@ public class SwerveDrive extends SubsystemBase {
 
         SmartDashboard.putBoolean("drift correction", true);
         rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED;
+
       }
 
       // Use the left joystick to set speed. Apply a cubic curve and the set max speed.
       Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
-      // Translation2d speed = leftStick.times(speedAdjust / leftStick.getNorm());
       Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
 
       // Convert field-relative speeds to robot-relative speeds.
-      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * cubedSpeed.getX(), cubedSpeed.getY(), rot, gyro.getRotation2d().times(-1));
+      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * cubedSpeed.getX(), cubedSpeed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
     } else {
       // Create robot-relative speeds.
       chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
@@ -106,7 +106,7 @@ public class SwerveDrive extends SubsystemBase {
 
   public void resetGyro() {
     gyro.reset();
-    rotTarget = 0;
+    rotTarget = new Rotation2d(0);
   }
   
   public void stopModules() {