Merge pull request #24 from ddjoshua01/main

Vortex Swerve

src/main/java/frc/robot/commands/ExtendClimberCommand.java

@@ -0,0 +1,43 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.ClimberSubsystem;
+
+
+import com.revrobotics.RelativeEncoder;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.ClimberSubsystem;
+
+public class ExtendClimberCommand extends Command {
+    private final ClimberSubsystem climberSubsystem;
+
+    private RelativeEncoder leftEncoder;
+
+//    private RelativeEncoder rightEncoder;
+
+    double leftEncoderPos;
+//    double rightEncoderPos;
+
+    public ExtendClimberCommand(ClimberSubsystem climberSubsystem) {
+        this.climberSubsystem = climberSubsystem;
+        this.leftEncoder = climberSubsystem.getLeftEncoder();
+//        this.rightEncoder = climberSubsystem.getRightEncoder();
+        addRequirements(climberSubsystem);
+    }
+
+    @Override
+    public void execute() {
+
+        leftEncoderPos = leftEncoder.getPosition();
+//        rightEncoderPos = rightEncoder.getPosition();
+
+        if (leftEncoderPos > 3.0) {
+            climberSubsystem.hold();
+        } else {
+            climberSubsystem.extend();
+        }
+
+    }
+
+}

src/main/java/frc/robot/commands/IntakeCommand.java

@@ -3,12 +3,15 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.IndexerSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
+import frc.robot.subsystems.LEDSubsystem;
 
 public class IntakeCommand extends Command {
     private final IntakeSubsystem intakeSubsystem;
 
     private final IndexerSubsystem indexerSubsystem;
 
+    private LEDSubsystem ledSubsystem;
+
     /**
      * Command to run the intake
      * @param intakeSubsystem The instance of {@link IntakeSubsystem}
@@ -26,6 +29,7 @@ public void execute() {
         if (!indexerSubsystem.isCenter()) {
             intakeSubsystem.setSpeed(.9);
             indexerSubsystem.rotateAllWheelsPercent(.9);
+            ledSubsystem.setLED(-0.71);
         } else {
             intakeSubsystem.setSpeed(0);
             indexerSubsystem.rotateAllWheelsPercent(0);

src/main/java/frc/robot/commands/RetractClimberCommand.java

@@ -0,0 +1,36 @@
+package frc.robot.commands;
+
+import com.revrobotics.RelativeEncoder;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.ClimberSubsystem;
+
+public class RetractClimberCommand extends Command {
+    private final ClimberSubsystem climberSubsystem;
+
+    private final RelativeEncoder leftEncoder;
+
+//    private RelativeEncoder rightEncoder;
+
+
+    public RetractClimberCommand(ClimberSubsystem climberSubsystem) {
+        this.climberSubsystem = climberSubsystem;
+        this.leftEncoder = climberSubsystem.getLeftEncoder();
+//        this.rightEncoder = climberSubsystem.getRightEncoder();
+        addRequirements(climberSubsystem);
+    }
+
+    @Override
+    public void execute() {
+        double leftEncoderPos = leftEncoder.getPosition();
+//        double rightEncoderPos = rightEncoder.getPosition();
+
+        climberSubsystem.retract();
+
+        if (leftEncoderPos < 1.0) {
+            climberSubsystem.hold();
+        } else {
+            climberSubsystem.retract();
+        }
+
+    }
+}

src/main/java/frc/robot/commands/SetLEDCommand.java

@@ -18,12 +18,39 @@ public SetLEDCommand(LEDSubsystem subsystem, LEDConstants.Status status) {
     @Override
     public void execute() {
         switch(status) {
-            case TEST_1:
-                //Confetti
-                subsystem.setLED(-0.87);
-            case TEST_2:
-                //Sinelon, Forest Palette
-                subsystem.setLED(-0.71);
+//            case TEST_1:
+//                //Confetti
+//                subsystem.setLED(-0.87);
+//            case TEST_2:
+//                //Sinelon, Forest Palette
+//                subsystem.setLED(-0.71);
+            case IDLE:
+                //Solid, Gold
+                subsystem.setLED(0.69);
+            case INTAKINGINGING:
+                //Solid, Aqua
+                subsystem.setLED(0.81);
+            case MOVING:
+                //Solid, White
+                subsystem.setLED(0.93);
+            case DISABLED:
+                //Solid, Red
+                subsystem.setLED(0.61);
+            case SHOOTING:
+                //Solid, Lime
+                subsystem.setLED(0.73);
+            case VISION_MOVING:
+                //Solid, Blue
+                subsystem.setLED(0.87);
+            case SPINUP:
+                //Solid, Violet
+                subsystem.setLED(0.91);
+            case CLIMBER_EXTENDING:
+                //Shot, Blue
+                subsystem.setLED(-0.83);
+            case CLIMBER_RETRACTING:
+                //Shot, Red
+                subsystem.setLED(-0.85);
         }
     }
 

src/main/java/frc/robot/commands/TranslateCommand.java

@@ -0,0 +1,26 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.SwerveSubsystem;
+
+public class TranslateCommand extends Command {
+
+    private final SwerveSubsystem swerveSubsystem;
+    private final double metersPerSecond;
+
+    public TranslateCommand(SwerveSubsystem swerveSubsystem, double metersPerSecond) {
+        this.swerveSubsystem = swerveSubsystem;
+        this.metersPerSecond = metersPerSecond;
+    }
+
+
+    @Override
+
+    public void execute() {
+        swerveSubsystem.drive(metersPerSecond, 0.0, 0.0, false, true);
+    }
+
+    public double getMetersPerSecond() {
+        return metersPerSecond;
+    }
+}

src/main/java/frc/robot/constants/ClimberConstants.java

@@ -0,0 +1,6 @@
+package frc.robot.constants;
+
+public class ClimberConstants {
+    public static final int leftSideID = 13;
+    public static final int rightSideID = 16;
+}

src/main/java/frc/robot/constants/DrivetrainConstants.java

@@ -27,7 +27,7 @@ public class DrivetrainConstants {
             new Translation2d(-wheelBase / 2, -trackWidth / 2)
     );
 
-    public static final double frontLeftChassisAngularOffset = 5.516 + Math.PI/2 + Math.PI;
+    public static final double frontLeftChassisAngularOffset = 5.516 - Math.PI/2;
     public static final double frontRightChassisAngularOffset = 6.107;
     public static final double rearLeftChassisAngularOffset = 0.876 + Math.PI;
     public static final double rearRightChassisAngularOffset = 1.036 + Math.PI/2;
@@ -45,13 +45,17 @@ public class DrivetrainConstants {
     public static final boolean gyroReversed = false;
     public static final boolean turningEncoderReversed = true;
 
-    public static final int drivingMotorPinionTeeth = 13;
+    public static final int drivingMotorPinionTeeth = 16;
 
-    public static final double freeSpeedRpm = 5676.0;
+    public static final int spurGearTeeth = 20;
+
+
+    //Free Speed RPM: 6784
+    public static final double freeSpeedRpm = 6784.0;
     public static final double drivingMotorFreeSpeedRps = freeSpeedRpm / 60.0;
     public static final double wheelDiameterMeters = 0.0762;
     public static final double wheelCircumferenceMeters = wheelDiameterMeters * Math.PI;
-    public static final double drivingMotorReduction = (45.0 * 22) / (drivingMotorPinionTeeth * 15);
+    public static final double drivingMotorReduction = (45.0 * spurGearTeeth) / (drivingMotorPinionTeeth * 15);
     public static final double driveWheelFreeSpeedRps = (drivingMotorFreeSpeedRps * wheelCircumferenceMeters) / drivingMotorReduction;
 
     public static final double drivingEncoderPositionFactor = (wheelDiameterMeters * Math.PI) / drivingMotorReduction;

src/main/java/frc/robot/constants/LEDConstants.java

@@ -7,7 +7,13 @@ public static enum Status {
         MOVING,
         VISION_MOVING,
         TEST_1,
-        TEST_2
+        TEST_2,
+        SHOOTING,
+        INTAKINGINGING,
+        SPINUP,
+        CLIMBER_EXTENDING,
+        CLIMBER_RETRACTING
     }
 
-}
+
+}

src/main/java/frc/robot/subsystems/ClimberSubsystem.java

@@ -0,0 +1,49 @@
+package frc.robot.subsystems;
+
+import com.revrobotics.CANSparkBase;
+import com.revrobotics.CANSparkLowLevel;
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.ClimberConstants;
+import frc.robot.subsystems.SwerveSubsystem;
+import frc.robot.constants.DrivetrainConstants;
+import frc.robot.commands.DriveCommands;
+
+
+public class ClimberSubsystem extends SubsystemBase {
+    private final CANSparkMax leftSide = new CANSparkMax(ClimberConstants.leftSideID, CANSparkLowLevel.MotorType.kBrushless);
+    // private CANSparkMax rightSide = new CANSparkMax(ClimberConstants.leftSideID, CANSparkLowLevel.MotorType.kBrushless);
+    private final RelativeEncoder leftSideEncoder = leftSide.getEncoder();
+//    private final RelativeEncoder rightSideEncoder = rightSide.getEncoder();
+
+    public RelativeEncoder getLeftEncoder() {
+        return leftSideEncoder;
+    }
+
+//    public RelativeEncoder getRightEncoder() {
+//        return rightSideEncoder;
+//    }
+
+    public void extend() {
+        //rightSide.set(0.1);
+        leftSide.set(0.1);
+    }
+
+    public void retract() {
+        //rightSide.set(-0.1);
+        leftSide.set(-0.1);
+    }
+
+
+    /**
+     * Put the brakes on the climber motors.
+     */
+    public void hold() {
+        leftSide.set(0.0);
+        //rightSide.set(0.0);
+        leftSide.setIdleMode(CANSparkBase.IdleMode.kBrake);
+        //rightSide.setIdleMode(CANSparkBase.IdleMode.kBrake);
+    }
+
+}

src/main/java/frc/robot/subsystems/LEDSubsystem.java

@@ -16,4 +16,4 @@ public void periodic() {
     public void setLED(double value) {
         BlinkinDriver.set(value);
     }
-}
+}

src/main/java/frc/robot/subsystems/SwerveSubsystem.java

@@ -321,6 +321,21 @@ private void resetOdometry(Pose2d pose) {
 
     public void drive(double forwardMetersPerSecond, double sidewaysMetersPerSecond, double radiansPerSecond, boolean fieldRelative, boolean rateLimit) {
         // forward is xspeed, sideways is yspeed
+//        double xSpeedCommanded = 0.0;
+//        double ySpeedCommanded = 0.0;
+//
+//        double[] areaPoints = {0.0, 0.0, 2.0, 2.0};
+//        double poseX = getPose().getX();
+//        double poseY = getPose().getY();
+//
+//        boolean aboveMinPoints = poseX >= areaPoints[0] && poseY >= areaPoints[1];
+//        boolean belowMaxPoints = poseX <= areaPoints[2] && poseY <= areaPoints[3];
+//
+//        if (aboveMinPoints && belowMaxPoints) {
+//            xSpeedCommanded /= 2;
+//            ySpeedCommanded /= 2;
+//        }
+
         double xSpeedCommanded;
         double ySpeedCommanded;