Skip to content
This repository has been archived by the owner on Oct 11, 2022. It is now read-only.

Commit

Permalink
Operator board interface, no implementation (#14)
Browse files Browse the repository at this point in the history
- added operator board interface
- cleaned up several files
  • Loading branch information
thetaspirit authored Sep 13, 2021
1 parent 87e7a57 commit b975c53
Show file tree
Hide file tree
Showing 9 changed files with 411 additions and 270 deletions.
7 changes: 6 additions & 1 deletion .vscode/settings.json
Original file line number Diff line number Diff line change
Expand Up @@ -13,5 +13,10 @@
"**/.settings": true,
"**/.factorypath": true,
"**/*~": true
}
},
"cSpell.words": [
"AHRS",
"deadzone",
"odometry"
]
}
326 changes: 171 additions & 155 deletions src/main/java/frc/robot/HardwareMap.java

Large diffs are not rendered by default.

3 changes: 2 additions & 1 deletion src/main/java/frc/robot/Limelight.java
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,8 @@ public static boolean hasTarget() {
/**
* Sets limelight’s LED state
*
* @param state LED state (0-3)
* @param state 0: use LED Mode set in the current pipeline. 1: force off. 2:
* force blink. 3: force on.
*/
public static void setLed(double state) {
NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(state);
Expand Down
117 changes: 117 additions & 0 deletions src/main/java/frc/robot/OperatorBoard.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,117 @@
package frc.robot;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj2.command.button.Button;

/**
* A wrapper class for our custom-made operator board. Does not have a
* constructor. Access statically. Access Buttons via public static state.
* Access axis values via methods. Use methods to make buttons light up.
*/
public class OperatorBoard {

private static Joystick HARDWARE;
public OperatorBoardButton BUTTON_ONE;
public OperatorBoardButton BUTTON_TWO;
public OperatorBoardButton BUTTON_THREE;
public OperatorBoardButton BUTTON_SIX;
public OperatorBoardButton BUTTON_ELEVEN;
public OperatorBoardButton BUTTON_TWELVE;
public OperatorBoardButton BUTTON_THIRTEEN;
public OperatorBoardButton BUTTON_FOURTEEN;
// TODO rename these, and use lowercase camel case names

/**
* Constructs one operator board. Since we only have one operator board, this
* class should be treated as a singleton.
*
* @param port The USB port number for the controller
*/
public OperatorBoard(int port) {
HARDWARE = new Joystick(port);

BUTTON_ONE = new OperatorBoardButton(1);
BUTTON_TWO = new OperatorBoardButton(2);
BUTTON_THREE = new OperatorBoardButton(3);
BUTTON_SIX = new OperatorBoardButton(6);
BUTTON_ELEVEN = new OperatorBoardButton(11);
BUTTON_TWELVE = new OperatorBoardButton(12);
BUTTON_THIRTEEN = new OperatorBoardButton(13);
BUTTON_FOURTEEN = new OperatorBoardButton(14);
}

/**
* Access the x-axis of the left joystick.
*
* @return Left is negative, right is positive.
*/
public double getLeftJoystickX() {
return -HARDWARE.getRawAxis(0);
// This value is negated because something is weird with the wiring.
}

/**
* Access the y-axis of the left joystick.
*
* @return Up is negative, down is positive.
*/
public double getLeftJoystickY() {
return HARDWARE.getRawAxis(1);
}

/**
* Access the x-axis of the right joystick.
*
* @return Up is negative, down is positive.
*/
public double getRightJoystickX() {
return HARDWARE.getRawAxis(2);
}

/**
* Access the y-axis of the right joystick.
*
* @return Up is negative, down is positive.
*/
public double getRightJoystickY() {
return HARDWARE.getRawAxis(3);
}

/**
* A custom button class for the operator board.
*/
public class OperatorBoardButton extends Button {
private int m_buttonNumber;

/**
* Constructs an OperatorBoardButton with a designated numerical ID
* @param button The button's number - a sort of ID
*/
public OperatorBoardButton(int button) {
m_buttonNumber = button;
}

/**
* Returns whether the button is being pressed.
* @return Whether or not the button is being pressed.
*/
@Override
public boolean get() {
return HARDWARE.getRawButton(m_buttonNumber);
}

/**
* Turns the button's light on, and keeps it on until turnLightOff() is called.
*/
public void turnLightOn() {
HARDWARE.setOutput(m_buttonNumber, true);
}

/**
* Turns the button's light off, and keeps it off until turnLightOn() is called.
*/
public void turnLightOff() {
HARDWARE.setOutput(m_buttonNumber, false);
}
}
}
194 changes: 95 additions & 99 deletions src/main/java/frc/robot/RobotContainer.java
Original file line number Diff line number Diff line change
Expand Up @@ -7,10 +7,9 @@
import com.revrobotics.CANSparkMax.IdleMode;

import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.GenericHID.Hand;
import edu.wpi.first.wpilibj.XboxController.Axis;
import edu.wpi.first.wpilibj.XboxController.Button;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
Expand All @@ -20,11 +19,10 @@
import frc.robot.commands.IntakeCommand;
import frc.robot.commands.MoveArmCommand;
import frc.robot.commands.OuttakeCommand;
import frc.robot.commands.ShootOneBallCommand;
import frc.robot.commands.ShooterOffCommand;
import frc.robot.commands.ShooterOnCommand;
import frc.robot.subsystems.ClimberSubsystem;
import frc.robot.commands.SwerveJoystickCommand;
import frc.robot.subsystems.ClimberSubsystem;
import frc.robot.commands.VisionAimingCommand;
import frc.robot.subsystems.IntakeSubsystem;
import frc.robot.subsystems.ShooterSubsystem;
Expand All @@ -38,99 +36,97 @@
* commands, and button mappings) should be declared here.
*/
public class RobotContainer {
// The robot's subsystems and commands are defined here...

private HardwareMap hardwareMap = new HardwareMap();

private XboxController m_driveController = hardwareMap.inputHardware.driveController;
private XboxController m_operatorController = hardwareMap.inputHardware.operatorController;

private ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem(hardwareMap.shooterHardware);
private SwerveDriveSubsystem m_swerveSubsystem = new SwerveDriveSubsystem(hardwareMap.swerveDriveHardware);

private SwerveJoystickCommand m_swerveJoystickCommand = new SwerveJoystickCommand(m_swerveSubsystem,
m_driveController);

private IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem(hardwareMap.intakeHardware);
private MoveArmCommand m_moveArmCommand = new MoveArmCommand(m_operatorController, m_intakeSubsystem);

private ClimberSubsystem m_climberSubsystem = new ClimberSubsystem(hardwareMap.climberHardware);
private ClimberControllerCommand m_climberControllerCommand = new ClimberControllerCommand(m_climberSubsystem,
m_operatorController);

/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
// Configure the button bindings
configureButtonBindings();
m_climberSubsystem.setDefaultCommand(m_climberControllerCommand);

m_swerveSubsystem.setDefaultCommand(m_swerveJoystickCommand);

m_intakeSubsystem.setDefaultCommand(m_moveArmCommand);
}

/**
* Use this method to define your button->command mappings. Buttons can be
* created by instantiating a {@link GenericHID} or one of its subclasses
* ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {

// turns on shooter when Left Bumper is pressed
new JoystickButton(m_operatorController, Button.kBumperLeft.value)
.whenPressed(new ShooterOnCommand(m_shooterSubsystem));
// turns off shooter when Right Bumper is pressed
new JoystickButton(m_operatorController, Button.kBumperRight.value)
.whenPressed(new ShooterOffCommand(m_shooterSubsystem));
// turns on Feeder and shoots ball

new JoystickButton(m_operatorController, Button.kB.value)
.whenPressed(new RunCommand(() -> m_shooterSubsystem.turnFeederOn(), m_shooterSubsystem))
.whenReleased(new RunCommand(() -> m_shooterSubsystem.turnFeederOff(), m_shooterSubsystem));

// runs intake while left trigger is held
new Trigger(() -> m_operatorController.getTriggerAxis(Hand.kLeft) > 0.5)
.whileActiveOnce(new IntakeCommand(m_intakeSubsystem));
// runs outtake while right trigger is held
new Trigger(() -> m_operatorController.getTriggerAxis(Hand.kRight) > 0.5)
.whileActiveOnce(new OuttakeCommand(m_intakeSubsystem));

// locks ratchet so it cannot extend, only retract (press when hooked on and
// want to raise bot)
// do not try and extend at this point!! (might break hardware)
// potential to-do - add check in code so when ratchet locked cannot send signal
// to extend
new JoystickButton(m_operatorController, Button.kY.value)
.whenPressed(new InstantCommand(m_climberSubsystem::lockRatchet, m_climberSubsystem));
// release ratchet so it can extend and retract
new JoystickButton(m_operatorController, Button.kX.value)
.whenPressed(new InstantCommand(m_climberSubsystem::releaseRatchet, m_climberSubsystem));
// releases the Climber when Start is pressed
new JoystickButton(m_operatorController, Button.kStart.value)
.whenPressed(new InstantCommand(m_climberSubsystem::releaseClimber, m_climberSubsystem));

// resets the gyro when the Start button is pressed
new JoystickButton(m_driveController, Button.kStart.value)
.whenPressed(new InstantCommand(m_swerveSubsystem::resetGyro, m_swerveSubsystem));
// Sets brake and coast mode with left bumper
new JoystickButton(m_driveController, Button.kBumperLeft.value)
.whenPressed(() -> m_swerveSubsystem.setDriveIdleMode(IdleMode.kCoast))
.whenReleased(() -> m_swerveSubsystem.setDriveIdleMode(IdleMode.kBrake));
new JoystickButton(m_driveController, Button.kA.value)
.whileHeld(new VisionAimingCommand(m_swerveSubsystem, m_driveController));

}

/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
// An ExampleCommand will run in autonomous
return null;
}
// The robot's subsystems and commands are defined here...

private HardwareMap hardwareMap = new HardwareMap();

private XboxController m_driveController = hardwareMap.inputHardware.driveController;
private XboxController m_operatorController = hardwareMap.inputHardware.operatorController;

private ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem(hardwareMap.shooterHardware);
private SwerveDriveSubsystem m_swerveSubsystem = new SwerveDriveSubsystem(hardwareMap.swerveDriveHardware);
private IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem(hardwareMap.intakeHardware);
private ClimberSubsystem m_climberSubsystem = new ClimberSubsystem(hardwareMap.climberHardware);

private SwerveJoystickCommand m_swerveJoystickCommand = new SwerveJoystickCommand(m_swerveSubsystem,
m_driveController);
private MoveArmCommand m_moveArmCommand = new MoveArmCommand(m_operatorController, m_intakeSubsystem);
private ClimberControllerCommand m_climberControllerCommand = new ClimberControllerCommand(m_climberSubsystem,
m_operatorController);

/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
public RobotContainer() {
configureButtonBindings();
m_swerveSubsystem.setDefaultCommand(m_swerveJoystickCommand);
m_intakeSubsystem.setDefaultCommand(m_moveArmCommand);
m_climberSubsystem.setDefaultCommand(m_climberControllerCommand);
}

/**
* Use this method to define your button->command mappings. Buttons can be
* created by instantiating a {@link GenericHID} or one of its subclasses
* ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
* passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
*/
private void configureButtonBindings() {

// turns on shooter when Left Bumper is pressed
new JoystickButton(m_operatorController, Button.kBumperLeft.value)
.whenPressed(new ShooterOnCommand(m_shooterSubsystem));

// turns off shooter when Right Bumper is pressed
new JoystickButton(m_operatorController, Button.kBumperRight.value)
.whenPressed(new ShooterOffCommand(m_shooterSubsystem));

// turns on Feeder when B button is pressed and shoots ball
new JoystickButton(m_operatorController, Button.kB.value)
.whenPressed(new RunCommand(() -> m_shooterSubsystem.turnFeederOn(), m_shooterSubsystem))
.whenReleased(new RunCommand(() -> m_shooterSubsystem.turnFeederOff(), m_shooterSubsystem));

// runs the intake while the left trigger is held on the operator controller
new Trigger(() -> m_operatorController.getTriggerAxis(Hand.kLeft) > 0.5)
.whileActiveOnce(new IntakeCommand(m_intakeSubsystem));

// runs the outtake while the right trigger is held
new Trigger(() -> m_operatorController.getTriggerAxis(Hand.kRight) > 0.5)
.whileActiveOnce(new OuttakeCommand(m_intakeSubsystem));

// locks ratchet when Y button pressed so it cannot extend, only retract
// (press when hooked on and want to raise bot)
new JoystickButton(m_operatorController, Button.kY.value)
.whenPressed(new InstantCommand(m_climberSubsystem::lockRatchet, m_climberSubsystem));

// release ratchet when X button pressed so it can extend and retract
new JoystickButton(m_operatorController, Button.kX.value)
.whenPressed(new InstantCommand(m_climberSubsystem::releaseRatchet, m_climberSubsystem));

// releases the Climber when Start is pressed
new JoystickButton(m_operatorController, Button.kStart.value)
.whenPressed(new InstantCommand(m_climberSubsystem::releaseClimber, m_climberSubsystem));

// resets the gyro when the Start button is pressed
new JoystickButton(m_driveController, Button.kStart.value)
.whenPressed(new InstantCommand(m_swerveSubsystem::resetGyro, m_swerveSubsystem));

// Sets brake and coast mode with left bumper
new JoystickButton(m_driveController, Button.kBumperLeft.value)
.whenPressed(() -> m_swerveSubsystem.setDriveIdleMode(IdleMode.kCoast))
.whenReleased(() -> m_swerveSubsystem.setDriveIdleMode(IdleMode.kBrake));

new JoystickButton(m_driveController, Button.kA.value)
.whileHeld(new VisionAimingCommand(m_swerveSubsystem, m_driveController));

}

/**
* Use this to pass the autonomous command to the main {@link Robot} class.
*
* @return the command to run in autonomous
*/
public Command getAutonomousCommand() {
// An ExampleCommand will run in autonomous
return null;
}
}
9 changes: 7 additions & 2 deletions src/main/java/frc/robot/Utils.java
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,6 @@
/* the project. */
/*----------------------------------------------------------------------------*/


package frc.robot;

/**
Expand All @@ -29,12 +28,18 @@ public static double deadZones(double input, double deadZone) {
return input;
}

public static double tolerance (double value, double desiredVal, double tolerance) {
/**
* If "value" is within "tolerance" of "desiredVal," it returns desiredVal.
* </br>
* If "value" is <i>outside</i> the tolerance, then it returns "value."
*/
public static double tolerance(double value, double desiredVal, double tolerance) {
if (Math.abs(value - desiredVal) < tolerance) {
return desiredVal;
}
return value;
}

/**
* Makes lower inputs smaller which allows for finer joystick control.
*
Expand Down
Loading

0 comments on commit b975c53

Please sign in to comment.