diff --git a/build.gradle b/build.gradle index 62a5d09..5d0d2da 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2019.1.1" + id "edu.wpi.first.GradleRIO" version "2019.4.1" } def ROBOT_MAIN_CLASS = "frc.robot.Main" diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index c094f0a..3fe5283 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -27,10 +27,14 @@ public static void init() { auxStick = new Joystick(RobotMap.auxillaryStick); Button moveArmToPos = new JoystickButton(leftDriveStick, 9); - Button movePivotDown = new JoystickButton(auxStick, 4); - Button movePivotUp = new JoystickButton(auxStick, 5); + Button moveArmToHighestRocket = new JoystickButton(leftDriveStick, 8); + Button driveClimberForward = new JoystickButton(rightDriveStick, 4); + Button driveClimberBackwards = new JoystickButton(rightDriveStick, 5); + moveArmToHighestRocket.whileHeld(new MoveToHighRocket()); moveArmToPos.whileHeld(new MoveTo47()); + driveClimberForward.whileHeld(new DriveClimberForward()); + driveClimberBackwards.whileHeld(new DriveClimberBackwards()); } public static Joystick getRightDriveStick() { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0e75a62..a9975dc 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -42,7 +42,7 @@ public void robotInit() { CameraServer.getInstance().startAutomaticCapture(); } - /** + /**h * This function is called every robot packet, no matter the mode. Use * this for items like diagnostics that you want ran during disabled, * autonomous, teleoperated and test. @@ -89,6 +89,7 @@ public void autonomousInit() { * autonomousCommand = new ExampleCommand(); break; } */ System.out.println("auto init"); + CommandBase.grabber.setForward(); } /** @@ -105,9 +106,6 @@ 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 (m_autonomousCommand != null) { - m_autonomousCommand.cancel(); - } //CommandBase.grabber.toggle(); } diff --git a/src/main/java/frc/robot/commands/AutoDriveToTarget.java b/src/main/java/frc/robot/commands/AutoDriveToTarget.java index 74c57db..9446716 100644 --- a/src/main/java/frc/robot/commands/AutoDriveToTarget.java +++ b/src/main/java/frc/robot/commands/AutoDriveToTarget.java @@ -18,12 +18,9 @@ public class AutoDriveToTarget extends CommandBase { public AutoDriveToTarget() { super("AutoDrive"); - System.out.println("wew"); - // Use requires() here to declare subsystem dependencies requires(driveTrain); requires(vision); requires(climber); - System.out.println("lad"); limelight = NetworkTableInstance.getDefault().getTable("limelight"); } diff --git a/src/main/java/frc/robot/commands/ClimberDrive.java b/src/main/java/frc/robot/commands/ClimberDrive.java index 4672e8a..fe2c300 100644 --- a/src/main/java/frc/robot/commands/ClimberDrive.java +++ b/src/main/java/frc/robot/commands/ClimberDrive.java @@ -45,14 +45,6 @@ protected void execute() { System.out.println("Toggle grabber"); } - if (rightStick.getRawButtonPressed(9)) { - climber.driveBack(0.5); - } - - if (auxJoystick.getRawButtonPressed(2)) { - climber.driveBack(1); - } - if (Math.abs(auxJoystick.getY()) <= 0.05) // Arm is disabled, just drives the climb motors here climber.driveArm(0); else diff --git a/src/main/java/frc/robot/commands/CommandBase.java b/src/main/java/frc/robot/commands/CommandBase.java index eb159a9..eab8a92 100644 --- a/src/main/java/frc/robot/commands/CommandBase.java +++ b/src/main/java/frc/robot/commands/CommandBase.java @@ -11,7 +11,6 @@ import frc.robot.subsystems.Climber; import frc.robot.subsystems.DriveTrain; import frc.robot.subsystems.Grabber; -import frc.robot.subsystems.Vacuum; import frc.robot.subsystems.Vision; import frc.robot.OI; @@ -23,7 +22,6 @@ public class CommandBase extends Command { public static Vision vision; public static Climber climber; public static Grabber grabber; - public static Vacuum vacuum; // Init all subsystems public static void init() { @@ -33,7 +31,6 @@ public static void init() { driveTrain = new DriveTrain(); vision = new Vision(); grabber = new Grabber(); - vacuum = new Vacuum(); OI.init(); } diff --git a/src/main/java/frc/robot/commands/VacuumDrive.java b/src/main/java/frc/robot/commands/DriveClimberBackwards.java similarity index 73% rename from src/main/java/frc/robot/commands/VacuumDrive.java rename to src/main/java/frc/robot/commands/DriveClimberBackwards.java index 6da4407..631a069 100644 --- a/src/main/java/frc/robot/commands/VacuumDrive.java +++ b/src/main/java/frc/robot/commands/DriveClimberBackwards.java @@ -7,37 +7,43 @@ package frc.robot.commands; -public class VacuumDrive extends CommandBase { // This is a dummy file - public VacuumDrive() { - super("dummyvacuumdrive"); +public class DriveClimberBackwards extends CommandBase { + public DriveClimberBackwards() { + super("driveclimberforward"); // Use requires() here to declare subsystem dependencies - requires(vacuum); + requires(climber); } // Called just before this Command runs the first time @Override protected void initialize() { + System.out.println("Driving climber backwards"); } // Called repeatedly when this Command is scheduled to run @Override protected void execute() { + climber.driveBack(-0.5); } // Make this return true when this Command no longer needs to run execute() @Override public boolean isFinished() { - return true; + return false; } // Called once after isFinished returns true @Override protected void end() { + System.out.println("done driving the climber backwards"); + climber.driveBack(0); } // Called when another command which requires one or more of the same // subsystems is scheduled to run @Override protected void interrupted() { + System.out.println("climber backwards drive interrupt"); + climber.driveBack(0); } } diff --git a/src/main/java/frc/robot/commands/RunVacuumForward.java b/src/main/java/frc/robot/commands/DriveClimberForward.java similarity index 74% rename from src/main/java/frc/robot/commands/RunVacuumForward.java rename to src/main/java/frc/robot/commands/DriveClimberForward.java index 476e2bc..cc46da4 100644 --- a/src/main/java/frc/robot/commands/RunVacuumForward.java +++ b/src/main/java/frc/robot/commands/DriveClimberForward.java @@ -7,24 +7,23 @@ package frc.robot.commands; -public class RunVacuumForward extends CommandBase { - public RunVacuumForward() { - super("drivevacuumcommand"); +public class DriveClimberForward extends CommandBase { + public DriveClimberForward() { + super("driveclimberforward"); // Use requires() here to declare subsystem dependencies - requires(vacuum); + requires(climber); } // Called just before this Command runs the first time @Override protected void initialize() { - System.out.println("Driving the vacuum motor to create suction..."); + System.out.println("Driving climber forward"); } // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - System.out.println("Suction in progress"); - vacuum.setForward(); + climber.driveBack(0.5); } // Make this return true when this Command no longer needs to run execute() @@ -36,13 +35,15 @@ public boolean isFinished() { // Called once after isFinished returns true @Override protected void end() { + System.out.println("done driving the climber forwards"); + climber.driveBack(0); } // Called when another command which requires one or more of the same // subsystems is scheduled to run @Override protected void interrupted() { - System.out.println("Done driving the vacuum motor forwards..."); - vacuum.setValue(0); + System.out.println("climber forward drive interrupted"); + climber.driveBack(0); } } diff --git a/src/main/java/frc/robot/commands/MoveTo47.java b/src/main/java/frc/robot/commands/MoveTo47.java index 99572a0..4585209 100644 --- a/src/main/java/frc/robot/commands/MoveTo47.java +++ b/src/main/java/frc/robot/commands/MoveTo47.java @@ -16,8 +16,6 @@ public class MoveTo47 extends CommandBase { public MoveTo47() { super("MoveTo47Inches"); requires(climber); - // Use requires() here to declare subsystem dependencies - // eg. requires(chassis); } // Called just before this Command runs the first time @@ -40,6 +38,7 @@ public boolean isFinished() { // Do not worry about this method. It will be inte // Called once after isFinished returns true @Override protected void end() { + climber.driveArm(0); } // Called when another command which requires one or more of the same diff --git a/src/main/java/frc/robot/commands/LiftToHeight.java b/src/main/java/frc/robot/commands/MoveToHighRocket.java similarity index 65% rename from src/main/java/frc/robot/commands/LiftToHeight.java rename to src/main/java/frc/robot/commands/MoveToHighRocket.java index 10a1731..6b910fe 100644 --- a/src/main/java/frc/robot/commands/LiftToHeight.java +++ b/src/main/java/frc/robot/commands/MoveToHighRocket.java @@ -7,38 +7,44 @@ package frc.robot.commands; -import edu.wpi.first.wpilibj.command.Command; +import frc.robot.RobotMap; -public class LiftToHeight extends Command { - public LiftToHeight() { - // Use requires() here to declare subsystem dependencies - // eg. requires(chassis); +public class MoveToHighRocket extends CommandBase { + + boolean upThere = false; + + public MoveToHighRocket() { + super("MoveToHighRocket"); + requires(climber); } // Called just before this Command runs the first time @Override protected void initialize() { + System.out.println("Move to high rocket started"); + climber.setArmPos(-15); } // Called repeatedly when this Command is scheduled to run @Override protected void execute() { + System.out.println("high rocket pid exec"); } - // Make this return true when this Command no longer needs to run execute() - @Override - protected boolean isFinished() { + public boolean isFinished() { // Do not worry about this method. It will be interrupted when the button is released (2) return false; } // Called once after isFinished returns true @Override protected void end() { + climber.driveArm(0); } // Called when another command which requires one or more of the same // subsystems is scheduled to run @Override protected void interrupted() { + System.out.println("Move to high rocket interrupted"); } } diff --git a/src/main/java/frc/robot/commands/RunVacuumBackwards.java b/src/main/java/frc/robot/commands/RunVacuumBackwards.java deleted file mode 100644 index 886bdc9..0000000 --- a/src/main/java/frc/robot/commands/RunVacuumBackwards.java +++ /dev/null @@ -1,46 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc.robot.commands; - -public class RunVacuumBackwards extends CommandBase { - public RunVacuumBackwards() { - super("drivevacuumbackwards"); - // Use requires() here to declare subsystem dependencies - requires(vacuum); - } - - // Called just before this Command runs the first time - @Override - protected void initialize() { - System.out.println("Starting to backdrive vacuum motor..."); - } - - // Called repeatedly when this Command is scheduled to run - @Override - protected void execute() { - System.out.println("Backdriving vacuum"); - vacuum.setBackwards(); - } - - // Make this return true when this Command no longer needs to run execute() - @Override - public boolean isFinished() { - return false; - } - - // Called once after isFinished returns true - @Override - protected void end() { - } - - @Override - protected void interrupted() { // Called when the button specified in the OI is released - System.out.println("Done backdriving the vacuum motor..."); - vacuum.setValue(0); - } -} diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 09e7705..932e483 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.PWMTalonSRX; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.command.Subsystem; import frc.robot.RobotMap; import frc.robot.commands.ClimberDrive; @@ -45,6 +46,8 @@ public Climber() { arm = new CANSparkMax(5, MotorType.kBrushless); armEncoder = new CANEncoder(arm); armPID = new CANPIDController(arm); + armEncoder.getVelocity(); + armEncoder.getPosition(); backDriveRight.setInverted(true); @@ -84,9 +87,11 @@ public void driveBack(double amnt) { public void toggleAll() { if (gg) { front.set(REVERSE); + Timer.delay(0.195); back.set(REVERSE); } else { front.set(FORWARD); + Timer.delay(0.195); back.set(FORWARD); } gg = !gg; diff --git a/src/main/java/frc/robot/subsystems/Grabber.java b/src/main/java/frc/robot/subsystems/Grabber.java index abfd781..504dec3 100644 --- a/src/main/java/frc/robot/subsystems/Grabber.java +++ b/src/main/java/frc/robot/subsystems/Grabber.java @@ -35,14 +35,17 @@ public Grabber() { public void toggle() { if (diskExtended) { + System.out.println("Grabber is closing."); disk.set(REVERSE); } else { + System.out.println("Grabber is opening."); disk.set(FORWARD); } diskExtended = !diskExtended; } public void setForward() { + System.out.println("Grabber has been set forward"); disk.set(FORWARD); diskExtended = true; } diff --git a/src/main/java/frc/robot/subsystems/Vacuum.java b/src/main/java/frc/robot/subsystems/Vacuum.java deleted file mode 100644 index 0384de1..0000000 --- a/src/main/java/frc/robot/subsystems/Vacuum.java +++ /dev/null @@ -1,41 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc.robot.subsystems; - -import edu.wpi.first.wpilibj.PWMVictorSPX; -import edu.wpi.first.wpilibj.command.Subsystem; -import frc.robot.commands.VacuumDrive; - -/** - * Add your docs here. - */ -public class Vacuum extends Subsystem { - - PWMVictorSPX vacuum; - - public Vacuum() { - vacuum = new PWMVictorSPX(0); - } - - public void setForward() { - vacuum.set(1); - } - - public void setBackwards() { - vacuum.set(-1); - } - - public void setValue(double val) { - vacuum.set(val); - } - - @Override - public void initDefaultCommand() { - setDefaultCommand(new VacuumDrive()); - } -} diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index fa047f7..4edf2b8 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -63,7 +63,7 @@ public void camMid() { public void camLow() { System.out.println("low"); - cameraServo.set(0.25); + cameraServo.set(0.10); } // Put methods for controlling this subsystem // here. Call these from Commands.