Dev

src/main/java/frc/robot/RobotContainer.java

@@ -7,42 +7,48 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.commands.CollectingCommands;
+import frc.robot.commands.GeneralCommands;
+import frc.robot.constants.OperatorConstants;
 import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.intake.IntakeCommands;
+import frc.robot.subsystems.intake.IntakeConstants;
 import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.shooter.ShooterCommands;
+import frc.robot.subsystems.shooter.ShooterConstants;
 import frc.robot.subsystems.tank.Tank;
-import frc.robot.subsystems.tank.TankCommands;
 import frc.robot.subsystems.transporter.Transporter;
-import frc.robot.commands.*;
+import frc.robot.subsystems.transporter.TransporterCommands;
+import frc.robot.subsystems.transporter.TransporterConstants;
 
 public class RobotContainer {
     public static final Intake INTAKE = new Intake();
-    public static Shooter SHOOTER =  new Shooter();
+    public static Shooter SHOOTER = new Shooter();
+    //    public static SparkShooter SPARK_SHOOTER = new SparkShooter();
     public static Transporter TRANSPORTER = new Transporter();
     public static final Tank TANK = new Tank();
-    private static final CommandXboxController DRIVER_CONTROLLER = new CommandXboxController(0);
 
     public RobotContainer() {
         configureBindings();
-        TANK.setDefaultCommand(
-                TankCommands.getArcadeDriveCommand(
-                        () -> -DRIVER_CONTROLLER.getLeftY(),
-                        DRIVER_CONTROLLER::getRightX
-                )
-        );
-
-        /*DRIVER_CONTROLLER.rightTrigger().whileTrue(
-                IntakeAndTransportTennisBallCommand.CollectTennisBall()
-        );
-
-        DRIVER_CONTROLLER.rightBumper().whileTrue(
-                IntakeAndTransportTennisBallCommand.EjectTennisBall()
-        );*/
     }
-    
+
     private void configureBindings() {
+        bindDefaultCommands();
+        bindControllerCommands();
+    }
+
+    private void bindDefaultCommands() {
+        TANK.setDefaultCommand(GeneralCommands.getTankDefaultCommand());
+        INTAKE.setDefaultCommand(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.REST));
+        SHOOTER.setDefaultCommand(ShooterCommands.getSetTargetStateCommand(ShooterConstants.ShooterState.REST));
+//        SPARK_SHOOTER.setDefaultCommand(SparkShooterCommands.getSetTargetStateCommand(SparkShooterConstants.SparkShooterState.REST));
+        TRANSPORTER.setDefaultCommand(TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.REST));
     }
-
+
+    private void bindControllerCommands() {
+        OperatorConstants.COLLECT.whileTrue(CollectingCommands.CollectTennisBall());
+    }
+
     public Command getAutonomousCommand() {
         return Commands.print("No autonomous command configured");
     }

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

@@ -0,0 +1,22 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import frc.robot.subsystems.intake.IntakeCommands;
+import frc.robot.subsystems.intake.IntakeConstants;
+import frc.robot.subsystems.shooter.ShooterCommands;
+import frc.robot.subsystems.shooter.ShooterConstants;
+import frc.robot.subsystems.sparkShooter.SparkShooterCommands;
+import frc.robot.subsystems.sparkShooter.SparkShooterConstants;
+import frc.robot.subsystems.transporter.TransporterCommands;
+import frc.robot.subsystems.transporter.TransporterConstants;
+
+public class CollectWhileShoot {
+    public static Command CollectingAndShootingCommand(){
+        return new ParallelCommandGroup(
+                ShooterCommands.getSetTargetStateCommand(ShooterConstants.ShooterState.SHOOT),
+                IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.COLLECT_STATE),
+                TransporterCommands.getSetTargetStateCommand(TransporterConstants.TransporterState.COLLECT_STATE)
+        );
+    }
+}

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

@@ -0,0 +1,35 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import frc.robot.constants.OperatorConstants;
+import frc.robot.subsystems.tank.TankCommands;
+
+public class CommandConstants {
+    private static final CommandXboxController DRIVER_CONTROLLER = OperatorConstants.DRIVER_CONTROLLER;
+    private static final double
+            MINIMUM_DRIVE_SHIFT_POWER = 0.30,
+            MINIMUM_ROTATION_SHIFT_POWER = 0.4;
+
+    public static final Command
+            DRIVE_TANK_COMMAND = TankCommands.getArcadeDriveCommand(
+            () -> calculateDriveStickAxisValue(DRIVER_CONTROLLER.getLeftY()),
+            () -> calculateRotationStickAxisValue(DRIVER_CONTROLLER.getRightX())
+    );
+
+    public static double calculateDriveStickAxisValue(double axisValue) {
+        return axisValue / calculateShiftModeValue(MINIMUM_DRIVE_SHIFT_POWER);
+    }
+
+    public static double calculateRotationStickAxisValue(double axisValue) {
+        return axisValue / calculateShiftModeValue(MINIMUM_ROTATION_SHIFT_POWER);
+    }
+
+    public static double calculateShiftModeValue(double minimumPower) {
+        final double squaredShiftModeValue = Math.sqrt(DRIVER_CONTROLLER.getRightTriggerAxis());
+        final double minimumShiftValueCoefficient = 1 - (1 / minimumPower);
+
+        return 1 - squaredShiftModeValue * minimumShiftValueCoefficient;
+    }
+}
+

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

@@ -0,0 +1,15 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.OperatorConstants;
+import frc.robot.subsystems.tank.TankCommands;
+
+public class GeneralCommands {
+
+    public static Command getTankDefaultCommand() {
+        return TankCommands.getArcadeDriveCommand(
+                () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getLeftY()),
+                () -> CommandConstants.calculateDriveStickAxisValue(OperatorConstants.DRIVER_CONTROLLER.getRightX())
+        );
+    }
+}

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

@@ -4,6 +4,8 @@
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import frc.robot.subsystems.shooter.ShooterCommands;
 import frc.robot.subsystems.shooter.ShooterConstants;
+import frc.robot.subsystems.sparkShooter.SparkShooterCommands;
+import frc.robot.subsystems.sparkShooter.SparkShooterConstants;
 import frc.robot.subsystems.transporter.TransporterCommands;
 import frc.robot.subsystems.transporter.TransporterConstants;
 

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

@@ -0,0 +1,13 @@
+package frc.robot.constants;
+
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+
+public class OperatorConstants {
+    private static final int DRIVER_CONTROLLER_PORT = 0;
+    public static final CommandXboxController DRIVER_CONTROLLER = new CommandXboxController(DRIVER_CONTROLLER_PORT);
+    public static final Trigger
+            SHOOT = DRIVER_CONTROLLER.rightBumper().and(DRIVER_CONTROLLER.leftTrigger().negate()),
+            COLLECT = DRIVER_CONTROLLER.leftTrigger().and(DRIVER_CONTROLLER.rightBumper().negate()),
+            SHOOT_WHILE_COLLECT = DRIVER_CONTROLLER.leftTrigger().and(DRIVER_CONTROLLER.rightBumper().negate());
+}

src/main/java/frc/robot/subsystems/intake/IntakeConstants.java

@@ -42,9 +42,9 @@ private static void configureFollowerMotor() {
     }
 
     public enum IntakeState {
-        COLLECT_STATE(-12.0),
-        EJECT_STATE(12.0),
-        REST(0.0);
+        COLLECT_STATE(3),
+        EJECT_STATE(-3),
+        REST(0);
 
         public final double targetVoltage;
 

src/main/java/frc/robot/subsystems/shooter/Shooter.java

@@ -4,7 +4,7 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Shooter extends SubsystemBase {
-    private final WPI_TalonSRX motor = ShooterConstants.MASTER_MOTOR;
+    private final WPI_TalonSRX motor = ShooterConstants.MOTOR;
 
     public Shooter() {
     }
@@ -18,7 +18,7 @@ void setTargetVoltage(double targetVoltage) {
     }
 
     void stop(){
-        ShooterConstants.MASTER_MOTOR.stopMotor();
+        ShooterConstants.MOTOR.stopMotor();
         ShooterConstants.FOLLOWER_MOTOR.stopMotor();
     }
 }

src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java

@@ -1,54 +1,32 @@
 package frc.robot.subsystems.shooter;
 
-import com.ctre.phoenix.motorcontrol.FollowerType;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 
 public class ShooterConstants {
-    private static final int
-            MASTER_MOTOR_ID = 9,
-            FOLLOWER_MOTOR_ID = 10;
-    static final WPI_TalonSRX
-            MASTER_MOTOR = new WPI_TalonSRX(MASTER_MOTOR_ID),
-            FOLLOWER_MOTOR = new WPI_TalonSRX(FOLLOWER_MOTOR_ID);
-
-    private static final boolean
-            SHOULD_MASTER_MOTOR_INVERT = true,
-            SHOULD_FOLLOWER_MOTOR_INVERT = false;
+    private static final int MASTER_MOTOR_ID = 8;
+    static final WPI_TalonSRX MOTOR = new WPI_TalonSRX(MASTER_MOTOR_ID);
+    private static final boolean SHOULD_MOTOR_INVERT = true;
     private static final NeutralMode NEUTRAL_MODE = NeutralMode.Brake;
 
     private static final double VOLTAGE_LIMIT = 12;
 
     static {
-        configureMasterMotor();
-        configureFollowerMotor();
-    }
-
-    private static void configureMasterMotor() {
-        MASTER_MOTOR.configFactoryDefault();
-        MASTER_MOTOR.enableVoltageCompensation(true);
-        MASTER_MOTOR.configVoltageCompSaturation(VOLTAGE_LIMIT);
-        MASTER_MOTOR.setNeutralMode(NEUTRAL_MODE);
-        MASTER_MOTOR.setInverted(SHOULD_MASTER_MOTOR_INVERT);
-    }
-
-    private static void configureFollowerMotor() {
-        FOLLOWER_MOTOR.configFactoryDefault();
-        FOLLOWER_MOTOR.enableVoltageCompensation(true);
-        FOLLOWER_MOTOR.configVoltageCompSaturation(VOLTAGE_LIMIT);
-        FOLLOWER_MOTOR.setNeutralMode(NEUTRAL_MODE);
-        FOLLOWER_MOTOR.setInverted(SHOULD_FOLLOWER_MOTOR_INVERT);
-        FOLLOWER_MOTOR.follow(MASTER_MOTOR, FollowerType.PercentOutput);
+        MOTOR.configFactoryDefault();
+        MOTOR.enableVoltageCompensation(true);
+        MOTOR.configVoltageCompSaturation(VOLTAGE_LIMIT);
+        MOTOR.setNeutralMode(NEUTRAL_MODE);
+        MOTOR.setInverted(SHOULD_MOTOR_INVERT);
     }
 
-    public enum ShooterState{
+    public enum ShooterState {
         REST(0),
         SHOOT(5),
         EJECT(2);
 
         public final double targetVoltage;
 
-        ShooterState(double targetVoltage){
+        ShooterState(double targetVoltage) {
             this.targetVoltage = targetVoltage;
         }
     }

src/main/java/frc/robot/subsystems/sparkShooter/SparkShooter.java

@@ -0,0 +1,27 @@
+package frc.robot.subsystems.sparkShooter;
+
+
+import com.revrobotics.spark.SparkMax;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class SparkShooter extends SubsystemBase {
+    private final SparkMax motor = SparkShooterConstants.MOTOR;
+
+    public SparkShooter() {
+    }
+
+    void stop() {
+        motor.stopMotor();
+    }
+
+    void setTargetState(SparkShooterConstants.SparkShooterState targetState) {
+        setTargetVoltage(targetState.targetVoltage);
+    }
+
+    private void setTargetVoltage(double targetVoltage) {
+        motor.setVoltage(targetVoltage);
+    }
+
+
+}
+

src/main/java/frc/robot/subsystems/sparkShooter/SparkShooterCommands.java

@@ -0,0 +1,14 @@
+package frc.robot.subsystems.sparkShooter;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.StartEndCommand;
+import frc.robot.RobotContainer;
+
+public class SparkShooterCommands {
+    public static Command getSetTargetStateCommand(SparkShooterConstants.SparkShooterState targetState) {
+        return new StartEndCommand(
+                () -> RobotContainer.SPARK_SHOOTER.setTargetState(targetState),
+                RobotContainer.SPARK_SHOOTER::stop
+        );
+    }
+}

src/main/java/frc/robot/subsystems/sparkShooter/SparkShooterConstants.java

@@ -0,0 +1,39 @@
+package frc.robot.subsystems.sparkShooter;
+
+import com.revrobotics.spark.SparkBase;
+import com.revrobotics.spark.SparkLowLevel;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.SparkBaseConfig;
+import com.revrobotics.spark.config.SparkMaxConfig;
+
+public class SparkShooterConstants {
+    private static final int MOTOR_ID = 20;
+
+    static final SparkMax MOTOR = new SparkMax(MOTOR_ID, SparkLowLevel.MotorType.kBrushless);
+
+    private static final boolean SHOULD_MOTOR_INVERT = false;
+    private static final SparkBaseConfig.IdleMode IDLE_MODE = SparkBaseConfig.IdleMode.kBrake;
+
+    private static final double VOLTAGE_LIMIT = 12;
+
+    static {
+        SparkMaxConfig config = new SparkMaxConfig();
+        config.inverted(SHOULD_MOTOR_INVERT);
+        config.voltageCompensation(VOLTAGE_LIMIT);
+        config.idleMode(SparkBaseConfig.IdleMode.kBrake);
+
+        MOTOR.configure(config, SparkBase.ResetMode.kResetSafeParameters, SparkBase.PersistMode.kNoPersistParameters);
+    }
+
+    public enum SparkShooterState {
+        REST(0),
+        SHOOT(5),
+        EJECT(2);
+
+        public final double targetVoltage;
+
+        SparkShooterState(double targetVoltage) {
+            this.targetVoltage = targetVoltage;
+        }
+    }
+}

src/main/java/frc/robot/subsystems/transporter/Transporter.java

@@ -4,7 +4,7 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Transporter extends SubsystemBase {
-    private final WPI_TalonSRX motor = TransporterConstants.MASTER_MOTOR;
+    private final WPI_TalonSRX motor = TransporterConstants.MOTOR;
 
     public Transporter() {
     }
@@ -18,7 +18,7 @@ void setTargetVoltage(double targetVoltage) {
     }
 
     void stop(){
-        TransporterConstants.MASTER_MOTOR.stopMotor();
+        TransporterConstants.MOTOR.stopMotor();
         TransporterConstants.FOLLOWER_MOTOR.stopMotor();
     }
 }