From: Harel Dor Date: Mon, 22 Feb 2016 05:06:00 +0000 (-0800) Subject: Add compressor control code and fix solenoid ports X-Git-Url: http://challenge-bot.com/repos/?p=3501%2Fstronghold-2016;a=commitdiff_plain;h=ee82dd56da8889e008eb1a09aa8b8e016a59fd72 Add compressor control code and fix solenoid ports --- diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java index 54ffd009..1a1a20bb 100644 --- a/src/org/usfirst/frc/team3501/robot/Constants.java +++ b/src/org/usfirst/frc/team3501/robot/Constants.java @@ -37,6 +37,7 @@ public class Constants { public final static int RIGHT_JOYSTICK_THUMB_PORT = 2; public final static int TOGGLE_SCALING_PORT = 0; + public final static int TOGGLE_COMPRESSOR_PORT = 12; } @@ -63,8 +64,12 @@ public class Constants { public static double encoderTolerance = 8.0, gyroTolerance = 5.0; public static final int MANUAL_MODE = 1, ENCODER_MODE = 2, GYRO_MODE = 3; - public static final int LEFT_FORWARD = 1, LEFT_REVERSE = 2, - RIGHT_FORWARD = 9, RIGHT_REVERSE = 10; + + public static final int MODULE_A_ID = 9, MODULE_B_ID = 10; + public static final int LEFT_FORWARD = 5, LEFT_REVERSE = 1, + RIGHT_FORWARD = 4, RIGHT_REVERSE = 0; + public static final int COMPRESSOR_ID = 9; + public static double time = 0; // Gearing constants diff --git a/src/org/usfirst/frc/team3501/robot/OI.java b/src/org/usfirst/frc/team3501/robot/OI.java index cb7e12d9..8de027b7 100644 --- a/src/org/usfirst/frc/team3501/robot/OI.java +++ b/src/org/usfirst/frc/team3501/robot/OI.java @@ -1,6 +1,7 @@ package org.usfirst.frc.team3501.robot; import org.usfirst.frc.team3501.robot.commands.driving.ChangeGear; +import org.usfirst.frc.team3501.robot.commands.driving.ToggleCompressor; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.buttons.Button; @@ -34,6 +35,7 @@ public class OI { public static Button intakeBoulder; public static Button shootBoulder; public static Button toggleGear; + public static Button toggleCompressor; // button to change robot to the scaling mode public static DigitalButton toggleScaling; @@ -46,6 +48,10 @@ public class OI { Constants.OI.RIGHT_JOYSTICK_TRIGGER_PORT); toggleGear.toggleWhenPressed(new ChangeGear()); + toggleCompressor = new JoystickButton(rightJoystick, + Constants.OI.TOGGLE_COMPRESSOR_PORT); + toggleCompressor.whenPressed(new ToggleCompressor()); + // passPortcullis = new DigitalButton( // new DigitalInput(Constants.OI.PASS_PORTCULLIS_PORT)); // passPortcullis.whenPressed(new PassPortcullis()); diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java index 5bb61f14..14f86db3 100644 --- a/src/org/usfirst/frc/team3501/robot/Robot.java +++ b/src/org/usfirst/frc/team3501/robot/Robot.java @@ -121,6 +121,7 @@ public class Robot extends IterativeRobot { @Override public void teleopInit() { Robot.driveTrain.setLowGear(); + Robot.driveTrain.startCompressor(); } @Override diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java index ac239361..f5a7d6c2 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java @@ -7,6 +7,7 @@ import org.usfirst.frc.team3501.robot.sensors.GyroLib; import org.usfirst.frc.team3501.robot.sensors.Lidar; import edu.wpi.first.wpilibj.CANTalon; +import edu.wpi.first.wpilibj.Compressor; import edu.wpi.first.wpilibj.CounterBase.EncodingType; import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.DoubleSolenoid.Value; @@ -29,6 +30,7 @@ public class DriveTrain extends PIDSubsystem { private GyroLib gyro; private DoubleSolenoid leftGearPiston, rightGearPiston; + private Compressor compressor; // Drivetrain specific constants that relate to the inches per pulse value for // the encoders @@ -62,11 +64,13 @@ public class DriveTrain extends PIDSubsystem { this.disable(); gyro.start(); - leftGearPiston = new DoubleSolenoid(10, Constants.DriveTrain.LEFT_FORWARD, - Constants.DriveTrain.LEFT_REVERSE); - rightGearPiston = new DoubleSolenoid(10, - Constants.DriveTrain.RIGHT_FORWARD, - Constants.DriveTrain.RIGHT_REVERSE); + leftGearPiston = new DoubleSolenoid(Constants.DriveTrain.MODULE_B_ID, + Constants.DriveTrain.LEFT_FORWARD, Constants.DriveTrain.LEFT_REVERSE); + rightGearPiston = new DoubleSolenoid(Constants.DriveTrain.MODULE_B_ID, + Constants.DriveTrain.RIGHT_FORWARD, Constants.DriveTrain.RIGHT_REVERSE); + + compressor = new Compressor(Constants.DriveTrain.COMPRESSOR_ID); + Constants.DriveTrain.inverted = false; } @@ -197,10 +201,10 @@ public class DriveTrain extends PIDSubsystem { /* * Method is a required method that the PID Subsystem uses to return the * calculated PID value to the driver - * + * * @param Gives the user the output from the PID algorithm that is calculated * internally - * + * * Body: Uses the output, does some filtering and drives the robot */ @Override @@ -213,12 +217,11 @@ public class DriveTrain extends PIDSubsystem { output = Math.signum(output) * 0.3; left = output; right = output + drift * Constants.DriveTrain.kp / 10; - drive(left, right); } else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) { left = output; right = -output; - arcadeDrive(0, output); } + drive(left, right); pidOutput = output; } @@ -229,7 +232,7 @@ public class DriveTrain extends PIDSubsystem { /* * Checks the drive mode - * + * * @return the current state of the robot in each state Average distance from * both sides of tank drive for Encoder Mode Angle from the gyro in GYRO_MODE */ @@ -265,9 +268,9 @@ public class DriveTrain extends PIDSubsystem { /* * constrains the distance to within -100 and 100 since we aren't going to * drive more than 100 inches - * + * * Configure Encoder PID - * + * * Sets the setpoint to the PID subsystem */ public void driveDistance(double dist, double maxTimeOut) { @@ -307,10 +310,10 @@ public class DriveTrain extends PIDSubsystem { /* * Turning method that should be used repeatedly in a command - * + * * First constrains the angle to within -360 and 360 since that is as much as * we need to turn - * + * * Configures Gyro PID and sets the setpoint as an angle */ public void turnAngle(double angle) { @@ -366,4 +369,19 @@ public class DriveTrain extends PIDSubsystem { rightGearPiston.set(gear); } + public void startCompressor() { + compressor.start(); + } + + public void stopCompressor() { + compressor.stop(); + } + + public void toggleCompressor() { + if (compressor.enabled()) + compressor.stop(); + else + compressor.start(); + } + }