From 6a7c805066e58b838b2cb1200ae794c1df429263 Mon Sep 17 00:00:00 2001 From: Brian Semrau Date: Tue, 27 Feb 2018 10:40:42 -0500 Subject: [PATCH] Modified controls and elevator operation --- .../usfirst/frc/team4737/lib/F310Gamepad.java | 12 ++-- src/org/usfirst/frc/team4737/lib/Gamepad.java | 13 ++-- .../frc/team4737/lib/XboxController.java | 14 +++-- src/org/usfirst/frc/team4737/robot/OI.java | 17 +++-- .../usfirst/frc/team4737/robot/RobotMap.java | 24 ++++--- .../commands/drivetrain/RelaxDrivetrain.java | 37 +++-------- .../drivetrain/TeleopRacingDrive.java | 46 ++++++++++++++ .../commands/drivetrain/TeleopTankDrive.java | 63 ++++++++++--------- .../commands/elevator/ControlElevator.java | 26 +++++--- .../robot/commands/elevator/HoldElevator.java | 15 +++-- .../commands/elevator/RelaxElevator.java | 2 + .../robot/commands/intake/ControlIntake.java | 2 +- .../team4737/robot/subsystems/Drivetrain.java | 10 ++- .../team4737/robot/subsystems/Elevator.java | 22 ++++++- 14 files changed, 193 insertions(+), 110 deletions(-) create mode 100644 src/org/usfirst/frc/team4737/robot/commands/drivetrain/TeleopRacingDrive.java diff --git a/src/org/usfirst/frc/team4737/lib/F310Gamepad.java b/src/org/usfirst/frc/team4737/lib/F310Gamepad.java index 523fca4..5fda677 100644 --- a/src/org/usfirst/frc/team4737/lib/F310Gamepad.java +++ b/src/org/usfirst/frc/team4737/lib/F310Gamepad.java @@ -62,13 +62,17 @@ public F310Gamepad(int usbPort) { R3 = new GamepadButton(this, "R3", 10); registerButton(R3, "RS"); - LS = new Thumbstick(this, "LS", 0, 1); - RS = new Thumbstick(this, "RS", 4, 5); + LS = new Thumbstick(this, "LS", 0, 1, false, true); + LS.X.setDeadzone(0.1); + LS.Y.setDeadzone(0.1); + RS = new Thumbstick(this, "RS", 4, 5, false, true); + RS.X.setDeadzone(0.1); + RS.Y.setDeadzone(0.1); // LT = new LogitechGamepadTriggerAxis(this, "LT", 2, true); // RT = new LogitechGamepadTriggerAxis(this, "RT", 3, false); - LT = new GamepadAxis(this, "LT", 2); - RT = new GamepadAxis(this, "RT", 3); + LT = new GamepadAxis(this, "LT", 2, false); + RT = new GamepadAxis(this, "RT", 3, false); DPAD = new DPad(this, "DPAD", 0); } diff --git a/src/org/usfirst/frc/team4737/lib/Gamepad.java b/src/org/usfirst/frc/team4737/lib/Gamepad.java index 316f67a..d459655 100644 --- a/src/org/usfirst/frc/team4737/lib/Gamepad.java +++ b/src/org/usfirst/frc/team4737/lib/Gamepad.java @@ -55,10 +55,13 @@ public class GamepadAxis extends Axis { private final int axis; private double deadzone; + + private boolean inverted; - public GamepadAxis(Gamepad gamepad, String name, int axis) { + public GamepadAxis(Gamepad gamepad, String name, int axis, boolean inverted) { super(gamepad, name); this.axis = axis; + this.inverted = inverted; gamepad.registerAxis(this, name); } @@ -75,7 +78,7 @@ public double getRaw() { @Override public double get() { - double raw = getRaw(); + double raw = inverted ? -getRaw() : getRaw(); if (raw < -deadzone) { return scale(raw, -1, -deadzone, -1, 0); } else if (raw > deadzone) { @@ -223,9 +226,9 @@ public class Thumbstick { private String name; - public Thumbstick(Gamepad gamepad, String name, int axisX, int axisY) { - X = new GamepadAxis(gamepad, name + "_X", axisX); - Y = new GamepadAxis(gamepad, name + "_Y", axisY); + public Thumbstick(Gamepad gamepad, String name, int axisX, int axisY, boolean invertX, boolean invertY) { + X = new GamepadAxis(gamepad, name + "_X", axisX, invertX); + Y = new GamepadAxis(gamepad, name + "_Y", axisY, invertY); this.name = name; diff --git a/src/org/usfirst/frc/team4737/lib/XboxController.java b/src/org/usfirst/frc/team4737/lib/XboxController.java index 00ee6e9..631ffea 100644 --- a/src/org/usfirst/frc/team4737/lib/XboxController.java +++ b/src/org/usfirst/frc/team4737/lib/XboxController.java @@ -37,11 +37,15 @@ public XboxController(int usbPort) { R3 = new GamepadButton(this, "R3", 10); registerButton(R3, "RS"); - LS = new Thumbstick(this, "LS", 0, 1); - RS = new Thumbstick(this, "RS", 4, 5); - - LT = new GamepadAxis(this, "LT", 2); - RT = new GamepadAxis(this, "RT", 3); + LS = new Thumbstick(this, "LS", 0, 1, false, false); + LS.X.setDeadzone(0.1); + LS.Y.setDeadzone(0.1); + RS = new Thumbstick(this, "RS", 4, 5, false, false); + RS.X.setDeadzone(0.1); + RS.Y.setDeadzone(0.1); + + LT = new GamepadAxis(this, "LT", 2, false); + RT = new GamepadAxis(this, "RT", 3, false); DPAD = new DPad(this, "DPAD", 0); } diff --git a/src/org/usfirst/frc/team4737/robot/OI.java b/src/org/usfirst/frc/team4737/robot/OI.java index 0228e56..646d6fb 100644 --- a/src/org/usfirst/frc/team4737/robot/OI.java +++ b/src/org/usfirst/frc/team4737/robot/OI.java @@ -8,13 +8,12 @@ package org.usfirst.frc.team4737.robot; import org.usfirst.frc.team4737.lib.Gamepad; +import org.usfirst.frc.team4737.lib.XboxController; import org.usfirst.frc.team4737.lib.F310Gamepad; -import org.usfirst.frc.team4737.robot.commands.drivetrain.TeleopTankDrive; +import org.usfirst.frc.team4737.robot.commands.drivetrain.TeleopRacingDrive; import org.usfirst.frc.team4737.robot.commands.elevator.ControlElevator; +import org.usfirst.frc.team4737.robot.commands.elevator.RelaxElevator; import org.usfirst.frc.team4737.robot.commands.intake.ControlIntake; -import org.usfirst.frc.team4737.robot.commands.intake.ReverseIntake; -import org.usfirst.frc.team4737.robot.commands.intake.RunIntake; -import org.usfirst.frc.team4737.robot.commands.intake.TwistIntake; import edu.wpi.first.wpilibj.buttons.Trigger; @@ -55,12 +54,11 @@ public class OI { public Gamepad operator; public OI() { - driver = new F310Gamepad(0); + driver = new XboxController(0); operator = new F310Gamepad(1); // User override to take control of the intake new Trigger() { - @Override public boolean get() { return operator.getAxis("LT").get() != 0 || operator.getAxis("RT").get() != 0 || operator.getAxis("RS_X").get() != 0; @@ -69,20 +67,21 @@ public boolean get() { // User override to take control of driving new Trigger() { - @Override public boolean get() { return driver.getThumbstick("LS").Y.get() != 0 || driver.getThumbstick("RS").Y.get() != 0; } - }.whileActive(new TeleopTankDrive()); + }.whileActive(new TeleopRacingDrive()); // User override to take control of the elevator new Trigger() { - @Override public boolean get() { return operator.getAxis("LS_Y").get() != 0; } }.whileActive(new ControlElevator()); + // Allow operator to instantly relax elevator in case of motor overheat + operator.getButton("Y").whileHeld(new RelaxElevator()); + // operator.getDPad("DPAD").LEFT.whileActive(new TwistIntake(1)); // operator.getDPad("DPAD").RIGHT.whileActive(new TwistIntake(-1)); diff --git a/src/org/usfirst/frc/team4737/robot/RobotMap.java b/src/org/usfirst/frc/team4737/robot/RobotMap.java index e4fd1da..ec7dec5 100644 --- a/src/org/usfirst/frc/team4737/robot/RobotMap.java +++ b/src/org/usfirst/frc/team4737/robot/RobotMap.java @@ -15,6 +15,10 @@ */ public class RobotMap { + // ################ + // TalonSRX CAN IDs + // ################ + // Drivetrain public static final int DRIVE_LEFT_MASTER = 11; public static final int DRIVE_LEFT_SLAVE = 12; @@ -28,16 +32,18 @@ public class RobotMap { // Elevator public static final int ELEVATOR_MOTOR = 17; + // ######### + // Constants + // ######### + /** - * The stall current of the elevator when fully extended - */ - public static final double ELEVATOR_TOPSTALL_AMPS = 40; // TODO measure - /** - * The power (-1 to 1) to give to the elevator for it to hold position with a - * POWER CUBE + * The voltage to give to the elevator for it to hold constant position with a + * cube */ - public static final double ELEVATOR_HOLD_PCT = 0; // TODO measure - - public static final double ELEVATOR_HOLD_TIME = 3; + public static final double ELEVATOR_HOLD_V = 1; // TODO measure + public static final double ELEVATOR_HOLD_TIME = 4; // TODO tune + public static final double ELEVATOR_MAX_DOWN_SPEED = -0.6; // TODO tune + + public static final double DRIVE_SLOW_SCALE = 0.5; } diff --git a/src/org/usfirst/frc/team4737/robot/commands/drivetrain/RelaxDrivetrain.java b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/RelaxDrivetrain.java index 5207607..564a318 100644 --- a/src/org/usfirst/frc/team4737/robot/commands/drivetrain/RelaxDrivetrain.java +++ b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/RelaxDrivetrain.java @@ -2,38 +2,21 @@ import org.usfirst.frc.team4737.robot.Robot; -import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.command.InstantCommand; /** * */ -public class RelaxDrivetrain extends Command { +public class RelaxDrivetrain extends InstantCommand { - public RelaxDrivetrain() { - requires(Robot.DRIVETRAIN); - } + public RelaxDrivetrain() { + requires(Robot.DRIVETRAIN); + this.setRunWhenDisabled(true); + } - // Called just before this Command runs the first time - protected void initialize() { - Robot.DRIVETRAIN.setCoastMode(); - } + // Called just before this Command runs the first time + protected void initialize() { + Robot.DRIVETRAIN.setCoastMode(); + } - // Called repeatedly when this Command is scheduled to run - protected void execute() { - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return false; - } - - // Called once after isFinished returns true - protected void end() { - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - } - } diff --git a/src/org/usfirst/frc/team4737/robot/commands/drivetrain/TeleopRacingDrive.java b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/TeleopRacingDrive.java new file mode 100644 index 0000000..5fd8fa8 --- /dev/null +++ b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/TeleopRacingDrive.java @@ -0,0 +1,46 @@ +package org.usfirst.frc.team4737.robot.commands.drivetrain; + +import org.usfirst.frc.team4737.robot.Robot; +import org.usfirst.frc.team4737.robot.RobotMap; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class TeleopRacingDrive extends Command { + + public TeleopRacingDrive() { + requires(Robot.DRIVETRAIN); + } + + // Called just before this Command runs the first time + protected void initialize() { + Robot.DRIVETRAIN.setBrakeMode(); + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + boolean slow = Robot.OI.driver.getButton("LB").get(); + double throttle = (Robot.OI.driver.getAxis("RT").get() - Robot.OI.driver.getAxis("LT").get()) + * (slow ? RobotMap.DRIVE_SLOW_SCALE : 1); + double steer = Robot.OI.driver.getThumbstick("LS").X.get() * (slow ? RobotMap.DRIVE_SLOW_SCALE : 1); + + Robot.DRIVETRAIN.arcadeDrive(throttle, steer); + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return false; + } + + // Called once after isFinished returns true + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + } + +} diff --git a/src/org/usfirst/frc/team4737/robot/commands/drivetrain/TeleopTankDrive.java b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/TeleopTankDrive.java index 990c8ac..213448a 100644 --- a/src/org/usfirst/frc/team4737/robot/commands/drivetrain/TeleopTankDrive.java +++ b/src/org/usfirst/frc/team4737/robot/commands/drivetrain/TeleopTankDrive.java @@ -9,35 +9,36 @@ */ public class TeleopTankDrive extends Command { - public TeleopTankDrive() { - // Use requires() here to declare subsystem dependencies - requires(Robot.DRIVETRAIN); - } - - // Called just before this Command runs the first time - protected void initialize() { - Robot.DRIVETRAIN.setBrakeMode(); - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - Robot.DRIVETRAIN.tankDrive( - -Robot.OI.driver.getThumbstick("LS").Y.get(), - -Robot.OI.driver.getThumbstick("RS").Y.get()); - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return false; - } - - // Called once after isFinished returns true - protected void end() { - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - } - + public TeleopTankDrive() { + // Use requires() here to declare subsystem dependencies + requires(Robot.DRIVETRAIN); + } + + // Called just before this Command runs the first time + protected void initialize() { + Robot.DRIVETRAIN.setBrakeMode(); + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + boolean slow = Robot.OI.driver.getButton("RB").get(); + Robot.DRIVETRAIN.tankDrive( + -Robot.OI.driver.getThumbstick("LS").Y.get() * (slow ? 0.6 : 1), + -Robot.OI.driver.getThumbstick("RS").Y.get() * (slow ? 0.6 : 1)); + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return false; + } + + // Called once after isFinished returns true + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + } + } diff --git a/src/org/usfirst/frc/team4737/robot/commands/elevator/ControlElevator.java b/src/org/usfirst/frc/team4737/robot/commands/elevator/ControlElevator.java index 51805e4..fe97da7 100644 --- a/src/org/usfirst/frc/team4737/robot/commands/elevator/ControlElevator.java +++ b/src/org/usfirst/frc/team4737/robot/commands/elevator/ControlElevator.java @@ -4,7 +4,6 @@ import org.usfirst.frc.team4737.robot.RobotMap; import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** * @@ -17,6 +16,7 @@ public ControlElevator() { // Called just before this Command runs the first time protected void initialize() { + Robot.ELEVATOR.setBrakeMode(); } // Called repeatedly when this Command is scheduled to run @@ -25,19 +25,27 @@ protected void execute() { double input = Robot.OI.operator.getAxis("LS_Y").get(); // Check if we're hitting the top - if (input > 0 && Robot.ELEVATOR.getMotorCurrent() > RobotMap.ELEVATOR_TOPSTALL_AMPS) { - input = 0; - } - - Robot.ELEVATOR.setSpeed(input + RobotMap.ELEVATOR_HOLD_PCT); + // if (input > 0 && Robot.ELEVATOR.isAtTop()) { + // input = 0; + // } - SmartDashboard.putNumber("elevator_input", input); + // Make sure we don't go down too fast + if (input < RobotMap.ELEVATOR_MAX_DOWN_SPEED) { + input = RobotMap.ELEVATOR_MAX_DOWN_SPEED; + } // Start hold command if no input if (input == 0) { - new HoldElevator().start(); - this.cancel(); + if (this.isRunning()) { + new HoldElevator().start(); + this.cancel(); + } + return; } + + // Set elevator speed + Robot.ELEVATOR.setSpeed(input); + } // Make this return true when this Command no longer needs to run execute() diff --git a/src/org/usfirst/frc/team4737/robot/commands/elevator/HoldElevator.java b/src/org/usfirst/frc/team4737/robot/commands/elevator/HoldElevator.java index e7d3b2e..1225ac0 100644 --- a/src/org/usfirst/frc/team4737/robot/commands/elevator/HoldElevator.java +++ b/src/org/usfirst/frc/team4737/robot/commands/elevator/HoldElevator.java @@ -11,6 +11,8 @@ */ public class HoldElevator extends TimedCommand { + private boolean startedBuzz; + public HoldElevator() { super(RobotMap.ELEVATOR_HOLD_TIME); requires(Robot.ELEVATOR); @@ -18,26 +20,31 @@ public HoldElevator() { // Called just before this Command runs the first time protected void initialize() { + Robot.ELEVATOR.setHoldPosition(true); + Robot.ELEVATOR.setBrakeMode(); + + startedBuzz = false; } // Called repeatedly when this Command is scheduled to run protected void execute() { - Robot.ELEVATOR.setSpeed(RobotMap.ELEVATOR_HOLD_PCT); - // If one second remaining, buzz controller - if (RobotMap.ELEVATOR_HOLD_TIME - this.timeSinceInitialized() < 1.0) { - new BuzzController(1, 0.5, Robot.OI.operator).start(); + if (RobotMap.ELEVATOR_HOLD_TIME - this.timeSinceInitialized() < 1.0 && !startedBuzz) { + new BuzzController(1, 0.3, Robot.OI.operator).start(); + startedBuzz = true; } } // Called once after timeout protected void end() { new RelaxElevator().start(); + Robot.ELEVATOR.setHoldPosition(false); } // Called when another command which requires one or more of the same // subsystems is scheduled to run protected void interrupted() { + Robot.ELEVATOR.setHoldPosition(false); } } diff --git a/src/org/usfirst/frc/team4737/robot/commands/elevator/RelaxElevator.java b/src/org/usfirst/frc/team4737/robot/commands/elevator/RelaxElevator.java index c547920..707e831 100644 --- a/src/org/usfirst/frc/team4737/robot/commands/elevator/RelaxElevator.java +++ b/src/org/usfirst/frc/team4737/robot/commands/elevator/RelaxElevator.java @@ -16,10 +16,12 @@ public RelaxElevator() { // Called just before this Command runs the first time protected void initialize() { Robot.ELEVATOR.setCoastMode(); + Robot.ELEVATOR.setHoldPosition(false); } // Called repeatedly when this Command is scheduled to run protected void execute() { + Robot.ELEVATOR.setSpeed(0); } // Make this return true when this Command no longer needs to run execute() diff --git a/src/org/usfirst/frc/team4737/robot/commands/intake/ControlIntake.java b/src/org/usfirst/frc/team4737/robot/commands/intake/ControlIntake.java index 7d4e56c..fbd9256 100644 --- a/src/org/usfirst/frc/team4737/robot/commands/intake/ControlIntake.java +++ b/src/org/usfirst/frc/team4737/robot/commands/intake/ControlIntake.java @@ -19,7 +19,7 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run protected void execute() { - double speed = -Robot.OI.operator.getAxis("LT").get() + Robot.OI.operator.getAxis("RT").get(); + double speed = Robot.OI.operator.getAxis("LT").get() - Robot.OI.operator.getAxis("RT").get(); double twist = Robot.OI.operator.getAxis("RS_X").get(); Robot.INTAKE.setLRSpeed(speed - twist, speed + twist); } diff --git a/src/org/usfirst/frc/team4737/robot/subsystems/Drivetrain.java b/src/org/usfirst/frc/team4737/robot/subsystems/Drivetrain.java index 297e60d..c5c06bf 100644 --- a/src/org/usfirst/frc/team4737/robot/subsystems/Drivetrain.java +++ b/src/org/usfirst/frc/team4737/robot/subsystems/Drivetrain.java @@ -29,7 +29,7 @@ public Drivetrain() { leftBackSlave.follow(leftFrontMaster); rightBackSlave.follow(rightFrontMaster); - + leftFrontMaster.configOpenloopRamp(0.25, 30); rightFrontMaster.configOpenloopRamp(0.25, 30); @@ -47,14 +47,14 @@ public void setBrakeMode() { rightFrontMaster.setNeutralMode(NeutralMode.Brake); rightBackSlave.setNeutralMode(NeutralMode.Brake); } - + public void setCoastMode() { leftFrontMaster.setNeutralMode(NeutralMode.Coast); leftBackSlave.setNeutralMode(NeutralMode.Coast); rightFrontMaster.setNeutralMode(NeutralMode.Coast); rightBackSlave.setNeutralMode(NeutralMode.Coast); } - + /** * Controls the drivetrain using two tank-drive joystick inputs * @@ -66,5 +66,9 @@ public void setCoastMode() { public void tankDrive(double leftInput, double rightInput) { drive.tankDrive(leftInput, rightInput); } + + public void arcadeDrive(double throttle, double steer) { + drive.arcadeDrive(throttle, steer); + } } diff --git a/src/org/usfirst/frc/team4737/robot/subsystems/Elevator.java b/src/org/usfirst/frc/team4737/robot/subsystems/Elevator.java index 6bbb088..a3b9936 100644 --- a/src/org/usfirst/frc/team4737/robot/subsystems/Elevator.java +++ b/src/org/usfirst/frc/team4737/robot/subsystems/Elevator.java @@ -7,6 +7,7 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -21,8 +22,17 @@ public class Elevator extends Subsystem { private double lastCurrent; private final double retention = 0.1; + private boolean hold; + public Elevator() { motor = new WPI_TalonSRX(RobotMap.ELEVATOR_MOTOR); + motor.setInverted(true); + // Prevent motor from stalling too hard + motor.configContinuousCurrentLimit(30, 30); + motor.enableCurrentLimit(true); + // Use voltage compensation to keep inputs reliable +// motor.configVoltageCompSaturation(12, 30); +// motor.enableVoltageCompensation(true); } public void initDefaultCommand() { @@ -37,6 +47,8 @@ public void periodic() { lastCurrent = temp; SmartDashboard.putNumber("elevator_current", current); +// SmartDashboard.putNumber("elevator_voltage", motor.getMotorOutputVoltage()); + } public double getMotorCurrent() { @@ -49,13 +61,17 @@ public double getMotorCurrent() { * ranges from -1.0 to 1.0 */ public void setSpeed(double speed) { - motor.set(ControlMode.PercentOutput, speed); + motor.set(ControlMode.PercentOutput, speed + (hold ? RobotMap.ELEVATOR_HOLD_V / motor.getBusVoltage() : 0)); + } + + public void setHoldPosition(boolean hold) { + this.hold = hold; } - + public void setBrakeMode() { motor.setNeutralMode(NeutralMode.Brake); } - + public void setCoastMode() { motor.setNeutralMode(NeutralMode.Coast); }