diff --git a/src/com/team3925/robot2016/OI.java b/src/com/team3925/robot2016/OI.java index 00261e0..9bf4e85 100644 --- a/src/com/team3925/robot2016/OI.java +++ b/src/com/team3925/robot2016/OI.java @@ -52,9 +52,9 @@ public final class OI { until it is finished as determined by it's isFinished method. button.whenReleased(new ExampleCommand()); */ - public Joystick xboxDriver; - public Joystick xboxShooter; - public Joystick wheel; + public Joystick driverFlightstick; + public Joystick driverWheel; + public Joystick shooterXbox; public Button startCollectBall; public Button startThrowBallFar; @@ -75,40 +75,40 @@ public final class OI { public OI() { - xboxDriver = new Joystick(0); - xboxShooter = new Joystick(1); - wheel = new Joystick(2); + driverFlightstick = new Joystick(0); + driverWheel = new Joystick(1); + shooterXbox = new Joystick(2); collectBall = new CollectBall(); throwBallFar = new ThrowBall(Constants.LAUNCHER_THROWBALL_FAR_ANGLE, 1, 5); throwBallNear = new ThrowBall(Constants.LAUNCHER_THROWBALL_NEAR_ANGLE, 1, 5); throwBallLow = new ThrowBall(0, 1, 1); - startCollectBall = new JoystickButton(xboxShooter, XboxHelper.A); + startCollectBall = new JoystickButton(shooterXbox, XboxHelper.A); startCollectBall.whenPressed(collectBall); startCollectBall.cancelWhenPressed(throwBallNear); startCollectBall.cancelWhenPressed(throwBallFar); startCollectBall.cancelWhenPressed(throwBallLow); - startThrowBallFar = new JoystickButton(xboxShooter, XboxHelper.Y); + startThrowBallFar = new JoystickButton(shooterXbox, XboxHelper.Y); startThrowBallFar.whenPressed(throwBallFar); startThrowBallFar.cancelWhenPressed(collectBall); startThrowBallFar.cancelWhenPressed(throwBallNear); startThrowBallFar.cancelWhenPressed(throwBallLow); - startThrowBallNear = new JoystickButton(xboxShooter, XboxHelper.X); + startThrowBallNear = new JoystickButton(shooterXbox, XboxHelper.X); startThrowBallNear.whenPressed(throwBallNear); startThrowBallNear.cancelWhenPressed(throwBallFar); startThrowBallNear.cancelWhenPressed(collectBall); startThrowBallNear.cancelWhenPressed(throwBallLow); - startThrowBallLow = new JoystickButton(xboxShooter, XboxHelper.B); + startThrowBallLow = new JoystickButton(shooterXbox, XboxHelper.B); startThrowBallLow.whenPressed(throwBallLow); startThrowBallLow.cancelWhenPressed(throwBallFar); startThrowBallLow.cancelWhenPressed(collectBall); startThrowBallLow.cancelWhenPressed(throwBallNear); - cancelCommands = new JoystickButton(xboxShooter, XboxHelper.START); + cancelCommands = new JoystickButton(shooterXbox, XboxHelper.START); cancelCommands.cancelWhenPressed(collectBall); cancelCommands.cancelWhenPressed(throwBallFar); cancelCommands.cancelWhenPressed(throwBallNear); @@ -194,13 +194,11 @@ public CommandGroup setAutonomous() { // ROBOT BEHAVIOR public double getManualDrive_ForwardValue() { -// return xboxDriver.getRawAxis(1); return FlightStickHelper.getAxis(FlightStickHelper.AXIS_Y); } public boolean getVisionShoot_GyroTurnEnable() { - return XboxHelper.getDriverAxis(XboxHelper.AXIS_TRIGGER_LEFT) > 0.2 || - XboxHelper.getDriverAxis(XboxHelper.AXIS_TRIGGER_RIGHT) > 0.2; // slightly larger deadzone for safety + return false; //TODO add user input } public double getManualDrive_RotateValue() { @@ -208,7 +206,7 @@ public double getManualDrive_RotateValue() { } public boolean getManualDrive_HighGearToggle() { - return XboxHelper.getDriverButton(XboxHelper.TRIGGER_LT); + return false; //TODO Figure out what to do with this } public boolean getManualDrive_QuickTurn() { @@ -253,7 +251,7 @@ public boolean getManualArms_GetArmValue() { } public boolean getCommandCancel() { - return XboxHelper.getShooterButton(START) || XboxHelper.getDriverButton(START); + return XboxHelper.getShooterButton(START); } diff --git a/src/com/team3925/robot2016/Robot.java b/src/com/team3925/robot2016/Robot.java index 68d8092..80f2d80 100644 --- a/src/com/team3925/robot2016/Robot.java +++ b/src/com/team3925/robot2016/Robot.java @@ -6,15 +6,15 @@ import static com.team3925.robot2016.Constants.DO_LOG_PDP_VALUES; import com.kauailabs.navx.frc.AHRS; -import com.team3925.robot2016.commands.auto.GyroDrive; import com.team3925.robot2016.commands.auto.GyroTurn; import com.team3925.robot2016.subsystems.DriveTrain; import com.team3925.robot2016.subsystems.IntakeAssist; import com.team3925.robot2016.subsystems.Launcher; import com.team3925.robot2016.util.DriveTrainSignal; -import com.team3925.robot2016.util.PixyCmu5; import com.team3925.robot2016.util.SmartdashBoardLoggable; import com.team3925.robot2016.util.TimeoutAction; +import com.team3925.robot2016.util.hidhelpers.FlightStickHelper; +import com.team3925.robot2016.util.hidhelpers.ThrustmasterHelper; import com.team3925.robot2016.util.hidhelpers.XboxHelper; import edu.wpi.first.wpilibj.DriverStation; @@ -46,7 +46,6 @@ public class Robot extends IterativeRobot implements SmartdashBoardLoggable { public static PowerDistributionPanel pdp; public static OI oi; public static CheesyDriveHelper cdh; - public static PixyCmu5 pixy; private static TimeoutAction brakeBeforeMatchEnd = new TimeoutAction(); //Commands @@ -54,7 +53,6 @@ public class Robot extends IterativeRobot implements SmartdashBoardLoggable { // private ManualDrive manualDrive; // private ManualIntakeAssist manualIntakeAssist; private GyroTurn visionGyroTurn = null; - private GyroDrive testing_GyroDrive; //Variables public static double deltaTime = 0; @@ -73,11 +71,6 @@ public Robot() { } catch (RuntimeException e) { DriverStation.reportError("There was an error instantiating the NavxMXP!\n" + e.getMessage(), true); } - try { - pixy = new PixyCmu5(168, .25); - } catch (Exception e) { - DriverStation.reportError("There was an error instantiating the Pixy/Frames!" + e.getMessage(), true); - } } /** @@ -99,12 +92,13 @@ public void robotInit() { // constructed yet. Thus, their requires() statements may grab null // pointers. Bad news. Don't move it. oi = new OI(); - XboxHelper.init(); + XboxHelper.config(oi.shooterXbox); + FlightStickHelper.config(oi.driverFlightstick); + ThrustmasterHelper.config(oi.driverWheel); cdh = new CheesyDriveHelper(driveTrain); //Creating Commands visionGyroTurn = new GyroTurn(0); - testing_GyroDrive = new GyroDrive(); reset(); } diff --git a/src/com/team3925/robot2016/commands/auto/GyroDrive.java b/src/com/team3925/robot2016/commands/auto/GyroDrive.java index 550d5c1..dd4ec62 100644 --- a/src/com/team3925/robot2016/commands/auto/GyroDrive.java +++ b/src/com/team3925/robot2016/commands/auto/GyroDrive.java @@ -69,8 +69,8 @@ protected void execute() { } pidLoop.calculate(currentAngle + rotations*360); - // normally negative - driveTrain.arcadeDrive(-forwardPower, pidLoop.get(), false); + // normally negative for drive power | pid loop input was positive on comp robot + driveTrain.arcadeDrive(-forwardPower, -pidLoop.get(), false); // logic for slowing down or stopping here diff --git a/src/com/team3925/robot2016/subsystems/Launcher.java b/src/com/team3925/robot2016/subsystems/Launcher.java index 375fad3..0448109 100644 --- a/src/com/team3925/robot2016/subsystems/Launcher.java +++ b/src/com/team3925/robot2016/subsystems/Launcher.java @@ -5,19 +5,15 @@ import static com.team3925.robot2016.Constants.LAUNCHER_AIM_KP; import com.team3925.robot2016.Constants; -import com.team3925.robot2016.Robot; import com.team3925.robot2016.RobotMap; import com.team3925.robot2016.util.Loopable; import com.team3925.robot2016.util.MiscUtil; -import com.team3925.robot2016.util.PixyCmu5; -import com.team3925.robot2016.util.PixyCmu5.PixyFrame; import com.team3925.robot2016.util.SmartdashBoardLoggable; import com.team3925.robot2016.util.SynchronousPID; import edu.wpi.first.wpilibj.CANTalon; import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.DoubleSolenoid.Value; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -35,8 +31,6 @@ public final class Launcher extends Subsystem implements SmartdashBoardLoggable, private final CANTalon motorAim = RobotMap.launcherMotorAim; private final DoubleSolenoid puncherSolenoid = RobotMap.launcherPuncherSolenoid; private SynchronousPID aimPidLoop = new SynchronousPID(); - private final PixyCmu5 pixy = Robot.pixy; - private PixyFrame latestFrame = null; private double turnAngle = Double.NaN; private boolean aimEnabled = false, @@ -125,26 +119,6 @@ public void update() { motorLeft.set(-intakeSpeed); motorRight.set(intakeSpeed); - - // CAMERA - - if (pixy != null) { - try { - latestFrame = pixy.getCurrentframes().get(0); - } catch (Exception e) { - DriverStation.reportWarning("Could not retrieve latest camera frame!", false); - } - - if (pixy.isObjectDetected()) { - turnAngle = PixyCmu5.degreesXFromCenter(latestFrame); - } else { - turnAngle = Double.NaN; - } - - } else { - turnAngle = Double.NaN; - } - } /** @@ -231,17 +205,6 @@ public void logData() { putNumberSD("MotorAim_getPosition", motorAim.getPosition()); putNumberSD("MotorAim_getSetpoint", motorAim.getSetpoint()); - // CAMERA - - if (pixy != null) { - putBooleanSD("Vision_Detected_Target", pixy.isObjectDetected()); - - if (latestFrame != null) { - putBooleanSD("Vision_CanShoot", pixy.isInXCenter(latestFrame)); - putNumberSD("AngleToTarget", turnAngle); - } - } - } } diff --git a/src/com/team3925/robot2016/util/hidhelpers/FlightStickHelper.java b/src/com/team3925/robot2016/util/hidhelpers/FlightStickHelper.java index 765229f..7eca40a 100644 --- a/src/com/team3925/robot2016/util/hidhelpers/FlightStickHelper.java +++ b/src/com/team3925/robot2016/util/hidhelpers/FlightStickHelper.java @@ -36,6 +36,7 @@ private FlightStickHelper() {} public static void config(Joystick stick) { FlightStickHelper.stick = stick; + needsInit = false; } public static double getAxis(int axis) { diff --git a/src/com/team3925/robot2016/util/hidhelpers/ThrustmasterHelper.java b/src/com/team3925/robot2016/util/hidhelpers/ThrustmasterHelper.java index 364b6ac..9c7d88d 100644 --- a/src/com/team3925/robot2016/util/hidhelpers/ThrustmasterHelper.java +++ b/src/com/team3925/robot2016/util/hidhelpers/ThrustmasterHelper.java @@ -29,6 +29,7 @@ private ThrustmasterHelper() {} public static void config(Joystick wheel) { ThrustmasterHelper.wheel = wheel; + needsInit = false; } public static double getAxis(int axis) { diff --git a/src/com/team3925/robot2016/util/hidhelpers/XboxHelper.java b/src/com/team3925/robot2016/util/hidhelpers/XboxHelper.java index a0654a4..91f183d 100644 --- a/src/com/team3925/robot2016/util/hidhelpers/XboxHelper.java +++ b/src/com/team3925/robot2016/util/hidhelpers/XboxHelper.java @@ -1,7 +1,6 @@ package com.team3925.robot2016.util.hidhelpers; import com.team3925.robot2016.Constants; -import com.team3925.robot2016.Robot; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.Joystick.RumbleType; @@ -14,7 +13,6 @@ public class XboxHelper { private XboxHelper() {} - private static Joystick driver; private static Joystick shooter; public static final int @@ -36,41 +34,35 @@ private XboxHelper() {} AXIS_TRIGGER_LEFT = 2, AXIS_TRIGGER_RIGHT = 3; - private static boolean hasInit = false; + private static boolean needsInit = true; - public static void init() { - driver = Robot.oi.xboxDriver; - shooter = Robot.oi.xboxShooter; + public static void config(Joystick xbox) { + shooter = xbox; + needsInit = false; } public static double getShooterAxis(int axis) { - if (!hasInit) { init(); } - return Math.abs(shooter.getRawAxis(axis)) > Math.abs(Constants.AXIS_TOLERANCE) ? shooter.getRawAxis(axis) : 0; + if (needsInit) { + return 0d; + } else { + return Math.abs(shooter.getRawAxis(axis)) > Math.abs(Constants.AXIS_TOLERANCE) ? shooter.getRawAxis(axis) : 0; + } } public static boolean getShooterButton(int button) { - if (!hasInit) { init(); } - return shooter.getRawButton(button); + if (needsInit) { + return false; + } else { + return shooter.getRawButton(button); + } } public static double getShooterPOV() { - if (!hasInit) { init(); } - return shooter.getPOV(); - } - - public static double getDriverAxis(int axis) { - if (!hasInit) { init(); } - return Math.abs(driver.getRawAxis(axis)) > Math.abs(Constants.AXIS_TOLERANCE) ? driver.getRawAxis(axis) : 0; - } - - public static boolean getDriverButton(int button) { - if (!hasInit) { init(); } - return driver.getRawButton(button); - } - - public static double getDriverPOV() { - if (!hasInit) { init(); } - return driver.getPOV(); + if (needsInit) { + return 0d; + } else { + return shooter.getPOV(); + } } public static void setShooterRumble(float magnitude) { @@ -78,9 +70,4 @@ public static void setShooterRumble(float magnitude) { shooter.setRumble(RumbleType.kRightRumble, magnitude); } - public static void setDriverRumble(float magnitude) { - driver.setRumble(RumbleType.kLeftRumble, magnitude); - driver.setRumble(RumbleType.kRightRumble, magnitude); - } - }