diff --git a/Freshman_Base_2019/.vscode/launch.json b/Freshman_Base_2019/.vscode/launch.json deleted file mode 100644 index c9c9713..0000000 --- a/Freshman_Base_2019/.vscode/launch.json +++ /dev/null @@ -1,21 +0,0 @@ -{ - // Use IntelliSense to learn about possible attributes. - // Hover to view descriptions of existing attributes. - // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 - "version": "0.2.0", - "configurations": [ - - { - "type": "wpilib", - "name": "WPILib Desktop Debug", - "request": "launch", - "desktop": true, - }, - { - "type": "wpilib", - "name": "WPILib roboRIO Debug", - "request": "launch", - "desktop": false, - } - ] -} diff --git a/Freshman_Base_2019/.vscode/settings.json b/Freshman_Base_2019/.vscode/settings.json deleted file mode 100644 index 860e319..0000000 --- a/Freshman_Base_2019/.vscode/settings.json +++ /dev/null @@ -1,14 +0,0 @@ -{ - "java.configuration.updateBuildConfiguration": "automatic", - "files.exclude": { - "**/.git": true, - "**/.svn": true, - "**/.hg": true, - "**/CVS": true, - "**/.DS_Store": true, - "bin/": true, - ".classpath": true, - ".project": true - }, - "wpilib.online": true -} diff --git a/Freshman_Base_2019/src/main/java/frc/robot/Robot.java b/Freshman_Base_2019/src/main/java/frc/robot/Robot.java index 93d2bc2..5a9b20a 100644 --- a/Freshman_Base_2019/src/main/java/frc/robot/Robot.java +++ b/Freshman_Base_2019/src/main/java/frc/robot/Robot.java @@ -1,103 +1,38 @@ package frc.robot; -//import com.ctre.phoenix.motorcontrol.can.TalonSRX; -import com.kauailabs.navx.frc.AHRS; - import edu.wpi.first.cameraserver.CameraServer; -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.SerialPort; import edu.wpi.first.wpilibj.TimedRobot; -import edu.wpi.first.wpilibj.Timer; -import frc.robot.autos.AutoCompilation; -import frc.robot.autos.RealAutos; import frc.robot.controls.DriverControls; import frc.robot.controls.OperatorControls; -import frc.robot.subsystems.Drivetrain; -import frc.robot.subsystems.HatchCollector; -import frc.robot.subsystems.Intake; -import frc.robot.subsystems.LineFollower; -import frc.robot.subsystems.Shooter; -import java.lang.Math; public class Robot extends TimedRobot { - public static Joystick driverJoy; - static Joystick operatorJoy; - - public static boolean firstTime = true; - static boolean buttonEnable = false, buttonPrev = false; - - final int firstDistance = 120; - final int secondDistance = 60; - - static int autostage = 0; - - public static AHRS NAVXgyro = new AHRS(SerialPort.Port.kMXP); - - DriverControls driver = new DriverControls(2); - OperatorControls operator = new OperatorControls(1); - - static Drivetrain drivingClass = new Drivetrain(); - public static Intake spinningIntake = new Intake(); - public static Shooter shootNow = new Shooter(); - static HatchCollector maticsMatics = new HatchCollector(); - static LineFollower lineFollower = new LineFollower(); - static RealAutos autos = new RealAutos(); - static AutoCompilation tryAutosAgain = new AutoCompilation(); - boolean startAuto = false; - boolean startAutoSet = false; + private DriverControls driverControls; + private OperatorControls operatorControls; - boolean startTimeSet = false; - boolean startTimeSetTwo = false; - public static double startTime = 0; - static double goNow = 0; - - double pi = Math.PI; @Override public void robotInit() { - driverJoy = new Joystick(0); - operatorJoy = new Joystick(1); + driverControls = new DriverControls(0); + operatorControls = new OperatorControls(1); CameraServer.getInstance().startAutomaticCapture(); - NAVXgyro.reset(); - drivingClass.initQuadrature(); } - @Override + @Override public void autonomousInit() { - - if (startTimeSet == false) { - startTime = Timer.getFPGATimestamp(); - startTimeSet = true; - } - - if (startTimeSetTwo == false) { - goNow = Timer.getFPGATimestamp(); - startTimeSetTwo = true; - } } @Override public void autonomousPeriodic() { - if (!drivingClass.moveDistance(60, .25)) { - drivingClass.moveDistance(60, .25); - } else if (drivingClass.moveDistance(60, .25)) { - frc.robot.subsystems.Shooter.autoShoot(); - } + driverControls.controls(); + operatorControls.controls(); } @Override public void teleopPeriodic() { - - driver.controls(); - operator.controls(); - - Drivetrain.drive(driverJoy.getRawAxis(1) * 1, driverJoy.getRawAxis(2) * 1, driverJoy.getRawAxis(3) * 1); - spinningIntake.spinIntake(driverJoy.getRawButton(1), driverJoy.getRawButton(4), driverJoy.getRawButton(2)); - shootNow.operatorShoot(operatorJoy.getRawAxis(1), operatorJoy.getRawAxis(5)); - maticsMatics.openClaw(operatorJoy.getRawAxis(2)); - maticsMatics.moveToSafety(operatorJoy.getRawButton(3), operatorJoy.getRawButton(1)); + driverControls.controls(); + operatorControls.controls(); } @Override @@ -107,6 +42,4 @@ public void testInit() { @Override public void testPeriodic() { } -} - -// Need to put LED code into the regular code \ No newline at end of file +} \ No newline at end of file diff --git a/Freshman_Base_2019/src/main/java/frc/robot/autos/AutoCompilation.java b/Freshman_Base_2019/src/main/java/frc/robot/autos/AutoCompilation.java deleted file mode 100644 index 2628f4b..0000000 --- a/Freshman_Base_2019/src/main/java/frc/robot/autos/AutoCompilation.java +++ /dev/null @@ -1,45 +0,0 @@ -package frc.robot.autos; - -import edu.wpi.first.wpilibj.Timer; -import frc.robot.Robot; - -public class AutoCompilation { - - boolean startTimeSet; - double startTime; - - public AutoCompilation() { - startTimeSet = false; - startTime = 0; - } - - public boolean pickUpTheBall() { - if (Robot.firstTime) { - Robot.firstTime = false; - Robot.NAVXgyro.reset(); - } - if (Timer.getFPGATimestamp() - startTime <= 5) { - Robot.spinningIntake.spinIntake(true, false, false); - return false; - } - else { - Robot.spinningIntake.spinIntake(false, false, false); - return true; - } - } - - public boolean shootTheBall() { - if (Robot.firstTime) { - Robot.firstTime = false; - Robot.NAVXgyro.reset(); - } - if (Timer.getFPGATimestamp() - startTime <= 2) { - Robot.shootNow.operatorShoot(0, .5); - return false; - } - else { - Robot.shootNow.operatorShoot(0,0); - return true; - } - } -} \ No newline at end of file diff --git a/Freshman_Base_2019/src/main/java/frc/robot/autos/RealAutos.java b/Freshman_Base_2019/src/main/java/frc/robot/autos/RealAutos.java deleted file mode 100644 index 39764a2..0000000 --- a/Freshman_Base_2019/src/main/java/frc/robot/autos/RealAutos.java +++ /dev/null @@ -1,38 +0,0 @@ -package frc.robot.autos; - -public class RealAutos { - static AutoCompilation autos; - double autostage; - - public RealAutos() { - autos = new AutoCompilation(); - autostage = 0; - } - - // public boolean firstAuto(boolean forward, boolean reverse, boolean stop, double shoot, double halfShoot, double distance, double speed, double setpoint) { - // if (Robot.autostage == 0) { - // if (autos.pickUpTheBall(forward, reverse, stop) == true) { - // Robot.autostage++; - // return false; - // } - // return false; - // } else if (Robot.autostage == 1) { - // if (autos.shootTheBall(shoot, halfShoot) == true) { - // Robot.autostage++; - // return false; - // } - // return false; - // } else if (Robot.autostage == 2) { - // if (Robot.drivingClass.moveDistance(distance, speed) == true) { - // ++Robot.autostage; - // return false; - // } - // return false; - // } else if (Robot.autostage == 3) { - // if (Robot.drivingClass.turnAngle(setpoint) == true) { - // Robot.autostage++; - // return true; - // } - // } return true; - // } -} \ No newline at end of file diff --git a/Freshman_Base_2019/src/main/java/frc/robot/controls/DriverControls.java b/Freshman_Base_2019/src/main/java/frc/robot/controls/DriverControls.java index d8cd8e4..b940d82 100644 --- a/Freshman_Base_2019/src/main/java/frc/robot/controls/DriverControls.java +++ b/Freshman_Base_2019/src/main/java/frc/robot/controls/DriverControls.java @@ -1,71 +1,51 @@ package frc.robot.controls; -//import frc.robot.Drivetrain; -//import frc.robot.Intake; - -//TRY TOGGLE USING THE D-PAD??? +import frc.robot.subsystems.Drivetrain; +import frc.robot.subsystems.Intake; public class DriverControls extends LogitechControls { - - double left; - double right; - double straight; - + + private double turningMultiplier = .75, slowTurningSpeed = .3; + public DriverControls(int port) { super(port); } - public void driverControls() { - controls(); - } - public void controls() { - straight = 0; - left = 0; - right = 0; - if(getDPadLeft()) { - if(Math.abs(getLeftYAxis()) > 0.1) { - straight = getLeftYAxis(); - } - if(getLeftTrigger() > 0.1) { - left = getLeftTrigger(); - } - if(getRightTrigger() > 0.1) { - right = getRightTrigger(); - } + /* + * Drivetrain + * Left Y axis - forward and back + * Triggers - left and right + * Bumpers - slower left and right + * A - run intake forward + * Y - run intake backwards + * B - stop intake + */ - frc.robot.subsystems.Drivetrain.drive(straight, right, left); - } - else if (getDPadRight()) { - if(Math.abs(getLeftYAxis()) > 0.1) { - straight = -getLeftYAxis(); - } - if(getLeftTrigger() > 0.1) { - left = -getLeftTrigger(); - } - if(getRightTrigger() > 0.1) { - right = -getRightTrigger(); - } + // Drivetrain + double leftYAxis = getLeftYAxis(); + double rightTrigger = getRightTrigger(); + double leftTrigger = getLeftTrigger(); - frc.robot.subsystems.Drivetrain.drive(straight, right, left); - } - if(getAButton()) { - frc.robot.subsystems.Intake.setSpeed(-1); - } - if(getBButton()) { - frc.robot.subsystems.Intake.stop(); - } - if(getXButton()) { - frc.robot.subsystems.Intake.setSpeed(1); - } - if(!getAButton()) { - frc.robot.subsystems.Intake.stop(); + boolean rightShoulderButton = getRightShoulderButton(); + boolean leftShoulderButton = getLeftShoulderButton(); + + double speedStraight = Math.abs(leftYAxis) > .08 ? leftYAxis : 0; + double speedRight = rightTrigger > .15 ? rightTrigger * turningMultiplier + : rightShoulderButton ? slowTurningSpeed : 0; + double speedLeft = leftTrigger > .15 ? leftTrigger * turningMultiplier + : leftShoulderButton ? slowTurningSpeed : 0; + Drivetrain.drive(speedStraight, speedRight, speedLeft); + + // Intake + if (getAButton()) { + Intake.run(false); } - if(!getBButton()) { - frc.robot.subsystems.Intake.stop(); + if (getYButton()) { + Intake.run(true); } - if(!getXButton()) { - frc.robot.subsystems.Intake.stop(); + if (getBButton()) { + Intake.stop(); } } -} \ No newline at end of file +} diff --git a/Freshman_Base_2019/src/main/java/frc/robot/controls/LogitechControls.java b/Freshman_Base_2019/src/main/java/frc/robot/controls/LogitechControls.java index a691230..69bf2e5 100644 --- a/Freshman_Base_2019/src/main/java/frc/robot/controls/LogitechControls.java +++ b/Freshman_Base_2019/src/main/java/frc/robot/controls/LogitechControls.java @@ -10,8 +10,16 @@ public LogitechControls(int port) { joy = new Joystick(port); } - //All buttons - + //Buttons + + public boolean getAButton() { + return joy.getRawButton(1); + } + + public boolean getBButton() { + return joy.getRawButton(2); + } + public boolean getXButton() { return joy.getRawButton(3); } @@ -20,42 +28,42 @@ public boolean getYButton() { return joy.getRawButton(4); } - public boolean getBButton() { - return joy.getRawButton(2); + public boolean getLeftShoulderButton() { + return joy.getRawButton(5); } - public boolean getAButton() { - return joy.getRawButton(1); + public boolean getRightShoulderButton() { + return joy.getRawButton(6); } - //All axes - - public double getLeftYAxis() { - return joy.getRawAxis(1); - } + //Axes public double getLeftXAxis() { return joy.getRawAxis(0); } - public double getRightYAxis() { - return joy.getRawAxis(5); + public double getLeftYAxis() { + return joy.getRawAxis(1); } public double getRightXAxis() { return joy.getRawAxis(4); } + public double getRightYAxis() { + return joy.getRawAxis(5); + } + public double getLeftTrigger() { - return (joy.getRawAxis(3)); + return (joy.getRawAxis(2)); } public double getRightTrigger() { - return (joy.getRawAxis(4)); + return (joy.getRawAxis(3)); } - //All DPad that probably won't work - + //D-PAD + public boolean getDPadUp() { return joy.getPOV() == 0; } diff --git a/Freshman_Base_2019/src/main/java/frc/robot/controls/OperatorControls.java b/Freshman_Base_2019/src/main/java/frc/robot/controls/OperatorControls.java index c475c0c..ea97360 100644 --- a/Freshman_Base_2019/src/main/java/frc/robot/controls/OperatorControls.java +++ b/Freshman_Base_2019/src/main/java/frc/robot/controls/OperatorControls.java @@ -1,50 +1,45 @@ package frc.robot.controls; -//import frc.robot.HatchCollector; -//import frc.robot.Shooter; +import frc.robot.subsystems.HatchCollector; +import frc.robot.subsystems.Shooter; public class OperatorControls extends LogitechControls { - String forward; - String backward; - String open; - String closed; - public OperatorControls(int port) { super(port); } - public void operatorControls() { - controls(); - } - public void controls() { - if (getLeftYAxis() > 0.5) { - frc.robot.subsystems.Shooter.setSpeed(1); - } - if (getLeftYAxis() < -0.5) { - frc.robot.subsystems.Shooter.setSpeed(-1); - } - if (getRightYAxis() > 0.5) { - frc.robot.subsystems.Shooter.setSpeed(0.6); - } - if (getRightYAxis() < -0.5) { - frc.robot.subsystems.Shooter.setSpeed(-0.6); - } - if (getLeftYAxis() < 0.5 && getLeftYAxis() > -0.5 && getRightYAxis() < 0.5 && getRightYAxis() > -0.5) { - frc.robot.subsystems.Shooter.setSpeed(0); - } - if (getRightTrigger() > 0.1) { - frc.robot.subsystems.HatchCollector.setPosition(closed); - } - if (getRightTrigger() < 0.1) { - frc.robot.subsystems.HatchCollector.setPosition(open); - } + /* + Hatch collector + Left trigger - closes claw + "A" - elbow in + "X" - elbow out + + Shooter + Left Y axis - shoots full speed + Right Y axis - shoots half speed + */ + + //Hatch collector + //Claw + if (getLeftTrigger() > .1) { + HatchCollector.closeClaw(); + }else{ + HatchCollector.openClaw(); + } + //Elbow if (getXButton()) { - frc.robot.subsystems.HatchCollector.setPosition(forward); + HatchCollector.extendElbow(); } if (getAButton()) { - frc.robot.subsystems.HatchCollector.setPosition(backward); + HatchCollector.retractElbow(); } + + //Shooter + double leftYAxis = getLeftYAxis(); + double rightYAxis = getRightYAxis(); + double speed = Math.abs(leftYAxis) > .08 ? leftYAxis : Math.abs(rightYAxis) > .08 ? rightYAxis * .45 : 0; + Shooter.shoot(speed); } } \ No newline at end of file diff --git a/Freshman_Base_2019/src/main/java/frc/robot/subsystems/Drivetrain.java b/Freshman_Base_2019/src/main/java/frc/robot/subsystems/Drivetrain.java index 499d0d8..fa71ba7 100644 --- a/Freshman_Base_2019/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/Freshman_Base_2019/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -1,144 +1,33 @@ package frc.robot.subsystems; import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.can.TalonSRX; -import edu.wpi.first.wpilibj.PIDController; -import edu.wpi.first.wpilibj.PIDOutput; -import frc.robot.Robot; - -public class Drivetrain implements PIDOutput { - - static TalonSRX fl; - static TalonSRX fr; - static TalonSRX bl; - static TalonSRX br; - public static Object drive; - double distance_traveled; - final int ticksPerInch = 233; - boolean firstTime,toggle=true; - PIDController PID; - double PIDValue; - final int kTimeoutMs = 30; - final boolean kDiscontinuityPresent = true; - final int kBookEnd_0 = 910; - final int kBookEnd_1 = 1137; - int startDistance; - boolean startDistanceSet; - boolean toggleMove = false; - - public Drivetrain() { - fl = new TalonSRX(1); - fr = new TalonSRX(2); - bl = new TalonSRX(3); - br = new TalonSRX(4); - fl.follow(bl); - fr.follow(br); - br.configOpenloopRamp(0.4); - bl.configOpenloopRamp(0.4); - br.setSensorPhase(true); - bl.setSensorPhase(true); - bl.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Absolute, 0, 1000); - br.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Absolute, 0, 1000); - bl.config_kP(0, .01); - br.config_kP(0, .01); - bl.config_kI(0, 0.0); - br.config_kI(0, 0.0); - bl.config_kD(0, 0.0); - br.config_kD(0, 0.0); - firstTime = false; - startDistance = 0; - startDistanceSet = false; - - PID = new PIDController(.05, 0.0, 0, Robot.NAVXgyro, this); - - PID.setAbsoluteTolerance(0); - PID.setInputRange(-180, 180); - PID.setOutputRange(-1, 1); - PID.setContinuous(); - PID.enable(); - - PIDValue = 0; +public class Drivetrain { + + private static TalonSRX frontLeft; + private static TalonSRX frontRight; + private static TalonSRX backLeft; + private static TalonSRX backRight; + + + static { + //Initialize motors and set master and slave + frontLeft = new TalonSRX(1); + frontRight = new TalonSRX(2); + backLeft = new TalonSRX(3); + backRight = new TalonSRX(4); + frontLeft.follow(backLeft); + frontRight.follow(backRight); + backRight.configOpenloopRamp(0.4); + backLeft.configOpenloopRamp(0.4); + backRight.setSensorPhase(true); + backLeft.setSensorPhase(true); } public static void drive(double speedStraight, double speedRight, double speedLeft) { - bl.set(ControlMode.PercentOutput, (speedStraight - speedRight + speedLeft)); - br.set(ControlMode.PercentOutput, (-speedStraight - speedRight + speedLeft)); + backLeft.set(ControlMode.PercentOutput, (speedStraight + speedRight - speedLeft)); + backRight.set(ControlMode.PercentOutput, (-speedStraight + speedRight - speedLeft)); } - public boolean moveDistance(double distance, double speed) { - if (startDistanceSet == false) { - startDistance = (br.getSelectedSensorPosition()); - startDistanceSet = true; - } - distance_traveled = ((br.getSelectedSensorPosition()) - startDistance)/ticksPerInch; - System.out.println("backright encoder" + br.getSelectedSensorPosition()); - System.out.println("startDistance" + startDistance); - System.out.println("distance_traveled" + distance_traveled); - if (Math.abs(distance_traveled) < distance) { - br.set(ControlMode.PercentOutput, (speed)); - bl.set(ControlMode.PercentOutput, -1*(speed)); - return false; - } else { - br.set(ControlMode.PercentOutput, (0)); - bl.set(ControlMode.PercentOutput, (0)); - Robot.NAVXgyro.reset(); - return true; - } - } - - public boolean spin(double distance, double speed) { - if (startDistanceSet == false) { - startDistance = (br.getSelectedSensorPosition()); - startDistanceSet = true; - } - distance_traveled = ((br.getSelectedSensorPosition()) - startDistance)/ticksPerInch; - if (Math.abs(distance_traveled) < distance) { - br.set(ControlMode.PercentOutput, (speed)); - bl.set(ControlMode.PercentOutput, (speed)); - return false; - } else { - br.set(ControlMode.PercentOutput, (0)); - bl.set(ControlMode.PercentOutput, (0)); - Robot.NAVXgyro.reset(); - return true; - } - } - - public boolean turnAngle(double setpoint) { - // if (!firstTime) { - // firstTime = true; - Robot.NAVXgyro.reset(); - PID.enable(); - PID.setSetpoint(setpoint); - if (!PID.onTarget()) { - System.out.println("Value: " + PIDValue); - bl.set(ControlMode.PercentOutput, (PIDValue)); - br.set(ControlMode.PercentOutput, (PIDValue)); - System.out.println("Error: " + PID.getError()); - return false; - } else { - bl.set(ControlMode.PercentOutput, (0)); - br.set(ControlMode.PercentOutput, (0)); - Robot.NAVXgyro.reset(); - PID.disable(); - return true; - } - } - - public void pidWrite(double output) { - PIDValue = output; - } - - public void initQuadrature() { - int pulseWidth = br.getSensorCollection().getPulseWidthPosition(); - if (kDiscontinuityPresent) { - int newCenter; - newCenter = (kBookEnd_0 + kBookEnd_1) / 2; - newCenter &= 0xFFF; - pulseWidth -= newCenter; - } - br.getSensorCollection().setQuadraturePosition(pulseWidth, kTimeoutMs); - } } \ No newline at end of file diff --git a/Freshman_Base_2019/src/main/java/frc/robot/subsystems/HatchCollector.java b/Freshman_Base_2019/src/main/java/frc/robot/subsystems/HatchCollector.java index 01fd2dd..7c3f126 100644 --- a/Freshman_Base_2019/src/main/java/frc/robot/subsystems/HatchCollector.java +++ b/Freshman_Base_2019/src/main/java/frc/robot/subsystems/HatchCollector.java @@ -5,45 +5,31 @@ public class HatchCollector { - static DoubleSolenoid claw; - static DoubleSolenoid safetyPosition; + private static DoubleSolenoid claw; + private static DoubleSolenoid elbow; + private static Compressor compressor; - public HatchCollector() { - Compressor compressor = new Compressor(); + static { + //Initialize the compressor and solenoids + compressor = new Compressor(); compressor.setClosedLoopControl(true); - claw = new DoubleSolenoid(2, 3); //first number is forward channel, second number is reverse channel - safetyPosition = new DoubleSolenoid(0, 1); + claw = new DoubleSolenoid(2, 3); + elbow = new DoubleSolenoid(0, 1); } - public static void setPosition(String position) { - if(position == "closed") { - claw.set(DoubleSolenoid.Value.kForward); - } - if(position == "open") { - claw.set(DoubleSolenoid.Value.kReverse); - } - if(position == "forward") { - safetyPosition.set(DoubleSolenoid.Value.kReverse); - } - if(position == "backward") { - safetyPosition.set(DoubleSolenoid.Value.kForward); - } + public static void openClaw() { + claw.set(DoubleSolenoid.Value.kReverse); } - - public void openClaw(double open) { - if (open > 0.1) { - claw.set(DoubleSolenoid.Value.kForward); - } - else { - claw.set(DoubleSolenoid.Value.kReverse); - } + + public static void closeClaw() { + claw.set(DoubleSolenoid.Value.kForward); + } + + public static void extendElbow() { + elbow.set(DoubleSolenoid.Value.kReverse); } - public void moveToSafety(boolean goForward, boolean goBackwards) { - if (goForward == true) { - safetyPosition.set(DoubleSolenoid.Value.kReverse); - } else if (goBackwards == true) { - safetyPosition.set(DoubleSolenoid.Value.kForward); - } + public static void retractElbow() { + elbow.set(DoubleSolenoid.Value.kForward); } -} \ No newline at end of file +} diff --git a/Freshman_Base_2019/src/main/java/frc/robot/subsystems/Intake.java b/Freshman_Base_2019/src/main/java/frc/robot/subsystems/Intake.java index 107eff0..62ce0a0 100644 --- a/Freshman_Base_2019/src/main/java/frc/robot/subsystems/Intake.java +++ b/Freshman_Base_2019/src/main/java/frc/robot/subsystems/Intake.java @@ -3,47 +3,19 @@ import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.can.TalonSRX; -import edu.wpi.first.wpilibj.Timer; -import frc.robot.Robot; - public class Intake { - static TalonSRX intake; - double intakeSpeed; - TalonSRX shooter; - - public Intake() { + private static TalonSRX intake; + static { + // Initialize the intake motor intake = new TalonSRX(5); } - public static void setSpeed(double speed) { - intake.set(ControlMode.PercentOutput, speed); + public static void run(boolean reverse) { + intake.set(ControlMode.PercentOutput, reverse ? 1 : -1); } public static void stop() { intake.set(ControlMode.PercentOutput, 0); } - - public void spinIntake(boolean forward, boolean reverse, boolean stop) { - if (forward == true) { // a = 1, b = 2, x = 3, y =4 - intake.set(ControlMode.PercentOutput, -1); - } else if (reverse == true) { - intake.set(ControlMode.PercentOutput, 1); - } else if (stop == true) { - intake.set(ControlMode.PercentOutput, 0); - } - } - public boolean autoIntake() { - if ((Timer.getFPGATimestamp() - Robot.startTime) <= 3) { - System.out.println("This is running."); - intake.set(ControlMode.PercentOutput, -1); - return false; - } else if ((Timer.getFPGATimestamp() - Robot.startTime) > 3) { - System.out.println("This is done."); - intake.set(ControlMode.PercentOutput, 0); - return true; - } else { - return false; - } - } } diff --git a/Freshman_Base_2019/src/main/java/frc/robot/subsystems/LineFollower.java b/Freshman_Base_2019/src/main/java/frc/robot/subsystems/LineFollower.java deleted file mode 100644 index 0d4b601..0000000 --- a/Freshman_Base_2019/src/main/java/frc/robot/subsystems/LineFollower.java +++ /dev/null @@ -1,62 +0,0 @@ -package frc.robot.subsystems; - -//import com.ctre.phoenix.motorcontrol.ControlMode; - -import edu.wpi.first.wpilibj.AnalogInput; -//import frc.robot.Robot; - -public class LineFollower { - AnalogInput blue = new AnalogInput(0); - AnalogInput yellow = new AnalogInput(1); - AnalogInput white = new AnalogInput(3); - AnalogInput green = new AnalogInput(2); - static Drivetrain drive; - boolean startTimeSet; - double startTime; - - public LineFollower() { - drive = new Drivetrain(); - startTimeSet = false; - startTime = 0.0; - } - - public void linay() { - int yellowValue = yellow.getValue(); - int whiteValue = white.getValue(); - int greenValue = green.getValue(); - int blueValue = blue.getValue(); - System.out.println("Yellow: " + yellowValue); - System.out.println("White: " + whiteValue); - System.out.println("Green: " + greenValue); - System.out.println("Blue: " + blueValue); - } - - // public void wheresLinay() { - // int yellowValue = yellow.getValue(); - // int greenValue = green.getValue(); - // if (Robot.driverJoy.getRawButton(7)) { - // if (yellowValue < 500) { // we need to turn to the right??? - // System.out.println("Too far to the left. Making right turn."); - // Drivetrain.drive.bl.set(ControlMode.PercentOutput, 0.2); - // Drivetrain.drive.br.set(ControlMode.PercentOutput, 0.2); - // } - // else if (yellowValue > 500) { // we're on target - // System.out.println("Locked on target."); - // Robot.drive.bl.set(ControlMode.PercentOutput, 0); - // Robot.drive.br.set(ControlMode.PercentOutput, 0); - // } - // } - // if (Robot.driverJoy.getRawButton(8)) { - // if (yellowValue < 500) { // we need to turn to the left??? - // System.out.println("Too far to the right. Making left turn."); - // Robot.drivingClass.bl.set(ControlMode.PercentOutput, -0.2); - // Robot.drivingClass.br.set(ControlMode.PercentOutput, -0.2); - // } - // else if (yellowValue > 500) { - // System.out.println("Locked on target."); - // Robot.drivingClass.bl.set(ControlMode.PercentOutput, 0); - // Robot.drivingClass.br.set(ControlMode.PercentOutput, 0); - // } - // } - // } -} \ No newline at end of file diff --git a/Freshman_Base_2019/src/main/java/frc/robot/subsystems/Shooter.java b/Freshman_Base_2019/src/main/java/frc/robot/subsystems/Shooter.java index ea51d45..816d7f6 100644 --- a/Freshman_Base_2019/src/main/java/frc/robot/subsystems/Shooter.java +++ b/Freshman_Base_2019/src/main/java/frc/robot/subsystems/Shooter.java @@ -3,43 +3,17 @@ import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.can.VictorSPX; -import edu.wpi.first.wpilibj.Timer; -import frc.robot.Robot; - public class Shooter { - static VictorSPX shooter; + private static VictorSPX shooter; - public Shooter() { + static { + //Initialize the shooting motor shooter = new VictorSPX(6); } - public static void setSpeed(double speed) { + public static void shoot(double speed) { shooter.set(ControlMode.PercentOutput, speed); } - - public void operatorShoot(double go, double halfShoot) { - if (go < -0.5) { - shooter.set(ControlMode.PercentOutput, -1); - } else if (go > 0.5) { - shooter.set(ControlMode.PercentOutput, 1); - } else if (halfShoot < -0.5) { - shooter.set(ControlMode.PercentOutput, -.6); - } else if (halfShoot > 0.5) { - shooter.set(ControlMode.PercentOutput, .6); - } - else { - shooter.set(ControlMode.PercentOutput, 0); - } - } - public static void autoShoot() { - if ((Timer.getFPGATimestamp() - Robot.startTime) <= 4) { - shooter.set(ControlMode.PercentOutput, -1); - } - else if ((Timer.getFPGATimestamp() - Robot.startTime) > 4) { - shooter.set(ControlMode.PercentOutput, 0); - } - else { - } - } + } \ No newline at end of file diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json deleted file mode 100644 index d4da1ce..0000000 --- a/vendordeps/Phoenix.json +++ /dev/null @@ -1,87 +0,0 @@ -{ - "fileName": "Phoenix.json", - "name": "CTRE-Phoenix", - "version": "5.12.0", - "uuid": "ab676553-b602-441f-a38d-f1296eff6537", - "mavenUrls": [ - "http://devsite.ctr-electronics.com/maven/release/" - ], - "jsonUrl": "http://devsite.ctr-electronics.com/maven/release/com/ctre/phoenix/Phoenix-latest.json", - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-java", - "version": "5.12.0" - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-java", - "version": "5.12.0" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.12.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64" - ] - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix", - "artifactId": "wpiapi-cpp", - "version": "5.12.0", - "libName": "CTRE_Phoenix_WPI", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "api-cpp", - "version": "5.12.0", - "libName": "CTRE_Phoenix", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "cci", - "version": "5.12.0", - "libName": "CTRE_PhoenixCCI", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "linuxathena", - "windowsx86-64", - "linuxx86-64" - ] - }, - { - "groupId": "com.ctre.phoenix", - "artifactId": "core", - "version": "5.12.0", - "libName": "CTRE_PhoenixCore", - "headerClassifier": "headers" - } - ] -} \ No newline at end of file