From a1bfac3e578628f126fc51b6d39a1186c93d772a Mon Sep 17 00:00:00 2001 From: Brian Semrau Date: Wed, 28 Feb 2018 12:13:11 -0500 Subject: [PATCH] Added IntakeGrip subsystem and controls --- src/org/usfirst/frc/team4737/robot/OI.java | 20 ++++--- src/org/usfirst/frc/team4737/robot/Robot.java | 1 + .../usfirst/frc/team4737/robot/RobotMap.java | 13 +++- .../robot/commands/intake/TwistIntake.java | 60 +++++++++---------- .../intakegrip/ControlIntakeGrip.java | 46 ++++++++++++++ .../commands/intakegrip/StopIntakeGrip.java | 39 ++++++++++++ .../frc/team4737/robot/subsystems/Intake.java | 2 +- .../team4737/robot/subsystems/IntakeGrip.java | 46 ++++++++++++++ 8 files changed, 185 insertions(+), 42 deletions(-) create mode 100644 src/org/usfirst/frc/team4737/robot/commands/intakegrip/ControlIntakeGrip.java create mode 100644 src/org/usfirst/frc/team4737/robot/commands/intakegrip/StopIntakeGrip.java create mode 100644 src/org/usfirst/frc/team4737/robot/subsystems/IntakeGrip.java diff --git a/src/org/usfirst/frc/team4737/robot/OI.java b/src/org/usfirst/frc/team4737/robot/OI.java index 646d6fb..d5afb1a 100644 --- a/src/org/usfirst/frc/team4737/robot/OI.java +++ b/src/org/usfirst/frc/team4737/robot/OI.java @@ -10,10 +10,10 @@ 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.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.drivetrain.*; +import org.usfirst.frc.team4737.robot.commands.elevator.*; +import org.usfirst.frc.team4737.robot.commands.intake.*; +import org.usfirst.frc.team4737.robot.commands.intakegrip.*; import edu.wpi.first.wpilibj.buttons.Trigger; @@ -78,12 +78,16 @@ 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)); + + // Take control of intake grip + operator.getButton("A").whileHeld(new ControlIntakeGrip()); + operator.getButton("B").whileHeld(new ControlIntakeGrip()); + + // 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/Robot.java b/src/org/usfirst/frc/team4737/robot/Robot.java index 9e300d5..aabc297 100644 --- a/src/org/usfirst/frc/team4737/robot/Robot.java +++ b/src/org/usfirst/frc/team4737/robot/Robot.java @@ -29,6 +29,7 @@ public class Robot extends TimedRobot { public static final Drivetrain DRIVETRAIN = new Drivetrain(); public static final Elevator ELEVATOR = new Elevator(); public static final Intake INTAKE = new Intake(); + public static final IntakeGrip INTAKEGRIP = new IntakeGrip(); public static final Climber CLIMBER = new Climber(); public static final ControlSystem CONTROLSYSTEM = new ControlSystem(); diff --git a/src/org/usfirst/frc/team4737/robot/RobotMap.java b/src/org/usfirst/frc/team4737/robot/RobotMap.java index 900c165..504da31 100644 --- a/src/org/usfirst/frc/team4737/robot/RobotMap.java +++ b/src/org/usfirst/frc/team4737/robot/RobotMap.java @@ -15,9 +15,9 @@ */ public class RobotMap { - // ################ - // TalonSRX CAN IDs - // ################ + // ##################### + // Actuator IDs/Channels + // ##################### // Drivetrain public static final int DRIVE_LEFT_MASTER = 11; @@ -29,6 +29,13 @@ public class RobotMap { public static final int INTAKE_LEFT = 15; public static final int INTAKE_RIGHT = 16; + public static final int INTAKE_LEFT_PISTON_MODULE = 0; + public static final int INTAKE_LEFT_PISTON_FORWARD_CHANNEL = 0; + public static final int INTAKE_LEFT_PISTON_REVERSE_CHANNEL = 1; + public static final int INTAKE_RIGHT_PISTON_MODULE = 0; + public static final int INTAKE_RIGHT_PISTON_FORWARD_CHANNEL = 2; + public static final int INTAKE_RIGHT_PISTON_REVERSE_CHANNEL = 3; + // Elevator public static final int ELEVATOR_MOTOR = 17; diff --git a/src/org/usfirst/frc/team4737/robot/commands/intake/TwistIntake.java b/src/org/usfirst/frc/team4737/robot/commands/intake/TwistIntake.java index 3bcd93c..7b9f8bf 100644 --- a/src/org/usfirst/frc/team4737/robot/commands/intake/TwistIntake.java +++ b/src/org/usfirst/frc/team4737/robot/commands/intake/TwistIntake.java @@ -10,34 +10,34 @@ public class TwistIntake extends Command { private double speed; - - public TwistIntake(double speed) { - requires(Robot.INTAKE); - - this.speed = speed; - } - - // 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.setLRSpeed(speed, -speed); - } - - // 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 TwistIntake(double speed) { + requires(Robot.INTAKE); + + this.speed = speed; + } + + // 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.setLRSpeed(speed, -speed); + } + + // 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/intakegrip/ControlIntakeGrip.java b/src/org/usfirst/frc/team4737/robot/commands/intakegrip/ControlIntakeGrip.java new file mode 100644 index 0000000..a14e189 --- /dev/null +++ b/src/org/usfirst/frc/team4737/robot/commands/intakegrip/ControlIntakeGrip.java @@ -0,0 +1,46 @@ +package org.usfirst.frc.team4737.robot.commands.intakegrip; + +import org.usfirst.frc.team4737.robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class ControlIntakeGrip extends Command { + + public ControlIntakeGrip() { + requires(Robot.INTAKEGRIP); + } + + // Called just before this Command runs the first time + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + boolean grab = Robot.OI.operator.getButton("A").get(); + boolean release = Robot.OI.operator.getButton("B").get(); + + if (grab) { + Robot.INTAKEGRIP.closePneumatics(); + } else if (release) { + Robot.INTAKEGRIP.openPneumatics(); + } + } + + // 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/intakegrip/StopIntakeGrip.java b/src/org/usfirst/frc/team4737/robot/commands/intakegrip/StopIntakeGrip.java new file mode 100644 index 0000000..9559cfc --- /dev/null +++ b/src/org/usfirst/frc/team4737/robot/commands/intakegrip/StopIntakeGrip.java @@ -0,0 +1,39 @@ +package org.usfirst.frc.team4737.robot.commands.intakegrip; + +import org.usfirst.frc.team4737.robot.Robot; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class StopIntakeGrip extends Command { + + public StopIntakeGrip() { + requires(Robot.INTAKEGRIP); + } + + // 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.INTAKEGRIP.disablePneumatics(); + } + + // 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/subsystems/Intake.java b/src/org/usfirst/frc/team4737/robot/subsystems/Intake.java index 1b6a2d1..22ed700 100644 --- a/src/org/usfirst/frc/team4737/robot/subsystems/Intake.java +++ b/src/org/usfirst/frc/team4737/robot/subsystems/Intake.java @@ -33,7 +33,7 @@ public void initDefaultCommand() { public void setSpeed(double speed) { setLRSpeed(speed, speed); } - + public void setLRSpeed(double left, double right) { leftMotor.set(left); rightMotor.set(right); diff --git a/src/org/usfirst/frc/team4737/robot/subsystems/IntakeGrip.java b/src/org/usfirst/frc/team4737/robot/subsystems/IntakeGrip.java new file mode 100644 index 0000000..db9b21a --- /dev/null +++ b/src/org/usfirst/frc/team4737/robot/subsystems/IntakeGrip.java @@ -0,0 +1,46 @@ +package org.usfirst.frc.team4737.robot.subsystems; + +import org.usfirst.frc.team4737.robot.RobotMap; +import org.usfirst.frc.team4737.robot.commands.intakegrip.StopIntakeGrip; + +import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.DoubleSolenoid.Value; +import edu.wpi.first.wpilibj.command.Subsystem; + +/** + * + */ +public class IntakeGrip extends Subsystem { + + private DoubleSolenoid leftPneumatic; + private DoubleSolenoid rightPneumatic; + + public IntakeGrip() { + leftPneumatic = new DoubleSolenoid(RobotMap.INTAKE_LEFT_PISTON_MODULE, + RobotMap.INTAKE_LEFT_PISTON_FORWARD_CHANNEL, RobotMap.INTAKE_LEFT_PISTON_REVERSE_CHANNEL); + rightPneumatic = new DoubleSolenoid(RobotMap.INTAKE_RIGHT_PISTON_MODULE, + RobotMap.INTAKE_RIGHT_PISTON_FORWARD_CHANNEL, RobotMap.INTAKE_RIGHT_PISTON_REVERSE_CHANNEL); + } + + public void initDefaultCommand() { + setDefaultCommand(new StopIntakeGrip()); + } + + public void closePneumatics() { + setPneumatics(Value.kReverse, Value.kReverse); + } + + public void openPneumatics() { + setPneumatics(Value.kForward, Value.kForward); + } + + public void disablePneumatics() { + setPneumatics(Value.kOff, Value.kOff); + } + + public void setPneumatics(DoubleSolenoid.Value left, DoubleSolenoid.Value right) { + leftPneumatic.set(left); + rightPneumatic.set(right); + } + +}