From 8c6983a3b38cc5735985f970165bae408c8eab15 Mon Sep 17 00:00:00 2001 From: Brian Semrau Date: Thu, 22 Feb 2018 14:43:02 -0500 Subject: [PATCH] Added elevator/intake control Also fixed tank drive, changed user inputs --- src/org/usfirst/frc/team4737/lib/Gamepad.java | 2 +- src/org/usfirst/frc/team4737/robot/OI.java | 45 ++++++++++++--- src/org/usfirst/frc/team4737/robot/Robot.java | 31 +++++----- .../usfirst/frc/team4737/robot/RobotMap.java | 19 +++++-- .../robot/commands/BuzzController.java | 40 +++++++++++++ .../robot/commands/ControlElevator.java | 57 +++++++++++++++++++ .../robot/commands/ControlIntake.java | 39 +++++++++++++ .../team4737/robot/commands/HoldElevator.java | 42 ++++++++++++++ .../robot/commands/RelaxElevator.java | 38 +++++++++++++ .../team4737/robot/commands/StopElevator.java | 1 - .../robot/commands/TeleopTankDrive.java | 4 +- .../team4737/robot/subsystems/Drivetrain.java | 11 ++-- .../team4737/robot/subsystems/Elevator.java | 33 ++++++++--- 13 files changed, 315 insertions(+), 47 deletions(-) create mode 100644 src/org/usfirst/frc/team4737/robot/commands/BuzzController.java create mode 100644 src/org/usfirst/frc/team4737/robot/commands/ControlElevator.java create mode 100644 src/org/usfirst/frc/team4737/robot/commands/ControlIntake.java create mode 100644 src/org/usfirst/frc/team4737/robot/commands/HoldElevator.java create mode 100644 src/org/usfirst/frc/team4737/robot/commands/RelaxElevator.java diff --git a/src/org/usfirst/frc/team4737/lib/Gamepad.java b/src/org/usfirst/frc/team4737/lib/Gamepad.java index 607677a..2111744 100644 --- a/src/org/usfirst/frc/team4737/lib/Gamepad.java +++ b/src/org/usfirst/frc/team4737/lib/Gamepad.java @@ -28,7 +28,7 @@ public String getNameID() { } - private abstract class Axis { + public abstract class Axis { protected final Gamepad gamepad; diff --git a/src/org/usfirst/frc/team4737/robot/OI.java b/src/org/usfirst/frc/team4737/robot/OI.java index 27eec7e..224628c 100644 --- a/src/org/usfirst/frc/team4737/robot/OI.java +++ b/src/org/usfirst/frc/team4737/robot/OI.java @@ -9,9 +9,13 @@ import org.usfirst.frc.team4737.lib.Gamepad; import org.usfirst.frc.team4737.lib.LogitechGamepad; +import org.usfirst.frc.team4737.robot.commands.ControlElevator; +import org.usfirst.frc.team4737.robot.commands.ControlIntake; import org.usfirst.frc.team4737.robot.commands.ReverseIntake; import org.usfirst.frc.team4737.robot.commands.RunIntake; +import org.usfirst.frc.team4737.robot.commands.TeleopTankDrive; +import edu.wpi.first.wpilibj.buttons.Trigger; /** * This class is the glue that binds the controls on the physical operator @@ -45,15 +49,38 @@ public class OI { // Start the command when the button is released and let it run the command // until it is finished as determined by it's isFinished method. // button.whenReleased(new ExampleCommand()); - - public Gamepad controller; - + + public Gamepad driver; + public Gamepad operator; + public OI() { - controller = new LogitechGamepad(0); - - controller.getButton("A").whileHeld(new RunIntake()); - controller.getButton("B").whileHeld(new ReverseIntake()); - + driver = new LogitechGamepad(0); + operator = new LogitechGamepad(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; + } + }.whileActive(new ControlIntake()); + + // 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()); + + // User override to take control of the elevator + new Trigger() { + @Override + public boolean get() { + return operator.getAxis("LS_Y").get() != 0; + } + }.whileActive(new ControlElevator()); + } - + } diff --git a/src/org/usfirst/frc/team4737/robot/Robot.java b/src/org/usfirst/frc/team4737/robot/Robot.java index 4b488b1..ec10d02 100644 --- a/src/org/usfirst/frc/team4737/robot/Robot.java +++ b/src/org/usfirst/frc/team4737/robot/Robot.java @@ -37,8 +37,8 @@ public class Robot extends TimedRobot { SendableChooser m_chooser = new SendableChooser<>(); /** - * This function is run when the robot is first started up and should be - * used for any initialization code. + * This function is run when the robot is first started up and should be used + * for any initialization code. */ @Override public void robotInit() { @@ -48,9 +48,9 @@ public void robotInit() { } /** - * This function is called once each time the robot enters Disabled mode. - * You can use it to reset any subsystem information you want to clear when - * the robot is disabled. + * This function is called once each time the robot enters Disabled mode. You + * can use it to reset any subsystem information you want to clear when the + * robot is disabled. */ @Override public void disabledInit() { @@ -64,25 +64,25 @@ public void disabledPeriodic() { /** * This autonomous (along with the chooser code above) shows how to select - * between different autonomous modes using the dashboard. The sendable - * chooser code works with the Java SmartDashboard. If you prefer the - * LabVIEW Dashboard, remove all of the chooser code and uncomment the - * getString code to get the auto name from the text box below the Gyro + * between different autonomous modes using the dashboard. The sendable chooser + * code works with the Java SmartDashboard. If you prefer the LabVIEW Dashboard, + * remove all of the chooser code and uncomment the getString code to get the + * auto name from the text box below the Gyro * *

* You can add additional auto modes by adding additional commands to the - * chooser code above (like the commented example) or additional comparisons - * to the switch structure below with additional strings & commands. + * chooser code above (like the commented example) or additional comparisons to + * the switch structure below with additional strings & commands. */ @Override public void autonomousInit() { m_autonomousCommand = m_chooser.getSelected(); /* - * String autoSelected = SmartDashboard.getString("Auto Selector", - * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand - * = new MyAutoCommand(); break; case "Default Auto": default: - * autonomousCommand = new ExampleCommand(); break; } + * String autoSelected = SmartDashboard.getString("Auto Selector", "Default"); + * switch(autoSelected) { case "My Auto": autonomousCommand = new + * MyAutoCommand(); break; case "Default Auto": default: autonomousCommand = new + * ExampleCommand(); break; } */ // schedule the autonomous command (example) @@ -124,4 +124,5 @@ public void teleopPeriodic() { @Override public void testPeriodic() { } + } diff --git a/src/org/usfirst/frc/team4737/robot/RobotMap.java b/src/org/usfirst/frc/team4737/robot/RobotMap.java index 9a54b66..e4fd1da 100644 --- a/src/org/usfirst/frc/team4737/robot/RobotMap.java +++ b/src/org/usfirst/frc/team4737/robot/RobotMap.java @@ -26,11 +26,18 @@ public class RobotMap { public static final int INTAKE_RIGHT = 16; // Elevator - public static final int ELEVATOR_MOTOR_A = 17; - public static final int ELEVATOR_MOTOR_B = 18; + public static final int ELEVATOR_MOTOR = 17; + + /** + * 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 + */ + public static final double ELEVATOR_HOLD_PCT = 0; // TODO measure + + public static final double ELEVATOR_HOLD_TIME = 3; - // If you are using multiple modules, make sure to define both the port - // number and the module. For example you with a rangefinder: - // public static int rangefinderPort = 1; - // public static int rangefinderModule = 1; } diff --git a/src/org/usfirst/frc/team4737/robot/commands/BuzzController.java b/src/org/usfirst/frc/team4737/robot/commands/BuzzController.java new file mode 100644 index 0000000..90bdeb5 --- /dev/null +++ b/src/org/usfirst/frc/team4737/robot/commands/BuzzController.java @@ -0,0 +1,40 @@ +package org.usfirst.frc.team4737.robot.commands; + +import org.usfirst.frc.team4737.lib.Gamepad; + +import edu.wpi.first.wpilibj.command.TimedCommand; + +/** + * + */ +public class BuzzController extends TimedCommand { + + private double strength; + private Gamepad controller; + + public BuzzController(double timeout, double strength, Gamepad controller) { + super(timeout); + // requires(none); + this.strength = strength; + this.controller = controller; + } + + // Called just before this Command runs the first time + protected void initialize() { + controller.setRumble(strength); + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + } + + // Called once after timeout + protected void end() { + controller.setRumble(0); + } + + // 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/ControlElevator.java b/src/org/usfirst/frc/team4737/robot/commands/ControlElevator.java new file mode 100644 index 0000000..aebc72a --- /dev/null +++ b/src/org/usfirst/frc/team4737/robot/commands/ControlElevator.java @@ -0,0 +1,57 @@ +package org.usfirst.frc.team4737.robot.commands; + +import org.usfirst.frc.team4737.robot.Robot; +import org.usfirst.frc.team4737.robot.RobotMap; + +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * + */ +public class ControlElevator extends Command { + + public ControlElevator() { + requires(Robot.ELEVATOR); + } + + // Called just before this Command runs the first time + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + // Control elevator movement, holding it in place by default + 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); + + SmartDashboard.putNumber("elevator_input", input); + + // Start hold command if no input + if (input == 0) { + new HoldElevator().start(); + this.cancel(); + } + } + + // 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/ControlIntake.java b/src/org/usfirst/frc/team4737/robot/commands/ControlIntake.java new file mode 100644 index 0000000..a1ca5fd --- /dev/null +++ b/src/org/usfirst/frc/team4737/robot/commands/ControlIntake.java @@ -0,0 +1,39 @@ +package org.usfirst.frc.team4737.robot.commands; + +import org.usfirst.frc.team4737.robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class ControlIntake extends Command { + + public ControlIntake() { + requires(Robot.INTAKE); + } + + // Called just before this Command runs the first time + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + Robot.INTAKE.setSpeed(-Robot.OI.operator.getAxis("LT").get() + Robot.OI.operator.getAxis("RT").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() { + Robot.INTAKE.setSpeed(0); + } +} diff --git a/src/org/usfirst/frc/team4737/robot/commands/HoldElevator.java b/src/org/usfirst/frc/team4737/robot/commands/HoldElevator.java new file mode 100644 index 0000000..c3474c9 --- /dev/null +++ b/src/org/usfirst/frc/team4737/robot/commands/HoldElevator.java @@ -0,0 +1,42 @@ +package org.usfirst.frc.team4737.robot.commands; + +import org.usfirst.frc.team4737.robot.Robot; +import org.usfirst.frc.team4737.robot.RobotMap; + +import edu.wpi.first.wpilibj.command.TimedCommand; + +/** + * + */ +public class HoldElevator extends TimedCommand { + + public HoldElevator() { + super(RobotMap.ELEVATOR_HOLD_TIME); + requires(Robot.ELEVATOR); + } + + // Called just before this Command runs the first time + protected void initialize() { + } + + // 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(); + } + } + + // Called once after timeout + protected void end() { + new RelaxElevator().start(); + } + + // 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/RelaxElevator.java b/src/org/usfirst/frc/team4737/robot/commands/RelaxElevator.java new file mode 100644 index 0000000..332cab5 --- /dev/null +++ b/src/org/usfirst/frc/team4737/robot/commands/RelaxElevator.java @@ -0,0 +1,38 @@ +package org.usfirst.frc.team4737.robot.commands; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class RelaxElevator extends StopElevator { + + public RelaxElevator() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + super(); + } + + // Called just before this Command runs the first time +// protected void initialize() { +// } +// +// // 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/StopElevator.java b/src/org/usfirst/frc/team4737/robot/commands/StopElevator.java index d2a4ea5..004726a 100644 --- a/src/org/usfirst/frc/team4737/robot/commands/StopElevator.java +++ b/src/org/usfirst/frc/team4737/robot/commands/StopElevator.java @@ -1,7 +1,6 @@ package org.usfirst.frc.team4737.robot.commands; import org.usfirst.frc.team4737.robot.Robot; -import org.usfirst.frc.team4737.robot.subsystems.Elevator; import edu.wpi.first.wpilibj.command.Command; diff --git a/src/org/usfirst/frc/team4737/robot/commands/TeleopTankDrive.java b/src/org/usfirst/frc/team4737/robot/commands/TeleopTankDrive.java index 7f9721a..e2515d5 100644 --- a/src/org/usfirst/frc/team4737/robot/commands/TeleopTankDrive.java +++ b/src/org/usfirst/frc/team4737/robot/commands/TeleopTankDrive.java @@ -21,8 +21,8 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run protected void execute() { Robot.DRIVETRAIN.tankDrive( - Robot.OI.controller.getThumbstick("LS").Y.get(), - Robot.OI.controller.getThumbstick("RS").Y.get()); + 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() diff --git a/src/org/usfirst/frc/team4737/robot/subsystems/Drivetrain.java b/src/org/usfirst/frc/team4737/robot/subsystems/Drivetrain.java index cc5ab9f..0c3dfb7 100644 --- a/src/org/usfirst/frc/team4737/robot/subsystems/Drivetrain.java +++ b/src/org/usfirst/frc/team4737/robot/subsystems/Drivetrain.java @@ -6,19 +6,20 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; /** * */ public class Drivetrain extends Subsystem { - // Put methods for controlling this subsystem - // here. Call these from Commands. private WPI_TalonSRX leftFrontMaster; private WPI_TalonSRX rightFrontMaster; private WPI_TalonSRX leftBackSlave; private WPI_TalonSRX rightBackSlave; + private DifferentialDrive drive; + public Drivetrain() { leftFrontMaster = new WPI_TalonSRX(RobotMap.DRIVE_LEFT_MASTER); leftBackSlave = new WPI_TalonSRX(RobotMap.DRIVE_LEFT_SLAVE); @@ -27,6 +28,9 @@ public Drivetrain() { leftBackSlave.follow(leftFrontMaster); rightBackSlave.follow(rightFrontMaster); + + drive = new DifferentialDrive(leftFrontMaster, rightFrontMaster); + } public void initDefaultCommand() { @@ -43,8 +47,7 @@ public void initDefaultCommand() { * - Right joystick input from -1.0 to 1.0 */ public void tankDrive(double leftInput, double rightInput) { - leftFrontMaster.set(leftInput); - rightFrontMaster.set(rightInput); + drive.tankDrive(leftInput, rightInput); } } diff --git a/src/org/usfirst/frc/team4737/robot/subsystems/Elevator.java b/src/org/usfirst/frc/team4737/robot/subsystems/Elevator.java index 32d594d..523f972 100644 --- a/src/org/usfirst/frc/team4737/robot/subsystems/Elevator.java +++ b/src/org/usfirst/frc/team4737/robot/subsystems/Elevator.java @@ -3,37 +3,52 @@ import org.usfirst.frc.team4737.robot.RobotMap; import org.usfirst.frc.team4737.robot.commands.StopElevator; +import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** * */ public class Elevator extends Subsystem { - // Put methods for controlling this subsystem - // here. Call these from Commands. + private WPI_TalonSRX motor; - private WPI_TalonSRX motorA; - private WPI_TalonSRX motorB; + private double current; + private double lastCurrent; + private final double retention = 0.1; public Elevator() { - motorA = new WPI_TalonSRX(RobotMap.ELEVATOR_MOTOR_A); - motorB = new WPI_TalonSRX(RobotMap.ELEVATOR_MOTOR_B); + motor = new WPI_TalonSRX(RobotMap.ELEVATOR_MOTOR); } public void initDefaultCommand() { - // Set the default command for a subsystem here. setDefaultCommand(new StopElevator()); } + @Override + public void periodic() { + // Measure current and apply a basic noise filter + double temp = current; + current = motor.getOutputCurrent() * (1 - retention) + lastCurrent * retention; + lastCurrent = temp; + + SmartDashboard.putNumber("elevator_current", current); + } + + public double getMotorCurrent() { + return current; + } + /** * * @param speed - * ranges from -1 to 1 + * ranges from -1.0 to 1.0 */ public void setSpeed(double speed) { - // TODO + motor.set(ControlMode.PercentOutput, speed); } + }