diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 71765a8..5c649da 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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,10 +28,10 @@ * 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 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 @@ -30,9 +39,20 @@ public class Robot extends TimedRobot { */ @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,9 +82,6 @@ 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); } /** @@ -70,15 +89,6 @@ public void autonomousInit() { */ @Override public void autonomousPeriodic() { - switch (m_autoSelected) { - case kCustomAuto: - // Put custom auto code here - break; - case kDefaultAuto: - default: - // Put default auto code here - break; - } } /** @@ -86,6 +96,8 @@ public void autonomousPeriodic() { */ @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; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/core/ControlSystem.java b/src/main/java/frc/robot/core/ControlSystem.java new file mode 100644 index 0000000..518f888 --- /dev/null +++ b/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; + + } + + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/core/Drivetrain.java b/src/main/java/frc/robot/core/Drivetrain.java new file mode 100644 index 0000000..cf0115c --- /dev/null +++ b/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); + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/core/Launcher.java b/src/main/java/frc/robot/core/Launcher.java new file mode 100644 index 0000000..fbec904 --- /dev/null +++ b/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. + + + } + } +} diff --git a/src/main/java/frc/robot/utils/temp.txt b/src/main/java/frc/robot/utils/temp.txt new file mode 100644 index 0000000..e69de29