Skip to content

Commit

Permalink
Merge branch 'OI/#252-NewOC' into carriage/#293-DisableFlag
Browse files Browse the repository at this point in the history
  • Loading branch information
TheMightyWarPig committed Apr 8, 2018
2 parents a0f7dc3 + 196eb20 commit 4edaa1f
Show file tree
Hide file tree
Showing 6 changed files with 317 additions and 12 deletions.
156 changes: 144 additions & 12 deletions src/main/java/org/frc5687/powerup/robot/OI.java
Original file line number Diff line number Diff line change
Expand Up @@ -11,9 +11,7 @@
import org.frc5687.powerup.robot.commands.actions.IntakeToScale;
import org.frc5687.powerup.robot.commands.actions.IntakeToSwitch;
import org.frc5687.powerup.robot.commands.auto.*;
import org.frc5687.powerup.robot.utils.Gamepad;
import org.frc5687.powerup.robot.utils.Helpers;
import org.frc5687.powerup.robot.utils.POV;
import org.frc5687.powerup.robot.utils.*;

public class OI {
public class ButtonNumbers {
Expand All @@ -25,6 +23,7 @@ public class ButtonNumbers {

private Joystick driverGamepad;
private Gamepad operatorGamepad;
private OperatorConsole console;

private JoystickButton intakeLeftIn;
private JoystickButton intakeRightIn;
Expand All @@ -51,14 +50,64 @@ public class ButtonNumbers {
private JoystickButton driverCarriageUp;
private JoystickButton driverCarriageDown;


private JoystickLight leftDriveMotorsLED;
private JoystickLight rightDriveMotorsLED;

private JoystickLight leftIntakeMotorLED;
private JoystickLight rightIntakeMotorLED;

private JoystickLight carriageMotorLED;

private JoystickLight armMotorLED;

private JoystickLight leftDriveEncoderLED;
private JoystickLight rightDriveEncoderLED;

private JoystickLight intakeBackIRLED;

private JoystickLight carriageEncoderLED;
private JoystickLight armPotLED;

private AxisButton climberHoldUp;
private AxisButton climberHoldDown;
private AxisButton intakeHoldUp;
private AxisButton intakeHoldDown;
private AxisButton carriageHoldUp;
private AxisButton carriageHoldDown;
private AxisButton armHoldUp;
private AxisButton armHoldDown;

private JoystickButton driveCapsOverride;
private JoystickButton driveLimitsOverride;

private JoystickButton climberKill;

private JoystickButton intakeIROverride;
private JoystickButton intakeKill;

private JoystickButton carriageZero;
private JoystickButton carriageCapsOverride;
private JoystickButton carriageLimitsOverride;
private JoystickButton carriageKill;

private JoystickButton armZeroTop;
private JoystickButton armZeroBottom;
private JoystickButton armCapsOverride;
private JoystickButton armLimitsOverride;
private JoystickButton armKill;



private POV driverPOV;

private Robot _robot;

public OI(Robot robot) {
_robot = robot;
driverGamepad = new Joystick(0);
driverGamepad = new Gamepad(0);
operatorGamepad = new Gamepad(1);
console = new OperatorConsole(2);

driverArmToScaleButton = new JoystickButton(driverGamepad, Gamepad.Buttons.Y.getNumber());
driverArmToSwitchButton = new JoystickButton(driverGamepad, Gamepad.Buttons.B.getNumber());
Expand Down Expand Up @@ -182,27 +231,110 @@ private double getSpeedFromAxis(Joystick gamepad, int axisNumber) {

public void initializeButtons(Robot robot) {
carriageZeroEncoder.whenPressed(new AutoZeroCarriage(robot.getCarriage()));
servoToggle.whenPressed(new ServoToggle(robot.getIntake()));

initializeDriverGamepad(robot);
initializeOperatorGamepad(robot);
initializeConsole(robot);

DriverStation.reportError("driverArmToIntakeButton " + (driverArmToIntakeButton ==null), false);
DriverStation.reportError("driverArmToDriveButton " + (driverArmToDriveButton ==null), false);
DriverStation.reportError("driverArmToSwitchButton " + (driverArmToSwitchButton ==null), false);
DriverStation.reportError("driverArmToScaleButton " + (driverArmToScaleButton ==null), false);

DriverStation.reportError("robot " + (robot==null), false);
DriverStation.reportError("carriage " + (robot.getCarriage()==null), false);
DriverStation.reportError("arm " + (robot.getArm()==null), false);
}

private void initializeDriverGamepad(Robot robot) {
driverArmToIntakeButton.whenPressed(new IntakeToFloor(robot.getCarriage(), robot.getArm()));
driverArmToDriveButton.whenPressed(new IntakeToDrive(robot.getCarriage(), robot.getArm()));
driverArmToSwitchButton.whenPressed(new IntakeToSwitch(robot.getCarriage(), robot.getArm()));
driverArmToScaleButton.whenPressed(new IntakeToScale(robot.getCarriage(), robot.getArm()));

}

private void initializeOperatorGamepad(Robot robot) {
operatorArmToIntakeButton.whenPressed(new IntakeToFloor(robot.getCarriage(), robot.getArm()));
operatorArmToDriveButton.whenPressed(new IntakeToDrive(robot.getCarriage(), robot.getArm()));
operatorArmToSwitchButton.whenPressed(new IntakeToSwitch(robot.getCarriage(), robot.getArm()));
operatorArmToScaleButton.whenPressed(new IntakeToScale(robot.getCarriage(), robot.getArm()));

servoToggle.whenPressed(new ServoToggle(robot.getIntake()));
}

DriverStation.reportError("driverArmToIntakeButton " + (driverArmToIntakeButton ==null), false);
DriverStation.reportError("driverArmToDriveButton " + (driverArmToDriveButton ==null), false);
DriverStation.reportError("driverArmToSwitchButton " + (driverArmToSwitchButton ==null), false);
DriverStation.reportError("driverArmToScaleButton " + (driverArmToScaleButton ==null), false);
private void initializeConsole(Robot robot) {
// LEDs
leftDriveMotorsLED = new JoystickLight(console, OperatorConsole.LEDs.D.getNumber());
rightDriveMotorsLED = new JoystickLight(console, OperatorConsole.LEDs.E.getNumber());

DriverStation.reportError("robot " + (robot==null), false);
DriverStation.reportError("carriage " + (robot.getCarriage()==null), false);
DriverStation.reportError("arm " + (robot.getArm()==null), false);
leftIntakeMotorLED = new JoystickLight(console, OperatorConsole.LEDs.F.getNumber());
rightIntakeMotorLED = new JoystickLight(console, OperatorConsole.LEDs.A.getNumber());

carriageMotorLED = new JoystickLight(console, OperatorConsole.LEDs.B.getNumber());

armMotorLED = new JoystickLight(console, OperatorConsole.LEDs.C.getNumber());

/*
leftDriveEncoderLED = new JoystickLight(console, OperatorConsole.LEDs.H.getNumber());
rightDriveEncoderLED = new JoystickLight(console, OperatorConsole.LEDs.G.getNumber());
intakeBackIRLED = new JoystickLight(console, OperatorConsole.LEDs.I.getNumber());
carriageEncoderLED = new JoystickLight(console, OperatorConsole.LEDs.J.getNumber());
armPotLED = new JoystickLight(console, OperatorConsole.LEDs.K.getNumber());
*/

climberHoldUp = new AxisButton(console, OperatorConsole.Axes.A.getNumber(), 0.75, 1.0);
climberHoldDown = new AxisButton(console, OperatorConsole.Axes.A.getNumber(), 0.25, 0.5);

intakeHoldUp = new AxisButton(console, OperatorConsole.Axes.B.getNumber(), 0.75, 1.0);
intakeHoldDown = new AxisButton(console, OperatorConsole.Axes.B.getNumber(), 0.25, 0.5);

carriageHoldUp = new AxisButton(console, OperatorConsole.Axes.C.getNumber(), 0.75, 1.0);
carriageHoldDown = new AxisButton(console, OperatorConsole.Axes.C.getNumber(), 0.25, 0.5);

armHoldUp = new AxisButton(console, OperatorConsole.Axes.D.getNumber(), 0.75, 1.0);
armHoldDown = new AxisButton(console, OperatorConsole.Axes.D.getNumber(), 0.25, 0.5);

driveCapsOverride = new JoystickButton(console, OperatorConsole.Buttons.A.getNumber());
driveLimitsOverride= new JoystickButton(console, OperatorConsole.Buttons.B.getNumber());;

climberKill = new JoystickButton(console, OperatorConsole.Buttons.C.getNumber());;

intakeIROverride = new JoystickButton(console, OperatorConsole.Buttons.D.getNumber());;
intakeKill = new JoystickButton(console, OperatorConsole.Buttons.E.getNumber());;

carriageZero = new JoystickButton(console, OperatorConsole.Buttons.F.getNumber());;
carriageCapsOverride = new JoystickButton(console, OperatorConsole.Buttons.G.getNumber());;
carriageLimitsOverride = new JoystickButton(console, OperatorConsole.Buttons.H.getNumber());;
carriageKill = new JoystickButton(console, OperatorConsole.Buttons.I.getNumber());;

armZeroTop = new JoystickButton(console, OperatorConsole.Buttons.J.getNumber());;
armZeroBottom = new JoystickButton(console, OperatorConsole.Buttons.K.getNumber());;
armCapsOverride = new JoystickButton(console, OperatorConsole.Buttons.L.getNumber());;
armLimitsOverride = new JoystickButton(console, OperatorConsole.Buttons.M.getNumber());;
armKill = new JoystickButton(console, OperatorConsole.Buttons.N.getNumber());;
}


public void updateConsole() {
/*
leftDriveMotorsLED.set(!_robot.getDriveTrain().isLeftHealthy());
rightDriveMotorsLED.set(!_robot.getDriveTrain().isRightHealthy());
leftIntakeMotorLED.set(!_robot.getIntake().isLeftHealthy());
rightIntakeMotorLED.set(!_robot.getIntake().isRightHealthy());
carriageMotorLED.set(_robot.getCarriage().isHealthy());
*/
int pos = (int)SmartDashboard.getNumber("DB/Slider 3", 0);

if (pos!=0) {
console.setOutput(Math.abs(pos), pos>0);
}

armMotorLED.set(_robot.getArm().isHealthy());
}

public void setDriverGamepadRumble(double leftIntensity, double rightIntensity) {
Expand Down
3 changes: 3 additions & 0 deletions src/main/java/org/frc5687/powerup/robot/Robot.java
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
import org.frc5687.powerup.robot.utils.JeVoisProxy;
import org.frc5687.powerup.robot.utils.LidarProxy;
import org.frc5687.powerup.robot.utils.PDP;
import sun.util.resources.cldr.or.CalendarData_or_IN;

public class Robot extends TimedRobot {

Expand Down Expand Up @@ -227,6 +228,8 @@ public void updateDashboard() {
intake.updateDashboard();
_climber.updateDashboard();
estimateIntakeHeight();

oi.updateConsole();
updateTick = 0;
}
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -400,6 +400,13 @@ public void updateDashboard() {

}

public boolean isLeftHealthy() {
return _leftMasterHC.IsHealthy() && _leftFollowerHC.IsHealthy();
}

public boolean isRightHealthy() {
return _rightMasterHC.IsHealthy() && _rightFollowerHC.IsHealthy();
}

private boolean checkCIM(double priorSpeed, boolean lost, int pdpChannel, String side, String pos) {
double currentDraw = _robot.getPDP().getCurrent(pdpChannel);
Expand Down
29 changes: 29 additions & 0 deletions src/main/java/org/frc5687/powerup/robot/utils/AxisButton.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,29 @@
package org.frc5687.powerup.robot.utils;

import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.buttons.Button;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
* Created by Ben Bernard on 3/15/2018.
*/
public class AxisButton extends Button {
private final GenericHID m_joystick;
private final int m_axisNumber;
private double _minRange;
private double _maxRange;

public AxisButton(GenericHID joystick, int axisNumber, double minRange, double maxRange) {
this.m_joystick = joystick;
this.m_axisNumber = axisNumber;
_minRange = minRange;
_maxRange = maxRange;
}

public boolean get() {
double val = this.m_joystick.getRawAxis(this.m_axisNumber);
return val >= _minRange && val <= _maxRange;
}


}
41 changes: 41 additions & 0 deletions src/main/java/org/frc5687/powerup/robot/utils/JoystickLight.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,41 @@
package org.frc5687.powerup.robot.utils;

/**
* Created by Ben Bernard on 3/14/2018.
*/
import edu.wpi.first.wpilibj.Joystick;

/**
* Utility class to make it easier to set (and read) the state of a joystick light
*/
public class JoystickLight {

private final Joystick m_joystick;
private final int m_outputNumber;
private boolean m_value = false;

/**
* Create a joystick button for triggering commands.
*
* @param joystick The Joystick object that has the lights (e.g. Joystick, Launchpad, etc)
* @param outputNumber The output number
*/
public JoystickLight(Joystick joystick, int outputNumber) {
m_joystick = joystick;
m_outputNumber = outputNumber;
}

/**
* Gets the value of the joystick button.
*
* @return The value of the joystick button
*/
public boolean get() {
return m_value;
}

public void set(boolean value) {
m_joystick.setOutput(m_outputNumber, m_value = value);
}

}
Loading

0 comments on commit 4edaa1f

Please sign in to comment.