From f98dde751ca425fcef3033cd5dca2fe864359f00 Mon Sep 17 00:00:00 2001 From: esloan Date: Mon, 20 Jan 2014 23:06:02 -0600 Subject: [PATCH] Issue #21: added compressor command and made some fixes to deal with mechanical changes --- frc2022_2014/nbproject/project.xml | 174 +++++++++--------- .../org/usfirst/frc2022_2014/RobotMap.java | 15 +- .../frc2022_2014/commands/CommandBase.java | 6 +- .../commands/CompressorController.java | 49 +++++ .../subsystems/CompressorSubsystem.java | 29 ++- 5 files changed, 181 insertions(+), 92 deletions(-) create mode 100644 frc2022_2014/src/org/usfirst/frc2022_2014/commands/CompressorController.java diff --git a/frc2022_2014/nbproject/project.xml b/frc2022_2014/nbproject/project.xml index 724d4cc..ba6d456 100644 --- a/frc2022_2014/nbproject/project.xml +++ b/frc2022_2014/nbproject/project.xml @@ -1,92 +1,92 @@ - + - org.netbeans.modules.ant.freeform - - - frc2022_2014 - - ${user.home}/.sunspotfrc.properties - build.properties - ${sunspot.home}/default.properties - - - - - java - src - - - - - jar-app - - - clean - - - deploy - run - - - clean - jar-app - - - deploy - debug-run - - - javadoc - - - - folder - build - jar-app - - - - - - src - - - build.xml - - - - - - - - - - - - deploy - - - - jar-deploy - - - - - - - - - src - ${sunspot.home}\lib\squawk.jar - ${sunspot.home}\lib\wpilibj.jar;${sunspot.home}\lib\networktables-crio.jar - build - 1.4 - - - - + --> + org.netbeans.modules.ant.freeform + + + frc2022_2014 + + ${user.home}/.sunspotfrc.properties + build.properties + ${sunspot.home}/default.properties + + + + + java + src + + + + + jar-app + + + clean + + + deploy + run + + + clean + jar-app + + + deploy + debug-run + + + javadoc + + + + folder + build + jar-app + + + + + + src + + + build.xml + + + + + + + + + + + + deploy + + + + jar-deploy + + + + + + + + + src + ${sunspot.home}/lib/squawk.jar + ${sunspot.home}/lib/wpilibj.jar:${sunspot.home}/lib/networktables-crio.jar + build + 1.4 + + + + diff --git a/frc2022_2014/src/org/usfirst/frc2022_2014/RobotMap.java b/frc2022_2014/src/org/usfirst/frc2022_2014/RobotMap.java index e55b1c0..85357b9 100644 --- a/frc2022_2014/src/org/usfirst/frc2022_2014/RobotMap.java +++ b/frc2022_2014/src/org/usfirst/frc2022_2014/RobotMap.java @@ -7,6 +7,7 @@ * floating around. */ public class RobotMap { + // If the port isn't set yet, make it -1 // For example to map the left and right motors, you could define the // following variables to use with your drivetrain subsystem. // public static final int leftMotor = 1; @@ -18,11 +19,19 @@ public class RobotMap { // public static final int rangefinderModule = 1; public static final int LeftMotorPort = 3; public static final int RightMotorPort = 2; - public static final int ShooterPort = 15163; + public static final int ShooterPort = -1; public static final int RetractorPort = -1; public static final int CollectorPort = -1; - public static final int backLimitSwitchPort = -1; - public static final int frontLimitSwitchPort = -1; + public static final int SolenoidPort1 = -1; + public static final int SolenoidPort2 = -1; + public static final int BackLimitSwitchPort = -1; + public static final int FrontLimitSwitchPort = -1; + + public static final int CompressSwitchChannel = -1; + public static final int CompressRelayChannel = -1; + public static final int LockRelayChannel = -1; + public static final int OutLimitSwitchChannel = -1; + public static final int InLimitSwitchChannel = -1; /* For MecanumDrive */ public static final int FrontRightMotorPort = -1; diff --git a/frc2022_2014/src/org/usfirst/frc2022_2014/commands/CommandBase.java b/frc2022_2014/src/org/usfirst/frc2022_2014/commands/CommandBase.java index 846c69c..dd376ca 100644 --- a/frc2022_2014/src/org/usfirst/frc2022_2014/commands/CommandBase.java +++ b/frc2022_2014/src/org/usfirst/frc2022_2014/commands/CommandBase.java @@ -8,6 +8,7 @@ import org.usfirst.frc2022_2014.subsystems.PinballShooter; import org.usfirst.frc2022_2014.subsystems.TankDrivebase; import org.usfirst.frc2022_2014.subsystems.BallCollection; +import org.usfirst.frc2022_2014.subsystems.CompressorSubsystem; import org.usfirst.frc2022_2014.subsystems.MecanumDrive; /** @@ -25,7 +26,8 @@ public abstract class CommandBase extends Command { public static ExampleSubsystem exampleSubsystem = new ExampleSubsystem(); public static TankDrivebase tankDrivebase = new TankDrivebase(RobotMap.LeftMotorPort, RobotMap.RightMotorPort); public static PinballShooter shooter = new PinballShooter(RobotMap.ShooterPort); - public static BallCollection collector = new BallCollection(RobotMap.CollectorPort,RobotMap.RetractorPort, RobotMap.backLimitSwitchPort, RobotMap.frontLimitSwitchPort); + public static BallCollection collector = new BallCollection(RobotMap.CollectorPort,RobotMap.RetractorPort, RobotMap.BackLimitSwitchPort, RobotMap.FrontLimitSwitchPort); + public static CompressorSubsystem compressor = new CompressorSubsystem(RobotMap.SolenoidPort1, RobotMap.SolenoidPort1, RobotMap.CompressSwitchChannel, RobotMap.CompressRelayChannel,RobotMap.LockRelayChannel, RobotMap.OutLimitSwitchChannel, RobotMap.InLimitSwitchChannel); public static MecanumDrive mecanumDrive = new MecanumDrive(RobotMap.FrontLeftMotorPort, RobotMap.FrontRightMotorPort, RobotMap.BackLeftMotorPort, RobotMap.BackRightMotorPort); public static void init() { @@ -35,6 +37,8 @@ public static void init() { // yet. Thus, their requires() statements may grab null pointers. Bad // news. Don't move it. oi = new OI(); + + compressor.start(); // Show what command your subsystem is running on the SmartDashboard SmartDashboard.putData(exampleSubsystem); diff --git a/frc2022_2014/src/org/usfirst/frc2022_2014/commands/CompressorController.java b/frc2022_2014/src/org/usfirst/frc2022_2014/commands/CompressorController.java new file mode 100644 index 0000000..1f677f9 --- /dev/null +++ b/frc2022_2014/src/org/usfirst/frc2022_2014/commands/CompressorController.java @@ -0,0 +1,49 @@ +package org.usfirst.frc2022_2014.commands; + +/** + * +1) Turn back solenoid +2) Turn on electromagnet (relay) +3) Wait until fully back (limit switch) +4) Turn forward solenoid + +The two subsystems for the pinball shooter and the compressor should be merged + * + * @author Emma + */ +public class CompressorController extends CommandBase { + + public CompressorController(){ + requires(compressor); + + } + + // Called just before this Command runs the first time + protected void initialize() { + compressor.relayOn(); + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + compressor.backwardSolenoid(); + } + + // 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() { + if(compressor.pistonOut()){ + compressor.forwardSolenoid(); + compressor.stop(); + } + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + compressor.stop(); + } +} diff --git a/frc2022_2014/src/org/usfirst/frc2022_2014/subsystems/CompressorSubsystem.java b/frc2022_2014/src/org/usfirst/frc2022_2014/subsystems/CompressorSubsystem.java index d0614bd..33b4d0c 100644 --- a/frc2022_2014/src/org/usfirst/frc2022_2014/subsystems/CompressorSubsystem.java +++ b/frc2022_2014/src/org/usfirst/frc2022_2014/subsystems/CompressorSubsystem.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj.Relay; import edu.wpi.first.wpilibj.Solenoid; import edu.wpi.first.wpilibj.command.Subsystem; +import org.usfirst.frc2022_2014.custom.LimitSwitch; /** * @@ -19,10 +20,19 @@ public class CompressorSubsystem extends Subsystem { private Compressor compressor; private DoubleSolenoid dsol; + private Relay spikerelay; + private LimitSwitch lswitchout; + private LimitSwitch lswitchin; + - public CompressorSubsystem(int sol1Port, int sol2Port, int compSwitchChannel, int compRelayChannel){ + public CompressorSubsystem(int sol1Port, int sol2Port, int compSwitchChannel, int compRelayChannel, int emlRelayChanel, int + limitSwitch1Channel, int limitSwitch2Channel){ compressor = new Compressor(compSwitchChannel, compRelayChannel); dsol = new DoubleSolenoid(sol1Port, sol2Port); + spikerelay = new Relay(emlRelayChanel); + lswitchout = new LimitSwitch(limitSwitch1Channel); + lswitchin = new LimitSwitch(limitSwitch2Channel); + } public void start(){ @@ -33,6 +43,23 @@ public void start(){ public void stop(){ dsol.set(DoubleSolenoid.Value.kOff); compressor.stop(); + spikerelay.set(Relay.Value.kOff); + } + + public void relayOn(){ + spikerelay.set(Relay.Value.kOn); + } + + public void relayOff(){ + spikerelay.set(Relay.Value.kOff); + } + + public boolean pistonOut(){ + return lswitchout.isTriggered(); + } + + public boolean pistonIn(){ + return lswitchin.isTriggered(); } public void closeSolenoid(){