It all started when the fire nation attacked

Rework with a class hierarchy and private variables for protection. Includes: Control system, basic tank arcade drive train and launcher class with debugs implemented. TODO: utils (vision, pid, ...), testing of primary classes, control system setup

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

@@ -7,9 +7,18 @@
 
 package frc.robot;
 
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.SparkMax;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.Spark;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.core.ControlSystem;
+import frc.robot.core.Drivetrain;
+import frc.robot.core.Launcher;
 
 /**
  * The VM is configured to automatically run this class, and to call the
@@ -19,20 +28,31 @@
  * project.
  */
 public class Robot extends TimedRobot {
-  private static final String kDefaultAuto = "Default";
-  private static final String kCustomAuto = "My Auto";
-  private String m_autoSelected;
-  private final SendableChooser<String> m_chooser = new SendableChooser<>();
+
+    private ControlSystem controlSystem;
+    private Drivetrain drivetrain;
+    private Launcher launcher;
 
   /**
    * This function is run when the robot is first started up and should be
    * used for any initialization code.
    */
   @Override
   public void robotInit() {
-    m_chooser.setDefaultOption("Default Auto", kDefaultAuto);
-    m_chooser.addOption("My Auto", kCustomAuto);
-    SmartDashboard.putData("Auto choices", m_chooser);
+
+      controlSystem = new ControlSystem(this);
+      drivetrain = new Drivetrain(this);
+      launcher = new Launcher(this);
+
+      controlSystem.init(true);
+      drivetrain.init(true);
+      launcher.init(true);
+
+  }
+
+  @Override
+  public void teleopInit() {
+
   }
 
   /**
@@ -45,6 +65,8 @@ public void robotInit() {
    */
   @Override
   public void robotPeriodic() {
+
+
   }
 
   /**
@@ -60,32 +82,22 @@ public void robotPeriodic() {
    */
   @Override
   public void autonomousInit() {
-    m_autoSelected = m_chooser.getSelected();
-    // m_autoSelected = SmartDashboard.getString("Auto Selector", kDefaultAuto);
-    System.out.println("Auto selected: " + m_autoSelected);
   }
 
   /**
    * This function is called periodically during autonomous.
    */
   @Override
   public void autonomousPeriodic() {
-    switch (m_autoSelected) {
-      case kCustomAuto:
-        // Put custom auto code here
-        break;
-      case kDefaultAuto:
-      default:
-        // Put default auto code here
-        break;
-    }
   }
 
   /**
    * This function is called periodically during operator control.
    */
   @Override
   public void teleopPeriodic() {
+      drivetrain.teleop(true);
+      launcher.teleop(true);
   }
 
   /**
@@ -94,4 +106,8 @@ public void teleopPeriodic() {
   @Override
   public void testPeriodic() {
   }
-}
+
+  public ControlSystem getControlSystem(){
+      return controlSystem;
+  }
+}

src/main/java/frc/robot/core/ControlSystem.java

@@ -0,0 +1,128 @@
+package frc.robot.core;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.Robot;
+
+
+public class ControlSystem{
+
+    private Robot robot;
+
+    private Joystick driveStick;
+    private Joystick opStick;
+
+    enum Sticks{
+        DRIVE_STICK(0), OPERATOR_STICK(1);
+
+        int port;
+        private Sticks(int port){
+            this.port = port;
+        }
+
+
+        public int getPort(){
+            return port;
+        } 
+    }
+
+    enum Axis{
+        X, Y, Z;
+    }
+
+    enum DriverButtons{
+        TEMP_BUTTON(0);
+
+        int port;
+        private DriverButtons(int port){
+            this.port = port;
+        }
+
+
+        public int getPort(){
+            return port;
+        }
+    }
+
+    enum OperatorButtons{
+        LAUNCH_BUTTON(0);
+
+        int port;
+        private OperatorButtons(int port){
+            this.port = port;
+        }
+
+
+        public int getPort(){
+            return port;
+        }
+    }
+
+    public ControlSystem(Robot robot){
+        this.robot = robot;
+    }
+
+    public void init(boolean debug){
+        driveStick = new Joystick(Sticks.DRIVE_STICK.getPort());
+        opStick = new Joystick(Sticks.OPERATOR_STICK.getPort());
+
+        if(debug){
+            SmartDashboard.putBoolean("Control System Initialization", true);
+        }
+    }
+
+    public double getJoystickAxis(Sticks sticks, Axis axis){
+        switch(sticks){
+            case DRIVE_STICK:
+                switch(axis){
+                    case X:
+                        return driveStick.getX();
+
+                    case Y:
+                        return driveStick.getY();
+
+                    case Z:
+                        return driveStick.getX();
+                }
+
+            case OPERATOR_STICK:
+                switch(axis){
+                    case X:
+                        return opStick.getX();
+
+                    case Y:
+                        return opStick.getY();
+
+                    case Z:
+                        return opStick.getX();
+            }
+
+            default:
+                return 0;
+        }
+    }
+
+    public boolean getDriverButton(DriverButtons driverButtons){
+        switch(driverButtons){
+            case TEMP_BUTTON:
+                return driveStick.getRawButton(DriverButtons.TEMP_BUTTON.getPort());
+
+            default: 
+                return false;
+
+        }
+
+    }
+
+    public boolean getOperatorButton(OperatorButtons operatorButtons){
+        switch(operatorButtons){
+            case LAUNCH_BUTTON:
+                return opStick.getRawButton(OperatorButtons.LAUNCH_BUTTON.getPort());
+
+            default: 
+                return false;
+
+        }
+
+    }
+}

src/main/java/frc/robot/core/Drivetrain.java

@@ -0,0 +1,65 @@
+package frc.robot.core;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.SparkMax;
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.Robot;
+import frc.robot.core.ControlSystem.Axis;
+import frc.robot.core.ControlSystem.Sticks;
+
+public class Drivetrain{
+
+    private Robot robot;
+
+    private ControlSystem controlSystem;
+
+    private CANSparkMax driveFL;
+    private CANSparkMax driveFR;
+    private CANSparkMax driveBL;
+    private CANSparkMax driveBR;
+
+    private double leftPower;
+    private double rightPower;
+
+    public Drivetrain(Robot robot){
+        this.robot = robot;
+    }
+
+    public void init(boolean debug){
+        controlSystem = robot.getControlSystem();
+
+        driveFL = new CANSparkMax(0, MotorType.kBrushless);
+        driveFR = new CANSparkMax(1, MotorType.kBrushless);
+        driveBL = new CANSparkMax(2, MotorType.kBrushless);
+        driveBR = new CANSparkMax(3, MotorType.kBrushless);
+
+        if(debug){
+            SmartDashboard.putBoolean("Drivetrain Initialization", true);
+        }
+
+    }
+
+    public void teleop(boolean debug){
+        leftPower = controlSystem.getJoystickAxis(Sticks.DRIVE_STICK, Axis.X) 
+            + controlSystem.getJoystickAxis(Sticks.DRIVE_STICK, Axis.Y);
+
+        rightPower = controlSystem.getJoystickAxis(Sticks.DRIVE_STICK, Axis.Y) 
+            - controlSystem.getJoystickAxis(Sticks.DRIVE_STICK, Axis.X);
+
+        driveBL.set(leftPower);
+        driveFL.set(leftPower);
+        driveBR.set(rightPower);
+        driveFR.set(rightPower);
+
+        if(debug){
+            SmartDashboard.putNumber("DT Left Power", leftPower);
+            SmartDashboard.putNumber("DT Right Power", rightPower);
+        }
+    }
+}

src/main/java/frc/robot/core/Launcher.java

@@ -0,0 +1,88 @@
+package frc.robot.core;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.can.TalonSRX;
+
+import edu.wpi.first.wpilibj.*;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.Robot;
+import frc.robot.core.ControlSystem.Axis;
+import frc.robot.core.ControlSystem.OperatorButtons;
+import frc.robot.core.ControlSystem.Sticks;
+
+public class Launcher {
+
+    private Robot robot;
+    private ControlSystem controlSystem;
+
+    private TalonSRX leftLaunchMotor;
+    private TalonSRX rightLaunchMotor; 
+    private TalonSRX rightAngleMotor;
+    private TalonSRX leftAngleMotor;
+
+    Compressor compressor;
+    Solenoid solenoid;
+
+    public Launcher(Robot robot){
+        this.robot = robot;
+    }
+
+    public void init(boolean debug){
+
+        controlSystem = robot.getControlSystem();
+
+        leftLaunchMotor  = new TalonSRX(4);
+        rightLaunchMotor = new TalonSRX(5);
+        rightAngleMotor  = new TalonSRX(6);
+        leftAngleMotor   = new TalonSRX(7);
+
+        compressor       = new Compressor(0);
+        solenoid         = new Solenoid(0);
+
+        if(debug){
+            SmartDashboard.putBoolean("Launcher Initalization", true);
+        }
+    }
+
+    public void teleop(boolean debug){
+
+        leftAngleMotor.set(ControlMode.PercentOutput, 
+            controlSystem.getJoystickAxis(Sticks.OPERATOR_STICK, Axis.Y)/2);
+
+        rightAngleMotor.set(ControlMode.PercentOutput,
+            controlSystem.getJoystickAxis(Sticks.OPERATOR_STICK, Axis.X)/2);
+
+        if (compressor.getPressureSwitchValue()) {
+            compressor.start();
+        } else {
+            compressor.stop();
+        }
+
+        if (controlSystem.getOperatorButton(OperatorButtons.LAUNCH_BUTTON)) {
+
+            leftLaunchMotor.set(ControlMode.PercentOutput, 0.5);//motors on
+            rightLaunchMotor.set(ControlMode.PercentOutput, 0.5);
+            solenoid.set(true);// Fires solenoid
+            try {
+                solenoid.wait(500);
+            }
+            catch (Exception e) {
+                solenoid.set(false);
+            }
+            solenoid.set(false);
+            leftLaunchMotor.set(ControlMode.PercentOutput, 0);  // motors off
+            rightLaunchMotor.set(ControlMode.PercentOutput, 0);
+        } else {
+            leftLaunchMotor.set(ControlMode.PercentOutput, 0); // Turns off motors if not pressed 
+            rightLaunchMotor.set(ControlMode.PercentOutput, 0);
+        }
+
+        if(debug){
+            SmartDashboard.putBoolean("Compressor Status", compressor.enabled());
+            SmartDashboard.putBoolean("Solenoid Status", solenoid.get());
+            // TODO fix my CTRE libs so i can debug status of motors.
+
+
+        }
+    }
+}