Merge se/swerve with main

src/main/java/com/stuypulse/robot/Robot.java

@@ -5,11 +5,22 @@
 
 package com.stuypulse.robot;
 
+import com.stuypulse.robot.constants.Settings.RobotType;
+
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
 public class Robot extends TimedRobot {
+
+    public static final RobotType ROBOT;
+
+    static {
+        if (Robot.isSimulation())
+            ROBOT = RobotType.SIM;
+        else
+            ROBOT = RobotType.fromString(System.getenv("serialnum"));
+    }
 
     private RobotContainer robot;
     private Command auto;

src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java

@@ -0,0 +1,60 @@
+package com.stuypulse.robot.commands.swerve;
+
+import com.stuypulse.stuylib.input.Gamepad;
+import com.stuypulse.stuylib.math.SLMath;
+import com.stuypulse.stuylib.streams.numbers.IStream;
+import com.stuypulse.stuylib.streams.numbers.filters.LowPassFilter;
+import com.stuypulse.stuylib.streams.vectors.VStream;
+import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone;
+import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter;
+import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit;
+
+import com.stuypulse.robot.constants.Settings.Driver.Drive;
+import com.stuypulse.robot.constants.Settings.Driver.Turn;
+import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
+
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class SwerveDriveDrive extends Command {
+
+    private final SwerveDrive swerve;
+
+    private final Gamepad driver;
+
+    private final VStream speed;
+    private final IStream turn;
+
+    public SwerveDriveDrive(Gamepad driver) {
+        swerve = SwerveDrive.getInstance();
+
+        speed = VStream.create(driver::getLeftStick)
+            .filtered(
+                new VDeadZone(Drive.DEADBAND),
+                x -> x.clamp(1),
+                x -> x.pow(Drive.POWER.get()),
+                x -> x.mul(Drive.MAX_TELEOP_SPEED.get()),
+                new VRateLimit(Drive.MAX_TELEOP_ACCEL.get()),
+                new VLowPassFilter(Drive.RC.get()));
+
+        turn = IStream.create(driver::getRightX)
+            .filtered(
+                x -> SLMath.deadband(x, Turn.DEADBAND.get()),
+                x -> SLMath.spow(x, Turn.POWER.get()),
+                x -> x * Turn.MAX_TELEOP_TURNING.get(),
+                new LowPassFilter(Turn.RC.get()));
+
+        this.driver = driver;
+
+        addRequirements(swerve);
+    }
+
+    @Override
+    public void execute() {
+        if (driver.getLeftTriggerPressed()) {
+            swerve.setChassisSpeeds(new ChassisSpeeds(speed.get().y, -speed.get().x, -turn.get()));
+        } else {
+            swerve.drive(speed.get(), turn.get());
+        }
+    }
+}

src/main/java/com/stuypulse/robot/constants/Motors.java

@@ -5,11 +5,9 @@
 
 package com.stuypulse.robot.constants;
 
-import com.ctre.phoenix.motorcontrol.NeutralMode;
-import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
-import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
-import com.revrobotics.CANSparkMax;
-import static com.revrobotics.CANSparkMax.IdleMode;
+import com.revrobotics.CANSparkBase.IdleMode;
+import com.revrobotics.CANSparkLowLevel.PeriodicFrame;
+import com.revrobotics.CANSparkBase;
 
 /*-
  * File containing all of the configurations that different motors require.
@@ -22,76 +20,61 @@
  */
 public interface Motors {
 
-    /** Classes to store all of the values a motor needs */
-
-    public static class TalonSRXConfig {
-        public final boolean INVERTED;
-        public final NeutralMode NEUTRAL_MODE;
-        public final int PEAK_CURRENT_LIMIT_AMPS;
-        public final double OPEN_LOOP_RAMP_RATE;
+    public enum StatusFrame {
+        APPLIED_OUTPUT_FAULTS,
+        MOTOR_VEL_VOLTS_AMPS,
+        MOTOR_POSITION,
+        ANALOG_SENSOR,
+        ALTERNATE_ENCODER,
+        ABS_ENCODER_POSIITION,
+        ABS_ENCODER_VELOCITY
+    }
 
-        public TalonSRXConfig(
-                boolean inverted,
-                NeutralMode neutralMode,
-                int peakCurrentLimitAmps,
-                double openLoopRampRate) {
-            this.INVERTED = inverted;
-            this.NEUTRAL_MODE = neutralMode;
-            this.PEAK_CURRENT_LIMIT_AMPS = peakCurrentLimitAmps;
-            this.OPEN_LOOP_RAMP_RATE = openLoopRampRate;
-        }
+    public static void disableStatusFrames(CANSparkBase motor, StatusFrame... ids) {
+        final int kDisableStatusFrame = 500;
 
-        public TalonSRXConfig(boolean inverted, NeutralMode neutralMode, int peakCurrentLimitAmps) {
-            this(inverted, neutralMode, peakCurrentLimitAmps, 0.0);
+        for (StatusFrame id : ids) {
+            motor.setPeriodicFramePeriod(PeriodicFrame.fromId(id.ordinal()), kDisableStatusFrame);
         }
+    }
 
-        public TalonSRXConfig(boolean inverted, NeutralMode neutralMode) {
-            this(inverted, neutralMode, 80);
-        }
+    /** Classes to store all of the values a motor needs */
+    public interface Amper {
+        CANSparkConfig LIFT_MOTOR = new CANSparkConfig(true, IdleMode.kBrake, 80);
+        CANSparkConfig SCORE_MOTOR = new CANSparkConfig(true, IdleMode.kBrake, 500, 0.1);
+    }
 
-        public void configure(WPI_TalonSRX motor) {
-            motor.setInverted(INVERTED);
-            motor.setNeutralMode(NEUTRAL_MODE);
-            motor.configContinuousCurrentLimit(PEAK_CURRENT_LIMIT_AMPS - 10, 0);
-            motor.configPeakCurrentLimit(PEAK_CURRENT_LIMIT_AMPS, 0);
-            motor.configPeakCurrentDuration(100, 0);
-            motor.enableCurrentLimit(true);
-            motor.configOpenloopRamp(OPEN_LOOP_RAMP_RATE);
-        }
+    public interface Swerve {
+        CANSparkConfig DRIVE_CONFIG = new CANSparkConfig(true, IdleMode.kBrake, 60, 0.1);
+        CANSparkConfig TURN_CONFIG = new CANSparkConfig(false, IdleMode.kBrake, 80);
     }
 
-    public static class VictorSPXConfig {
-        public final boolean INVERTED;
-        public final NeutralMode NEUTRAL_MODE;
-        public final double OPEN_LOOP_RAMP_RATE;
+    public interface Intake {
+        CANSparkConfig MOTOR_CONFIG = new CANSparkConfig(false, IdleMode.kBrake, 500, 0.25);
+    }
 
-        public VictorSPXConfig(
-                boolean inverted,
-                NeutralMode neutralMode,
-                double openLoopRampRate) {
-            this.INVERTED = inverted;
-            this.NEUTRAL_MODE = neutralMode;
-            this.OPEN_LOOP_RAMP_RATE = openLoopRampRate;
-        }
+    public interface Shooter {
+        CANSparkConfig LEFT_SHOOTER = new CANSparkConfig(false, IdleMode.kCoast, 500, 0.5);
+        CANSparkConfig RIGHT_SHOOTER = new CANSparkConfig(true, IdleMode.kCoast, 500, 0.5);
+    }
 
-        public VictorSPXConfig(boolean inverted, NeutralMode neutralMode) {
-            this(inverted, neutralMode, 0.0);
-        }
+    public interface Conveyor {
+        CANSparkConfig GANDALF_MOTOR = new CANSparkConfig(true, IdleMode.kBrake,500, 0.25);
+        CANSparkConfig SHOOTER_FEEDER_MOTOR = new CANSparkConfig(false, IdleMode.kBrake, 500, 0.1);
+    }
 
-        public void configure(WPI_VictorSPX motor) {
-            motor.setInverted(INVERTED);
-            motor.setNeutralMode(NEUTRAL_MODE);
-            motor.configOpenloopRamp(OPEN_LOOP_RAMP_RATE);
-        }
+    public interface Climber {
+        CANSparkConfig LEFT_MOTOR = new CANSparkConfig(true, IdleMode.kBrake, 80, 0.1);
+        CANSparkConfig RIGHT_MOTOR = new CANSparkConfig(false, IdleMode.kBrake, 80, 0.1);
     }
 
-    public static class CANSparkMaxConfig {
+    public static class CANSparkConfig {
         public final boolean INVERTED;
         public final IdleMode IDLE_MODE;
         public final int CURRENT_LIMIT_AMPS;
         public final double OPEN_LOOP_RAMP_RATE;
 
-        public CANSparkMaxConfig(
+        public CANSparkConfig(
                 boolean inverted,
                 IdleMode idleMode,
                 int currentLimitAmps,
@@ -102,29 +85,20 @@ public CANSparkMaxConfig(
             this.OPEN_LOOP_RAMP_RATE = openLoopRampRate;
         }
 
-        public CANSparkMaxConfig(boolean inverted, IdleMode idleMode, int currentLimitAmps) {
+        public CANSparkConfig(boolean inverted, IdleMode idleMode, int currentLimitAmps) {
             this(inverted, idleMode, currentLimitAmps, 0.0);
         }
 
-        public CANSparkMaxConfig(boolean inverted, IdleMode idleMode) {
-            this(inverted, idleMode, 80);
+        public CANSparkConfig(boolean inverted, IdleMode idleMode) {
+            this(inverted, idleMode, 500);
         }
 
-        public void configure(CANSparkMax motor) {
+        public void configure(CANSparkBase motor) {
             motor.setInverted(INVERTED);
             motor.setIdleMode(IDLE_MODE);
             motor.setSmartCurrentLimit(CURRENT_LIMIT_AMPS);
             motor.setOpenLoopRampRate(OPEN_LOOP_RAMP_RATE);
             motor.burnFlash();
         }
-
-        public void configureAsFollower(CANSparkMax motor, CANSparkMax follows) {
-            motor.setInverted(INVERTED);
-            motor.setIdleMode(IDLE_MODE);
-            motor.setSmartCurrentLimit(CURRENT_LIMIT_AMPS);
-            motor.setOpenLoopRampRate(OPEN_LOOP_RAMP_RATE);
-            motor.follow(follows);
-            motor.burnFlash();
-         }
-     }
+    }
 }

src/main/java/com/stuypulse/robot/constants/Ports.java

@@ -12,4 +12,34 @@ public interface Gamepad {
         int OPERATOR = 1;
         int DEBUGGER = 2;
     }
+
+    public interface Gyro {
+        int PIGEON2 = 5;
+    }
+
+    public interface Swerve {
+        public interface BackRight {
+            int DRIVE = 10;
+            int TURN  = 11;
+            int ENCODER = 1;
+        }
+
+        public interface FrontRight {
+            int DRIVE = 12;
+            int TURN = 13;
+            int ENCODER = 2;
+        }
+
+        public interface FrontLeft {
+            int DRIVE = 14;
+            int TURN = 15;
+            int ENCODER = 3;
+        }
+
+        public interface BackLeft {
+            int DRIVE = 16;
+            int TURN = 17;
+            int ENCODER = 4;
+        }
+    }
 }