diff --git a/Robot2019/src/main/java/frc/robot/OI.java b/Robot2019/src/main/java/frc/robot/OI.java index 1c306c2..331c0fa 100644 --- a/Robot2019/src/main/java/frc/robot/OI.java +++ b/Robot2019/src/main/java/frc/robot/OI.java @@ -18,9 +18,11 @@ import frc.robot.commands.ToggleHatch; import frc.robot.commands.ToggleCamera; import frc.robot.commands.ActuateClimberRails; +import frc.robot.commands.Climb; import frc.robot.subsystems.HatchPanel; import frc.robot.subsystems.Cargo; import frc.robot.subsystems.Climber; +import frc.robot.subsystems.Drivetrain; /** * This class is the glue that binds the controls on the physical operator @@ -33,10 +35,11 @@ public class OI { JoystickButton toggleHatchBtn; JoystickButton cargoIntakeBtn, cargoEjectBtn; JoystickButton climberRailBtn; - + JoystickButton climbBtn; JoystickButton toggleCameraBtn; - OI(Cargo cargo, HatchPanel hp, Climber climber, UsbCamera driveCamera, UsbCamera hatchCamera, VideoSink cameraServer) { + OI(Drivetrain dt, HatchPanel hp, Cargo cargo, Climber climber, UsbCamera driveCamera, UsbCamera hatchCamera, + VideoSink cameraServer) { leftJoy = new Joystick(0); rightJoy = new Joystick(1); manipulator = new Joystick(2); @@ -57,6 +60,9 @@ public class OI { climberRailBtn = new JoystickButton(manipulator, 5); // TODO: confirm button number climberRailBtn.whenPressed(new ActuateClimberRails(climber)); + climbBtn = new JoystickButton(manipulator, 4); // TODO: confirm button number + climbBtn.whenPressed(new Climb(climber, dt)); + toggleCameraBtn = new JoystickButton(leftJoy, 2); toggleCameraBtn.whenPressed(new ToggleCamera(driveCamera, hatchCamera, cameraServer)); } diff --git a/Robot2019/src/main/java/frc/robot/Robot.java b/Robot2019/src/main/java/frc/robot/Robot.java index 4845317..90941ad 100644 --- a/Robot2019/src/main/java/frc/robot/Robot.java +++ b/Robot2019/src/main/java/frc/robot/Robot.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.command.Scheduler; +import frc.robot.commands.TeleopDrive; import frc.robot.subsystems.Cargo; import frc.robot.subsystems.Climber; import frc.robot.subsystems.Drivetrain; @@ -23,15 +24,15 @@ public class Robot extends TimedRobot { @Override public void robotInit() { + dt = new Drivetrain(RobotMap.leftMaster, RobotMap.leftSlave1, RobotMap.leftSlave2, RobotMap.rightMaster, + RobotMap.rightSlave1, RobotMap.rightSlave2, RobotMap.leftEnc, RobotMap.rightEnc, RobotMap.ahrs); hp = new HatchPanel(RobotMap.hatchPistons); cargo = new Cargo(RobotMap.cargoRoller, RobotMap.pdp, RobotMap.cargoPDPPort); climber = new Climber(RobotMap.climberMotor, RobotMap.climberEncoder, RobotMap.ahrs, RobotMap.climberPistons); - - oi = new OI(cargo, hp, climber, RobotMap.driveCamera, RobotMap.hatchCamera, RobotMap.cameraServer); - dt = new Drivetrain(RobotMap.leftMaster, RobotMap.leftSlave1, RobotMap.leftSlave2, RobotMap.rightMaster, - RobotMap.rightSlave1, RobotMap.rightSlave2, oi.leftJoy, oi.rightJoy, RobotMap.leftEnc, RobotMap.rightEnc, - RobotMap.ahrs); + oi = new OI(dt, hp, cargo, climber, RobotMap.driveCamera, RobotMap.hatchCamera, RobotMap.cameraServer); + + dt.setDefaultCommand(new TeleopDrive(dt, oi.leftJoy, oi.rightJoy)); } /** diff --git a/Robot2019/src/main/java/frc/robot/commands/TeleopDrive.java b/Robot2019/src/main/java/frc/robot/commands/TeleopDrive.java index 7dcfd4d..8a4de96 100644 --- a/Robot2019/src/main/java/frc/robot/commands/TeleopDrive.java +++ b/Robot2019/src/main/java/frc/robot/commands/TeleopDrive.java @@ -7,12 +7,14 @@ package frc.robot.commands; +import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.subsystems.Drivetrain; public class TeleopDrive extends Command { Drivetrain dt; + Joystick leftJoy, rightJoy; /** * Handles all the teleoperated driving functionality @@ -20,9 +22,11 @@ public class TeleopDrive extends Command { * @param dt the Drivetrain object to use, passing it in is useful for testing * purposes */ - public TeleopDrive(Drivetrain dt) { + public TeleopDrive(Drivetrain dt, Joystick leftJoy, Joystick rightJoy) { requires(dt); this.dt = dt; + this.leftJoy = leftJoy; + this.rightJoy = rightJoy; } @Override @@ -35,8 +39,8 @@ protected void execute() { } private void arcadeDrive() { - double speed = -dt.leftJoy.getY(); - double rot = dt.rightJoy.getX(); + double speed = -leftJoy.getY(); + double rot = rightJoy.getX(); // System.out.println("Speed: " + speed + ", Rotation: " + rot); @@ -84,8 +88,8 @@ private void arcadeDrive() { } private void tankDrive() { - double left = -dt.leftJoy.getY(); - double right = -dt.rightJoy.getY(); + double left = -leftJoy.getY(); + double right = -rightJoy.getY(); if (SmartDashboard.getBoolean("Square Joysticks", true)) { left = Math.copySign(left * left, left); diff --git a/Robot2019/src/main/java/frc/robot/subsystems/Drivetrain.java b/Robot2019/src/main/java/frc/robot/subsystems/Drivetrain.java index 41955f9..f3b283d 100644 --- a/Robot2019/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/Robot2019/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -12,10 +12,8 @@ import com.kauailabs.navx.frc.AHRS; import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.commands.TeleopDrive; public class Drivetrain extends Subsystem { public enum Side { @@ -24,12 +22,11 @@ public enum Side { private WPI_TalonSRX leftMaster, rightMaster; private Encoder leftEnc, rightEnc; - public Joystick leftJoy, rightJoy; private AHRS ahrs; public Drivetrain(WPI_TalonSRX leftMaster, BaseMotorController leftSlave1, BaseMotorController leftSlave2, - WPI_TalonSRX rightMaster, BaseMotorController rightSlave1, BaseMotorController rightSlave2, Joystick leftJoy, - Joystick rightJoy, Encoder leftEnc, Encoder rightEnc, AHRS ahrs) { + WPI_TalonSRX rightMaster, BaseMotorController rightSlave1, BaseMotorController rightSlave2, Encoder leftEnc, + Encoder rightEnc, AHRS ahrs) { leftSlave1.follow(leftMaster); leftSlave2.follow(leftMaster); @@ -43,24 +40,23 @@ public Drivetrain(WPI_TalonSRX leftMaster, BaseMotorController leftSlave1, BaseM rightSlave1.setInverted(true); rightSlave2.setInverted(true); - this.leftJoy = leftJoy; - this.rightJoy = rightJoy; - this.leftEnc = leftEnc; this.rightEnc = rightEnc; - - double pulseFraction = 1.0/256; + + double pulseFraction = 1.0 / 256; double wheelDiameter = 5; leftEnc.setDistancePerPulse(pulseFraction * Math.PI * wheelDiameter); rightEnc.setDistancePerPulse(pulseFraction * Math.PI * wheelDiameter); this.ahrs = ahrs; - } + /** + * teleop drive initialized in Robot.robotInit() to avoid dependency loops + * between dt and oi + */ @Override public void initDefaultCommand() { - setDefaultCommand(new TeleopDrive(this)); } public void drive(double left, double right) { @@ -76,7 +72,8 @@ public void stop() { } public boolean isStalled() { - return leftMaster.getOutputCurrent() >= 30 || rightMaster.getOutputCurrent() >= 30; // TODO: Find value that actually works (test) + return leftMaster.getOutputCurrent() >= 30 || rightMaster.getOutputCurrent() >= 30; // TODO: Find value that + // actually works (test) } public double getEncDist(Side type) {