diff --git a/.classpath b/.classpath index ca6a26e..3335664 100644 --- a/.classpath +++ b/.classpath @@ -1,10 +1,11 @@ - + + diff --git a/build/jars/NetworkTables.jar b/build/jars/NetworkTables.jar new file mode 100644 index 0000000..3b578a3 Binary files /dev/null and b/build/jars/NetworkTables.jar differ diff --git a/build/jars/WPILib.jar b/build/jars/WPILib.jar new file mode 100644 index 0000000..fa955eb Binary files /dev/null and b/build/jars/WPILib.jar differ diff --git a/build/jars/cscore.jar b/build/jars/cscore.jar new file mode 100644 index 0000000..7b95664 Binary files /dev/null and b/build/jars/cscore.jar differ diff --git a/build/jars/navx_frc.jar b/build/jars/navx_frc.jar new file mode 100644 index 0000000..7d87d72 Binary files /dev/null and b/build/jars/navx_frc.jar differ diff --git a/build/jars/opencv.jar b/build/jars/opencv.jar new file mode 100644 index 0000000..5edacd2 Binary files /dev/null and b/build/jars/opencv.jar differ diff --git a/build/org/usfirst/frc/team5401/robot/OI.class b/build/org/usfirst/frc/team5401/robot/OI.class index f3f826c..bd44809 100644 Binary files a/build/org/usfirst/frc/team5401/robot/OI.class and b/build/org/usfirst/frc/team5401/robot/OI.class differ diff --git a/build/org/usfirst/frc/team5401/robot/Robot.class b/build/org/usfirst/frc/team5401/robot/Robot.class index 2cf7fcf..d00dfef 100644 Binary files a/build/org/usfirst/frc/team5401/robot/Robot.class and b/build/org/usfirst/frc/team5401/robot/Robot.class differ diff --git a/build/org/usfirst/frc/team5401/robot/RobotMap.class b/build/org/usfirst/frc/team5401/robot/RobotMap.class index b976b1a..8535551 100644 Binary files a/build/org/usfirst/frc/team5401/robot/RobotMap.class and b/build/org/usfirst/frc/team5401/robot/RobotMap.class differ diff --git a/build/org/usfirst/frc/team5401/robot/commands/feederArmUpDown.class b/build/org/usfirst/frc/team5401/robot/commands/feederArmUpDown.class new file mode 100644 index 0000000..d00bc57 Binary files /dev/null and b/build/org/usfirst/frc/team5401/robot/commands/feederArmUpDown.class differ diff --git a/build/org/usfirst/frc/team5401/robot/commands/feederControl.class b/build/org/usfirst/frc/team5401/robot/commands/feederControl.class new file mode 100644 index 0000000..4367cb4 Binary files /dev/null and b/build/org/usfirst/frc/team5401/robot/commands/feederControl.class differ diff --git a/build/org/usfirst/frc/team5401/robot/commands/feederInOut.class b/build/org/usfirst/frc/team5401/robot/commands/feederInOut.class new file mode 100644 index 0000000..d343f1e Binary files /dev/null and b/build/org/usfirst/frc/team5401/robot/commands/feederInOut.class differ diff --git a/build/org/usfirst/frc/team5401/robot/subsystems/DriveBase.class b/build/org/usfirst/frc/team5401/robot/subsystems/DriveBase.class deleted file mode 100644 index 680156c..0000000 Binary files a/build/org/usfirst/frc/team5401/robot/subsystems/DriveBase.class and /dev/null differ diff --git a/build/org/usfirst/frc/team5401/robot/subsystems/Infeed.class b/build/org/usfirst/frc/team5401/robot/subsystems/Infeed.class new file mode 100644 index 0000000..4ec1896 Binary files /dev/null and b/build/org/usfirst/frc/team5401/robot/subsystems/Infeed.class differ diff --git a/dist/FRCUserProgram.jar b/dist/FRCUserProgram.jar new file mode 100644 index 0000000..5e4542f Binary files /dev/null and b/dist/FRCUserProgram.jar differ diff --git a/src/org/usfirst/frc/team5401/robot/OI.java b/src/org/usfirst/frc/team5401/robot/OI.java index 54b5ebf..74d4ed9 100644 --- a/src/org/usfirst/frc/team5401/robot/OI.java +++ b/src/org/usfirst/frc/team5401/robot/OI.java @@ -123,7 +123,7 @@ else if (value < -.5){ //Feeder In/Out public int getTriggers_Operator(){ double left = operatorCtrlr.getRawAxis(RobotMap.LEFT_TRIGGER_AXIS); - double right = operatorCtrlr.getRawAxis(RobotMap.LEFT_TRIGGER_AXIS); + double right = operatorCtrlr.getRawAxis(RobotMap.RIGHT_TRIGGER_AXIS); if (right > .1){ return 1; } diff --git a/src/org/usfirst/frc/team5401/robot/Robot.java b/src/org/usfirst/frc/team5401/robot/Robot.java index ac0898c..2d4a954 100644 --- a/src/org/usfirst/frc/team5401/robot/Robot.java +++ b/src/org/usfirst/frc/team5401/robot/Robot.java @@ -19,17 +19,17 @@ */ public class Robot extends IterativeRobot { - public static DriveBase driveBase; - public static Loader loader; - public static GearMechanism gearMechanism; - public static Unjammer unjammer; - public static CompressorSubsystem compressorsubsystem; +// public static DriveBase driveBase; +// public static Loader loader; +// public static GearMechanism gearMechanism; +// public static Unjammer unjammer; +// public static CompressorSubsystem compressorsubsystem; public static Infeed infeed; - public static Climber climber; +// public static Climber climber; public static OI oi; - Command autonomousCommand; - SendableChooser chooser; +// Command autonomousCommand; +// SendableChooser chooser; /** * This function is run when the robot is first started up and should be @@ -37,17 +37,17 @@ public class Robot extends IterativeRobot { */ @Override public void robotInit() { - driveBase = new DriveBase(); - loader = new Loader(); - gearMechanism = new GearMechanism(); - unjammer = new Unjammer(); - compressorsubsystem = new CompressorSubsystem(); +// driveBase = new DriveBase(); +// loader = new Loader(); +// gearMechanism = new GearMechanism(); +// unjammer = new Unjammer(); +// compressorsubsystem = new CompressorSubsystem(); infeed = new Infeed(); - climber = new Climber(); +// climber = new Climber(); oi = new OI(); // chooser.addObject("My Auto", new MyAutoCommand()); - SmartDashboard.putData("Auto mode", chooser); +// SmartDashboard.putData("Auto mode", chooser); } /** @@ -78,7 +78,7 @@ public void disabledPeriodic() { */ @Override public void autonomousInit() { - autonomousCommand = (Command) chooser.getSelected(); +// autonomousCommand = (Command) chooser.getSelected(); /* * String autoSelected = SmartDashboard.getString("Auto Selector", @@ -88,7 +88,7 @@ public void autonomousInit() { */ // schedule the autonomous command (example) - if (autonomousCommand != null) autonomousCommand.start(); +// if (autonomousCommand != null) autonomousCommand.start(); } @@ -106,8 +106,8 @@ public void teleopInit() { // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove // this line or comment it out. - if (autonomousCommand != null) - autonomousCommand.cancel(); +// if (autonomousCommand != null) +// autonomousCommand.cancel(); } /** diff --git a/src/org/usfirst/frc/team5401/robot/RobotMap.java b/src/org/usfirst/frc/team5401/robot/RobotMap.java index 732779f..6d934be 100644 --- a/src/org/usfirst/frc/team5401/robot/RobotMap.java +++ b/src/org/usfirst/frc/team5401/robot/RobotMap.java @@ -35,9 +35,9 @@ public class RobotMap { //PWM Motors public static final int LEFT_DRIVE_MOTOR_1 = 0; public static final int RIGHT_DRIVE_MOTOR_1 = 1; - public static final int LEFT_DRIVE_MOTOR_2 = 3; + public static final int LEFT_DRIVE_MOTOR_2 = 2; public static final int RIGHT_DRIVE_MOTOR_2 = 4; - public static final int INFEEDER_BAR = 5; + public static final int INFEEDER_BAR = 3; public static final int CLIMBER_MOTOR = 6; //Sensors @@ -50,12 +50,12 @@ public class RobotMap { public static final int PCM_ID = 0; public static final int SHIFTER_IN = 0; public static final int SHIFTER_OUT = 1; - public static final int GEAR_DOOR_CLOSE = 2; - public static final int GEAR_DOOR_OPEN = 3; +// public static final int GEAR_DOOR_CLOSE = 2; +// public static final int GEAR_DOOR_OPEN = 3; public static final int UNJAM_IN = 4; public static final int UNJAM_OUT = 5; - public static final int INFEED_IN = 6; - public static final int INFEED_OUT = 7; + public static final int INFEED_IN = 3; + public static final int INFEED_OUT = 2; //Analog public static final int PRESSURE_SENSOR = 0; diff --git a/src/org/usfirst/frc/team5401/robot/commands/Climb.java b/src/org/usfirst/frc/team5401/robot/commands/Climb.java deleted file mode 100644 index ca66237..0000000 --- a/src/org/usfirst/frc/team5401/robot/commands/Climb.java +++ /dev/null @@ -1,48 +0,0 @@ -package org.usfirst.frc.team5401.robot.commands; - -import org.usfirst.frc.team5401.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - -/** - * - */ -public class Climb extends Command { - - private int input; - - public Climb(int direction) { - requires(Robot.climber); - input = direction; - } - - // Called just before this Command runs the first time - protected void initialize() { - if (input == 1){ - Robot.climber.climbUp(); - } - else if (input == 0){ - Robot.climber.climbStop(); - } - - } - - // 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 true; - } - - // 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.climber.climbStop(); - } -} diff --git a/src/org/usfirst/frc/team5401/robot/commands/PopGear.java b/src/org/usfirst/frc/team5401/robot/commands/PopGear.java deleted file mode 100644 index 54df409..0000000 --- a/src/org/usfirst/frc/team5401/robot/commands/PopGear.java +++ /dev/null @@ -1,40 +0,0 @@ -package org.usfirst.frc.team5401.robot.commands; - -import edu.wpi.first.wpilibj.command.Command; -import org.usfirst.frc.team5401.robot.Robot; -/** - * - */ -public class PopGear extends Command { - - public PopGear() { - // Use requires() here to declare subsystem dependencies - // eg. requires(chassis); - requires(Robot.gearMechanism); - } - - // Called just before this Command runs the first time - protected void initialize() { - Robot.gearMechanism.openDoor(); - } - - // 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 true; - } - - // Called once after isFinished returns true - protected void end() { - Robot.gearMechanism.closeDoor(); - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - Robot.gearMechanism.closeDoor(); - } -} diff --git a/src/org/usfirst/frc/team5401/robot/commands/loadShooter.java b/src/org/usfirst/frc/team5401/robot/commands/loadShooter.java deleted file mode 100644 index b5a1b18..0000000 --- a/src/org/usfirst/frc/team5401/robot/commands/loadShooter.java +++ /dev/null @@ -1,42 +0,0 @@ -package org.usfirst.frc.team5401.robot.commands; - -import edu.wpi.first.wpilibj.command.Command; - -import org.usfirst.frc.team5401.robot.Robot; -import org.usfirst.frc.team5401.robot.RobotMap; -import org.usfirst.frc.team5401.robot.subsystems.Loader; - -/** - * - */ -public class loadShooter extends Command { - - public loadShooter() { - // Use requires() here to declare subsystem dependencies - requires(Robot.loader); - } - - // Called just before this Command runs the first time - protected void initialize() { - Robot.loader.switchState(); - } - - // 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 true; - } - - // 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.loader.stopLoader(); - } -} diff --git a/src/org/usfirst/frc/team5401/robot/commands/toggleCompressor.java b/src/org/usfirst/frc/team5401/robot/commands/toggleCompressor.java deleted file mode 100644 index 1249434..0000000 --- a/src/org/usfirst/frc/team5401/robot/commands/toggleCompressor.java +++ /dev/null @@ -1,40 +0,0 @@ -package org.usfirst.frc.team5401.robot.commands; - -import edu.wpi.first.wpilibj.command.Command; -import org.usfirst.frc.team5401.robot.Robot; - -/** - * - */ -public class toggleCompressor extends Command { - - public toggleCompressor() { - // Use requires() here to declare subsystem dependencies - // eg. requires(chassis); - requires(Robot.compressorsubsystem); - } - - // 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.compressorsubsystem.switchState(); - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return true; - } - - // 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.compressorsubsystem.stopCompressor(); - } -} diff --git a/src/org/usfirst/frc/team5401/robot/commands/unjamIn.java b/src/org/usfirst/frc/team5401/robot/commands/unjamIn.java deleted file mode 100644 index 685c958..0000000 --- a/src/org/usfirst/frc/team5401/robot/commands/unjamIn.java +++ /dev/null @@ -1,39 +0,0 @@ -package org.usfirst.frc.team5401.robot.commands; - -import edu.wpi.first.wpilibj.command.Command; -import org.usfirst.frc.team5401.robot.Robot; - -/** - * - */ -public class unjamIn extends Command { - - public unjamIn() { - // Use requires() here to declare subsystem dependencies - // eg. requires(chassis); - requires(Robot.unjammer); - } - - // 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.unjammer.unjamIn(); - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return true; - } - - // 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/team5401/robot/commands/unjamToggle.java b/src/org/usfirst/frc/team5401/robot/commands/unjamToggle.java deleted file mode 100644 index f57f6c1..0000000 --- a/src/org/usfirst/frc/team5401/robot/commands/unjamToggle.java +++ /dev/null @@ -1,48 +0,0 @@ -package org.usfirst.frc.team5401.robot.commands; - -import org.usfirst.frc.team5401.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - -/** - * - */ -public class unjamToggle extends Command { - - private int input; - - public unjamToggle(int direction) { - // Use requires() here to declare subsystem dependencies - // eg. requires(chassis); - requires(Robot.unjammer); - input = direction; - } - - // Called just before this Command runs the first time - protected void initialize() { - if(input == 1){ - Robot.unjammer.unjamIn(); - } - else if(input == -1){ - Robot.unjammer.unjamOut(); - } - } - - // 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 true; - } - - // 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/team5401/robot/commands/xboxMove.java b/src/org/usfirst/frc/team5401/robot/commands/xboxMove.java deleted file mode 100644 index 5fba8d4..0000000 --- a/src/org/usfirst/frc/team5401/robot/commands/xboxMove.java +++ /dev/null @@ -1,114 +0,0 @@ -package org.usfirst.frc.team5401.robot.commands; - -import org.usfirst.frc.team5401.robot.Robot; -import org.usfirst.frc.team5401.robot.RobotMap; - -import edu.wpi.first.wpilibj.command.Command; - -/** - * - */ -public class xboxMove extends Command { - - private final double MINIMUM_VELOCITY_FOR_HIGH_GEAR; //Experimentally Determined, REMEMBER inches per second - private final double MAXIMUM_VELOCITY_FOR_LOW_GEAR; - - double velocitySample1; - double velocitySample2; - - public xboxMove() { - - MINIMUM_VELOCITY_FOR_HIGH_GEAR = 35;// REMEMBER inches per second - MAXIMUM_VELOCITY_FOR_LOW_GEAR = 45; - - velocitySample1 = 0; - velocitySample2 = 0; - - // Use requires() here to declare subsystem dependencies - // eg. requires(chassis); - requires(Robot.driveBase); - } - - // Called just before this Command runs the first time - protected void initialize() { - Robot.driveBase.shiftHighToLow(); - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - double slew = Robot.oi.readLeftStickX_Driver() * -1; - - double throttle = Robot.oi.readRightTrigger_Driver(); - double reverse = Robot.oi.readLeftTrigger_Driver(); - boolean turn = Robot.oi.getTurnButton_Driver(); - boolean precision = Robot.oi.getPrecision_Driver(); - boolean brake = Robot.oi.getBrake_Driver(); - boolean invert = Robot.oi.getDriveInvertButton_Driver(); - - boolean shiftLow = Robot.oi.getBack_Driver(); - boolean shiftHigh = Robot.oi.getStart_Driver(); - - //Manual Gearing - if(shiftHigh){ - Robot.driveBase.shiftLowToHigh(); - } - else if(shiftLow){ - Robot.driveBase.shiftHighToLow(); - } - double right = 0, left = 0, sensitivity = 1; - - if (brake){ - left = 0; - right = 0; - } else if(!turn){ - if (slew > RobotMap.DRIVE_THRESHHOLD){ //If Slew is positive (Thumbstick pushed right), go Right - left = (throttle - reverse) * sensitivity; //Send Left full power - right = (throttle - reverse) * sensitivity * (1 - slew); //Send Right partial power, tempered by how hard the thumbstick is being pushed - } else if (slew < (-1 * RobotMap.DRIVE_THRESHHOLD)){ //If Slew is negative (Thumbstick pushed left), go Left - left = (throttle - reverse) * sensitivity * (1 + slew); //Send Left partial power, tempered by how hard thumbstick is being pushed left - right = (throttle - reverse) * sensitivity; //Send right full power - } else { //Drive forward/back normally - left = (throttle - reverse) * sensitivity; - right = (throttle - reverse) * sensitivity; - } - } else { - if (invert){ - slew *= -1; - } - if (Math.abs(slew) > RobotMap.DRIVE_THRESHHOLD){ - left = RobotMap.DRIVE_SPIN_SENSITIVITY * slew; - right = RobotMap.DRIVE_SPIN_SENSITIVITY * slew * -1; - } - } - - - Robot.driveBase.drive(left, right); - - - - - /*****Shifting Gear Code*********/ - Robot.driveBase.getEncDist(); - - //Gets new final velocity - velocitySample2 = Robot.driveBase.getVelocity(); - } - - - - // 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() { - Robot.driveBase.Stop(); - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - Robot.driveBase.Stop(); - } -} diff --git a/src/org/usfirst/frc/team5401/robot/subsystems/Climber.java b/src/org/usfirst/frc/team5401/robot/subsystems/Climber.java deleted file mode 100644 index a6bf95c..0000000 --- a/src/org/usfirst/frc/team5401/robot/subsystems/Climber.java +++ /dev/null @@ -1,37 +0,0 @@ -package org.usfirst.frc.team5401.robot.subsystems; - -import edu.wpi.first.wpilibj.command.Subsystem; - -import org.usfirst.frc.team5401.robot.RobotMap; - -import edu.wpi.first.wpilibj.VictorSP; - -/** - * - */ -public class Climber extends Subsystem { - - private VictorSP climbMotor; - - private double SPEED; - - public Climber(){ - climbMotor = new VictorSP(RobotMap.CLIMBER_MOTOR); - - SPEED = .9; - } - - public void initDefaultCommand() { - // Set the default command for a subsystem here. - //setDefaultCommand(new MySpecialCommand()); - } - - public void climbUp(){ - climbMotor.set(SPEED); - } - - public void climbStop(){ - climbMotor.set(0); - } -} - diff --git a/src/org/usfirst/frc/team5401/robot/subsystems/CompressorSubsystem.java b/src/org/usfirst/frc/team5401/robot/subsystems/CompressorSubsystem.java deleted file mode 100644 index f024109..0000000 --- a/src/org/usfirst/frc/team5401/robot/subsystems/CompressorSubsystem.java +++ /dev/null @@ -1,48 +0,0 @@ -package org.usfirst.frc.team5401.robot.subsystems; - -import edu.wpi.first.wpilibj.command.Subsystem; - -import org.usfirst.frc.team5401.robot.RobotMap; - -import edu.wpi.first.wpilibj.Compressor; - -/** - * - */ -public class CompressorSubsystem extends Subsystem { - - private Compressor compressor; - - public CompressorSubsystem(){ - compressor = new Compressor(RobotMap.PCM_ID); - } - - public void initDefaultCommand() { - // Set the default command for a subsystem here. - //setDefaultCommand(new MySpecialCommand()); - } - - public void startCompressor(){ - compressor.setClosedLoopControl(true); - compressor.start(); - } - - public void stopCompressor(){ - compressor.stop(); - } - - public boolean isEnabled(){ - return compressor.enabled(); - } - - public void switchState(){ - if(isEnabled()){ - stopCompressor(); - } - - else { - startCompressor(); - } - } -} - diff --git a/src/org/usfirst/frc/team5401/robot/subsystems/DriveBase.java b/src/org/usfirst/frc/team5401/robot/subsystems/DriveBase.java deleted file mode 100644 index 7fc0af7..0000000 --- a/src/org/usfirst/frc/team5401/robot/subsystems/DriveBase.java +++ /dev/null @@ -1,121 +0,0 @@ -package org.usfirst.frc.team5401.robot.subsystems; - -import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.VictorSP; -import edu.wpi.first.wpilibj.DoubleSolenoid; - -import org.usfirst.frc.team5401.robot.RobotMap; - -//import com.kauailabs.navx.frc.AHRS; -import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.SerialPort; - -/** - * - */ -public class DriveBase extends Subsystem { - - double LOW_GEAR_LEFT_DPP; - double LOW_GEAR_RIGHT_DPP; - double HIGH_GEAR_LEFT_DPP; - double HIGH_GEAR_RIGHT_DPP; - double GYRO_OFFSET; - - // Put methods for controlling this subsystem - // here. Call these from Commands. - private VictorSP leftDrive1; - private VictorSP leftDrive2; - private VictorSP rightDrive1; - private VictorSP rightDrive2; - - private DoubleSolenoid gearShift; - - private Encoder leftEncoder; - private Encoder rightEncoder; -// AHRS gyro; - - public DriveBase(){ - LOW_GEAR_LEFT_DPP = -.0189249; - LOW_GEAR_RIGHT_DPP = -.0189249; - - - leftDrive1 = new VictorSP(RobotMap.LEFT_DRIVE_MOTOR_1); - leftDrive2 = new VictorSP(RobotMap.LEFT_DRIVE_MOTOR_2); - rightDrive1 = new VictorSP(RobotMap.RIGHT_DRIVE_MOTOR_1); - rightDrive2 = new VictorSP(RobotMap.RIGHT_DRIVE_MOTOR_2); - gearShift = new DoubleSolenoid(RobotMap.PCM_ID, RobotMap.SHIFTER_IN, RobotMap.SHIFTER_OUT); - leftEncoder = new Encoder(RobotMap.LEFT_ENC_A, RobotMap.LEFT_ENC_B, true, Encoder.EncodingType.k4X); - rightEncoder = new Encoder(RobotMap.RIGHT_ENC_A, RobotMap.RIGHT_ENC_B, true, Encoder.EncodingType.k4X); - -// gyro = new AHRS(SerialPort.Port.kMXP); -// gyro.reset(); - } - - public void drive(double leftDriveDesired, double rightDriveDesired){ - leftDrive1.set(leftDriveDesired); - rightDrive1.set(-1* rightDriveDesired); - leftDrive2.set(leftDriveDesired); - rightDrive2.set(-1* rightDriveDesired); - } - - public void Stop(){ - leftDrive1.stopMotor(); - leftDrive2.stopMotor(); - rightDrive1.stopMotor(); - rightDrive2.stopMotor(); - } - - public void shiftLowToHigh(){ - gearShift.set(DoubleSolenoid.Value.kForward); - leftEncoder.setDistancePerPulse(HIGH_GEAR_LEFT_DPP); - rightEncoder.setDistancePerPulse(HIGH_GEAR_RIGHT_DPP); - } - - public void shiftHighToLow(){ - gearShift.set(DoubleSolenoid.Value.kReverse); - leftEncoder.setDistancePerPulse(LOW_GEAR_LEFT_DPP); - rightEncoder.setDistancePerPulse(LOW_GEAR_RIGHT_DPP); - } - - public double getVelocity(){ - double velocity = (Math.abs(leftEncoder.getRate()) + Math.abs(rightEncoder.getRate()))/2; - return velocity; - } - - public void setDPPLow(){ - leftEncoder.setDistancePerPulse(LOW_GEAR_LEFT_DPP); - rightEncoder.setDistancePerPulse(LOW_GEAR_RIGHT_DPP); - } - - public void setDPPHigh(){ - leftEncoder.setDistancePerPulse(HIGH_GEAR_LEFT_DPP); - rightEncoder.setDistancePerPulse(HIGH_GEAR_RIGHT_DPP); - } - - public double getEncDist(){ - double rawDistLeft = leftEncoder.get(); - double rawDistRight = rightEncoder.get(); - double leftDistance = leftEncoder.getDistance(); - double rightDistance = rightEncoder.getDistance(); - double encoderDistance = (leftDistance + rightDistance)/2; - return encoderDistance; - } - - public void resetEncoders(){ - leftEncoder.reset(); - rightEncoder.reset(); - } - -/* public double reportGyro(){ - double currentAngle = gyro.getAngle(); - double currentPitch = gyro.getPitch(); - double currentRoll = gyro.currentRoll(); - return currentAngle; - } -*/ - public void initDefaultCommand() { - // Set the default command for a subsystem here. - //setDefaultCommand(new MySpecialCommand()); - } -} - diff --git a/src/org/usfirst/frc/team5401/robot/subsystems/ExampleSubsystem.java b/src/org/usfirst/frc/team5401/robot/subsystems/ExampleSubsystem.java deleted file mode 100644 index 1b1fc05..0000000 --- a/src/org/usfirst/frc/team5401/robot/subsystems/ExampleSubsystem.java +++ /dev/null @@ -1,16 +0,0 @@ -package org.usfirst.frc.team5401.robot.subsystems; - -import edu.wpi.first.wpilibj.command.Subsystem; - -/** - * - */ -public class ExampleSubsystem extends Subsystem { - // Put methods for controlling this subsystem - // here. Call these from Commands. - - public void initDefaultCommand() { - // Set the default command for a subsystem here. - // setDefaultCommand(new MySpecialCommand()); - } -} diff --git a/src/org/usfirst/frc/team5401/robot/subsystems/GearMechanism.java b/src/org/usfirst/frc/team5401/robot/subsystems/GearMechanism.java deleted file mode 100644 index 873e5fa..0000000 --- a/src/org/usfirst/frc/team5401/robot/subsystems/GearMechanism.java +++ /dev/null @@ -1,40 +0,0 @@ -package org.usfirst.frc.team5401.robot.subsystems; - -import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.DoubleSolenoid; -import org.usfirst.frc.team5401.robot.RobotMap; -//import org.usfirst.frc.team5401.robot.commands.PopGear; - -/** - * - */ -public class GearMechanism extends Subsystem { - - private DoubleSolenoid gearPop; - - private boolean enabled; - - public GearMechanism(){ - gearPop = new DoubleSolenoid(RobotMap.PCM_ID, RobotMap.GEAR_DOOR_CLOSE, RobotMap.GEAR_DOOR_OPEN); - } - - // Put methods for controlling this subsystem - // here. Call these from Commands. - - public void initDefaultCommand() { - // Set the default command for a subsystem here. - //setDefaultCommand(new MySpecialCommand()); -// setDefaultCommand(new PopGear()); - } - - public void openDoor(){ - enabled = true; - gearPop.set(DoubleSolenoid.Value.kForward); - } - public void closeDoor(){ - enabled = false; - gearPop.set(DoubleSolenoid.Value.kReverse); - } - -} - diff --git a/src/org/usfirst/frc/team5401/robot/subsystems/Infeed.java b/src/org/usfirst/frc/team5401/robot/subsystems/Infeed.java index f90da20..531698a 100644 --- a/src/org/usfirst/frc/team5401/robot/subsystems/Infeed.java +++ b/src/org/usfirst/frc/team5401/robot/subsystems/Infeed.java @@ -7,6 +7,7 @@ import org.usfirst.frc.team5401.robot.RobotMap; //import org.usfirst.frc.team5401.robot.commands.feederControl; +import org.usfirst.frc.team5401.robot.commands.feederControl; /** * @@ -21,8 +22,8 @@ public class Infeed extends Subsystem { private AnalogInput pressureSensor; private double inputVoltage; private final static double DEFAULT_VOLTS = 5.0; - private final int SLOPE = 250; - private final int Y_INTERCEPT = -20; +// private final int SLOPE = 250; +// private final int Y_INTERCEPT = -20; public Infeed(){ feederArm = new DoubleSolenoid(RobotMap.PCM_ID, RobotMap.INFEED_IN, RobotMap.INFEED_OUT); @@ -37,7 +38,7 @@ public Infeed(){ public void initDefaultCommand() { // Set the default command for a subsystem here. //setDefaultCommand(new MySpecialCommand()); - // setDefaultCommand(new feederControl()); + setDefaultCommand(new feederControl()); } public void feedDirection(int direction){ diff --git a/src/org/usfirst/frc/team5401/robot/subsystems/Loader.java b/src/org/usfirst/frc/team5401/robot/subsystems/Loader.java deleted file mode 100644 index 5c853d7..0000000 --- a/src/org/usfirst/frc/team5401/robot/subsystems/Loader.java +++ /dev/null @@ -1,62 +0,0 @@ -package org.usfirst.frc.team5401.robot.subsystems; - -import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.VictorSP; -import org.usfirst.frc.team5401.robot.RobotMap; - -/** - * - */ -public class Loader extends Subsystem { - - // Put methods for controlling this subsystem - // here. Call these from Commands. - - private VictorSP Conveyors; - private VictorSP meteringBar; - - private double loadSpeed; - private boolean enabled; - - public Loader(){ - - Conveyors = new VictorSP(RobotMap.conveyors); - meteringBar = new VictorSP(RobotMap.meteringMotor); - - loadSpeed = -.8; - enabled = false; - - } - public void initDefaultCommand() { - // Set the default command for a subsystem here. - //setDefaultCommand(new MySpecialCommand()); - } - public void runLoader(){ - enabled = true; - Conveyors.set(loadSpeed); - meteringBar.set(loadSpeed); - - } - - public void stopLoader(){ - enabled = false; - Conveyors.set(0); - meteringBar.set(0); - } - - public boolean isEnabled(){ - return enabled; - } - - public void switchState(){ - if(!enabled){ - runLoader(); - } - else{ - stopLoader(); - } - } -} - - - diff --git a/src/org/usfirst/frc/team5401/robot/subsystems/Unjammer.java b/src/org/usfirst/frc/team5401/robot/subsystems/Unjammer.java deleted file mode 100644 index 0f92888..0000000 --- a/src/org/usfirst/frc/team5401/robot/subsystems/Unjammer.java +++ /dev/null @@ -1,34 +0,0 @@ -package org.usfirst.frc.team5401.robot.subsystems; - -import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.DoubleSolenoid; -import org.usfirst.frc.team5401.robot.RobotMap; - -/** - * - */ -public class Unjammer extends Subsystem { - - // Put methods for controlling this subsystem - // here. Call these from Commands. - - private DoubleSolenoid unjam; - - public Unjammer(){ - unjam = new DoubleSolenoid(RobotMap.PCM_ID, RobotMap.UNJAM_IN, RobotMap.UNJAM_OUT); - } - - public void initDefaultCommand() { - // Set the default command for a subsystem here. - //setDefaultCommand(new MySpecialCommand()); - } - - public void unjamOut(){ - unjam.set(DoubleSolenoid.Value.kForward); - } - - public void unjamIn(){ - unjam.set(DoubleSolenoid.Value.kReverse); - } -} -