Code review pull request

src/main/java/frc/robot/CommandContainer.java

@@ -0,0 +1,146 @@
+package frc.robot;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.commands.GenericCalibrateKF;
+import frc.robot.commands.RunWhenDisabledCommand;
+import frc.robot.commands.TurnAndPositionToTargetCMD;
+import frc.robot.commands.TurnToTargetCMD;
+import frc.robot.commands.command_groups.CollectCMDGP;
+import frc.robot.commands.command_groups.ShootCMDGP;
+import frc.robot.commands.command_groups.ShootWithPitcherCMDGP;
+import frc.robot.constants.RobotConstants;
+import frc.robot.motion_profiling.AutoPath;
+import frc.robot.motion_profiling.TrigonSwerveControllerCMDGP;
+import frc.robot.subsystems.climber.LiftCMD;
+import frc.robot.subsystems.climber.WinchCMD;
+import frc.robot.subsystems.drivetrain.SupplierFieldDriveCMD;
+import frc.robot.subsystems.intake_opener.IntakeOpenerCMD;
+import frc.robot.subsystems.loader.LoaderCMD;
+import frc.robot.subsystems.shooter.CalibrateShooterKfCMD;
+import frc.robot.subsystems.shooter.ShooterCMD;
+import frc.robot.vision.Target;
+import frc.robot.vision.limelights.PitcherLimelight;
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.GenericHID.Hand;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * Contains all the commands used by the driver, operator, or tester. This
+ * commands should be called in the RobotContainer either by the controllers or
+ * by the Smartdashboard.
+ */
+public class CommandContainer {
+	public static final double DRIVETRAIN_X_SENSITIVITY = 1.5;
+	public static final double DRIVETRAIN_Y_SENSITIVITY = 1.5;
+	public static final double DRIVETRAIN_Z_SENSITIVITY = 1.5;
+
+	// CMDGP
+	public final ShootCMDGP SHOOT_CMDGP;
+	public final ShootWithPitcherCMDGP SHOOT_WITH_PITCHER_CMDGP;
+	public final CollectCMDGP COLLECT_CMDGP;
+
+	// Calibration
+	public final CalibrateShooterKfCMD CALIBRATE_SHOOTER_KF_CMD;
+	public final GenericCalibrateKF CALIBRATE_LOADER_KF_CMD;
+
+	// Shooter
+	public final ShooterCMD SHOOTER_CMD;
+
+	// Drivetrain
+	public final SupplierFieldDriveCMD SUPPLIER_FIELD_DRIVE_CMD;
+	public final TurnToTargetCMD TURN_TO_TARGET_CMD;
+	public final TurnAndPositionToTargetCMD TURN_AND_POSITION_TO_TARGET_CMD;
+	public final RunWhenDisabledCommand RESET_DIRECTION;
+	public final InstantCommand CHANGE_DRIVE_ROTATION;
+	public final InstantCommand TOGGLE_DRIVETRAIN_MOTORS_NEUTRAL_MODE_CMD;
+	public final TrigonSwerveControllerCMDGP MOTION_TEST;
+
+	// Intake
+	public final IntakeOpenerCMD OPEN_INTAKE_CMD;
+	public final IntakeOpenerCMD CLOSE_INTAKE_CMD;
+
+	// Pitcher
+	public final InstantCommand TOGGLE_PITCHER;
+	public final InstantCommand OPEN_PITCHER;
+	public final InstantCommand CLOSE_PITCHER;
+
+	// Loader
+	public final LoaderCMD LOADER_CMD;
+
+	// Climber
+	public final LiftCMD OPEN_LIFT_CMD;
+	public final LiftCMD CLOSE_LIFT_CMD;
+	public final WinchCMD WINCH_CMD;
+
+	public CommandContainer(SubsystemContainer subsystemContainer, RobotConstants robotConstants,
+			PitcherLimelight limelight, GenericHID driverController) {
+		// CMDGP
+		SHOOT_CMDGP = new ShootCMDGP(subsystemContainer, robotConstants, limelight);
+		SHOOT_WITH_PITCHER_CMDGP = new ShootWithPitcherCMDGP(subsystemContainer, robotConstants, limelight);
+		COLLECT_CMDGP = new CollectCMDGP(subsystemContainer, robotConstants);
+
+		// Testing
+		CALIBRATE_SHOOTER_KF_CMD = new CalibrateShooterKfCMD(subsystemContainer.SHOOTER_SS,
+				robotConstants.shooterConstants);
+		CALIBRATE_LOADER_KF_CMD = new GenericCalibrateKF(subsystemContainer.LOADER_SS,
+				robotConstants.loaderConstants.FEEDFORWARD_CONSTANTS);
+
+		// Shooter
+		SmartDashboard.putNumber("Shooter/Desired Velocity", 0);
+		SHOOTER_CMD = new ShooterCMD(subsystemContainer.SHOOTER_SS, null, robotConstants.shooterConstants,
+				() -> SmartDashboard.getNumber("Shooter/Desired Velocity", 0));
+
+		// Drivetrain
+		SUPPLIER_FIELD_DRIVE_CMD = new SupplierFieldDriveCMD(subsystemContainer.DRIVETRAIN_SS,
+				robotConstants.drivetrainConstants,
+				() -> Math.signum(driverController.getX(Hand.kRight)) * Math.abs(Math.pow(driverController.getX(Hand.kRight), 3))
+						/ DRIVETRAIN_X_SENSITIVITY,
+				() -> Math.signum(driverController.getY(Hand.kRight)) * Math.abs(Math.pow(driverController.getY(Hand.kRight), 3))
+						/ DRIVETRAIN_Y_SENSITIVITY,
+				() -> Math.signum(driverController.getX(Hand.kLeft)) * Math.abs(Math.pow(driverController.getX(Hand.kLeft), 3))
+						/ DRIVETRAIN_Z_SENSITIVITY);
+		TURN_TO_TARGET_CMD = new TurnToTargetCMD(subsystemContainer.DRIVETRAIN_SS, limelight,
+				robotConstants.visionConstants, Target.PowerPort);
+		TURN_AND_POSITION_TO_TARGET_CMD = new TurnAndPositionToTargetCMD(subsystemContainer.DRIVETRAIN_SS, limelight,
+				robotConstants.visionConstants, Target.PowerPort);
+		RESET_DIRECTION = new RunWhenDisabledCommand(() -> {
+			subsystemContainer.DRIVETRAIN_SS.resetGyro();
+			subsystemContainer.DRIVETRAIN_SS.resetOdometry(new Pose2d());
+		});
+		RESET_DIRECTION.addRequirements(subsystemContainer.DRIVETRAIN_SS);
+		CHANGE_DRIVE_ROTATION = new InstantCommand(() -> {
+			subsystemContainer.DRIVETRAIN_SS.setAngle(subsystemContainer.DRIVETRAIN_SS.getAngle() + 90);
+			subsystemContainer.DRIVETRAIN_SS.resetOdometry(
+					new Pose2d(0, 0, Rotation2d.fromDegrees(subsystemContainer.DRIVETRAIN_SS.getAngle())));
+		});
+		CHANGE_DRIVE_ROTATION.addRequirements(subsystemContainer.DRIVETRAIN_SS);
+		TOGGLE_DRIVETRAIN_MOTORS_NEUTRAL_MODE_CMD = new InstantCommand(
+				subsystemContainer.DRIVETRAIN_SS::toggleMotorsNeutralMode);
+		MOTION_TEST = new TrigonSwerveControllerCMDGP(subsystemContainer.DRIVETRAIN_SS,
+				robotConstants.motionProfilingConstants, AutoPath.Test);
+
+		// Intake
+		OPEN_INTAKE_CMD = new IntakeOpenerCMD(subsystemContainer.INTAKE_OPENER_SS, robotConstants.intakeOpenerConstants,
+				true);
+		CLOSE_INTAKE_CMD = new IntakeOpenerCMD(subsystemContainer.INTAKE_OPENER_SS,
+				robotConstants.intakeOpenerConstants, true);
+
+		// Pitcher
+		TOGGLE_PITCHER = new InstantCommand(subsystemContainer.PITCHER_SS::toggleSolenoid,
+				subsystemContainer.PITCHER_SS);
+		OPEN_PITCHER = new InstantCommand(() -> subsystemContainer.PITCHER_SS.setSolenoidState(true));
+		CLOSE_PITCHER = new InstantCommand(() -> subsystemContainer.PITCHER_SS.setSolenoidState(false));
+
+		// Loader
+		LOADER_CMD = new LoaderCMD(subsystemContainer.LOADER_SS, robotConstants.loaderConstants,
+				robotConstants.loaderConstants.DEFAULT_SHOOTING_VELOCITY);
+
+		// Climber
+		OPEN_LIFT_CMD = new LiftCMD(subsystemContainer.LIFT_SS, robotConstants.climberConstants);
+		CLOSE_LIFT_CMD = new LiftCMD(subsystemContainer.LIFT_SS, robotConstants.climberConstants,
+				() -> -robotConstants.climberConstants.DEFAULT_LIFT_POWER);
+		WINCH_CMD = new WinchCMD(subsystemContainer.WINCH_SS, robotConstants.climberConstants);
+	}
+}

src/main/java/frc/robot/DashboardDataContainer.java

@@ -0,0 +1,60 @@
+package frc.robot;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import frc.robot.constants.RobotConstants;
+import frc.robot.utilities.DashboardController;
+import frc.robot.vision.limelights.PitcherLimelight;
+
+import static edu.wpi.first.wpilibj.smartdashboard.SmartDashboard.*;
+
+/**
+ * DashboardDataContainer contains all the data to be viewed or put in the
+ * dashboard. UpdateDashboard should be called periodicly.
+ */
+public class DashboardDataContainer {
+    DashboardController dashboardController;
+
+    public DashboardDataContainer(SubsystemContainer subsystemContainer, RobotConstants robotConstants,
+            PitcherLimelight limelight, GenericHID driverController, CommandContainer container) {
+        dashboardController = new DashboardController();
+
+        // CMDGP
+        putData("Shoot without pitcher CMDGP", container.SHOOT_CMDGP);
+        putData(" shoot ", container.SHOOT_WITH_PITCHER_CMDGP);
+        putData(" collect ", container.COLLECT_CMDGP);
+
+        // Calibration
+        putData("CalibrateShooterKfCMD", container.CALIBRATE_SHOOTER_KF_CMD);
+        putData("CalibrateLoaderKfCMD", container.CALIBRATE_LOADER_KF_CMD);
+
+        // Shooter
+        putData("Shooter Command", container.SHOOTER_CMD);
+
+        // Drivetrain
+        putData("TurnToTargetCMD", container.TURN_TO_TARGET_CMD);
+        putData("TurnAndPositionToTargetCMD", container.TURN_AND_POSITION_TO_TARGET_CMD);
+        putData("Drivetrain/ResetDirection", container.RESET_DIRECTION);
+        putData("ToggleMotorsModeCMD", container.TOGGLE_DRIVETRAIN_MOTORS_NEUTRAL_MODE_CMD);
+        putData("TrigonSwerveControllerCMDGP", container.MOTION_TEST);
+
+        // Pitcher
+        putData("toggle solenoid", container.TOGGLE_PITCHER);
+        putData("Pitcher/Open", container.OPEN_PITCHER);
+        putData("Pitcher/Close", container.CLOSE_PITCHER);
+
+        // Loader
+        putData("Loader/Load", container.LOADER_CMD);
+
+        // DashboardController
+        dashboardController.addBoolean("Pitcher/State", subsystemContainer.PITCHER_SS::getSolenoidState);
+        dashboardController.addNumber("Shooter/Velocity", subsystemContainer.SHOOTER_SS::getVelocityRPM);
+        dashboardController.addNumber("Loader/Velocity", subsystemContainer.LOADER_SS::getVelocity);
+        dashboardController.addNumber("PitcherLimelight/distance", limelight::calculateDistanceFromTower);
+        dashboardController.addNumber("Xbox/X", driverController::getX);
+        dashboardController.addNumber("Xbox/Y", driverController::getY);
+    }
+
+    public void updateDashboard() {
+        dashboardController.update();
+    }
+}

src/main/java/frc/robot/Main.java

@@ -0,0 +1,29 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018 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 frc.robot;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all.
+ * Unless you know what you are doing, do not modify this file except to
+ * change the parameter class to the startRobot call.
+ */
+public final class Main {
+  private Main() {
+  }
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}

src/main/java/frc/robot/Robot.java

@@ -0,0 +1,54 @@
+package frc.robot;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+public class Robot extends TimedRobot {
+	private RobotContainer robotContainer;
+
+	@Override
+	public void robotInit() {
+		robotContainer = new RobotContainer();
+		// robot.win=true
+	}
+
+	@Override
+	public void robotPeriodic() {
+		robotContainer.periodic();
+		CommandScheduler.getInstance().run();
+	}
+
+	@Override
+	public void autonomousInit() {
+		robotContainer.autonomousInit();
+	}
+
+	@Override
+	public void autonomousPeriodic() {
+	}
+
+	@Override
+	public void teleopInit() {
+		robotContainer.teleopInit();
+	}
+
+	@Override
+	public void teleopPeriodic() {
+	}
+
+	@Override
+	public void disabledInit() {
+	}
+
+	@Override
+	public void disabledPeriodic() {
+	}
+
+	@Override
+	public void testInit() {
+	}
+
+	@Override
+	public void testPeriodic() {
+	}
+}