From 616b3be9606f92ee54c7ce969d8314e327df67e7 Mon Sep 17 00:00:00 2001 From: Curtis Fowler Date: Sun, 24 Feb 2019 16:56:23 -0500 Subject: [PATCH] not so pretty, but it works :/ --- src/main/java/frc/robot/OI.java | 4 -- src/main/java/frc/robot/RobotMap.java | 2 +- .../java/frc/robot/commands/CommandBase.java | 3 -- .../frc/robot/commands/MovePivotDown.java | 48 ------------------ .../java/frc/robot/commands/MovePivotUp.java | 49 ------------------- .../frc/robot/commands/VisionControl.java | 13 +++++ .../java/frc/robot/subsystems/ArmPivot.java | 37 -------------- .../java/frc/robot/subsystems/Climber.java | 6 +-- .../java/frc/robot/subsystems/Grabber.java | 3 +- .../java/frc/robot/subsystems/Vision.java | 18 +++++++ 10 files changed, 37 insertions(+), 146 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/MovePivotDown.java delete mode 100644 src/main/java/frc/robot/commands/MovePivotUp.java delete mode 100644 src/main/java/frc/robot/subsystems/ArmPivot.java diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index a5bef57..c094f0a 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -27,14 +27,10 @@ public static void init() { auxStick = new Joystick(RobotMap.auxillaryStick); Button moveArmToPos = new JoystickButton(leftDriveStick, 9); - Button driveVacuum = new JoystickButton(auxStick, 8); Button movePivotDown = new JoystickButton(auxStick, 4); Button movePivotUp = new JoystickButton(auxStick, 5); moveArmToPos.whileHeld(new MoveTo47()); - driveVacuum.whileHeld(new RunVacuumForward()); - movePivotDown.whileHeld(new MovePivotDown()); - movePivotUp.whileHeld(new MovePivotUp()); } public static Joystick getRightDriveStick() { diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 9f149fd..36dc877 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -36,7 +36,7 @@ public class RobotMap { public static final int leftBackMotor = 4; public static final int rightFrontMotor = 1; public static final int rightBackMotor = 2; - public static final float speedLimiter = 0.2f; // Out of 1.0, this value represents the max speed that the robot will + public static final float speedLimiter = 0.4f; // Out of 1.0, this value represents the max speed that the robot will // go. 0.8, for example, means 80% max motor speed public static class rightStick { diff --git a/src/main/java/frc/robot/commands/CommandBase.java b/src/main/java/frc/robot/commands/CommandBase.java index cd1f0d7..eb159a9 100644 --- a/src/main/java/frc/robot/commands/CommandBase.java +++ b/src/main/java/frc/robot/commands/CommandBase.java @@ -8,7 +8,6 @@ package frc.robot.commands; import edu.wpi.first.wpilibj.command.Command; -import frc.robot.subsystems.ArmPivot; import frc.robot.subsystems.Climber; import frc.robot.subsystems.DriveTrain; import frc.robot.subsystems.Grabber; @@ -25,7 +24,6 @@ public class CommandBase extends Command { public static Climber climber; public static Grabber grabber; public static Vacuum vacuum; - public static ArmPivot pivot; // Init all subsystems public static void init() { @@ -36,7 +34,6 @@ public static void init() { vision = new Vision(); grabber = new Grabber(); vacuum = new Vacuum(); - pivot = new ArmPivot(); OI.init(); } diff --git a/src/main/java/frc/robot/commands/MovePivotDown.java b/src/main/java/frc/robot/commands/MovePivotDown.java deleted file mode 100644 index 1a40d4a..0000000 --- a/src/main/java/frc/robot/commands/MovePivotDown.java +++ /dev/null @@ -1,48 +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 MovePivotDown extends CommandBase { - public MovePivotDown() { - super("MovePivotDown"); - // Use requires() here to declare subsystem dependencies - requires(pivot); - } - - // Called just before this Command runs the first time - @Override - protected void initialize() { - System.out.println("Drive arm down init"); - } - - // Called repeatedly when this Command is scheduled to run - @Override - protected void execute() { - System.out.println("Pivot arm down..."); - pivot.driveArm(-1); - } - - // 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() { - System.out.println("stop driving arm down"); - pivot.driveArm(0); // peaceful stop - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - @Override - protected void interrupted() { - } -} diff --git a/src/main/java/frc/robot/commands/MovePivotUp.java b/src/main/java/frc/robot/commands/MovePivotUp.java deleted file mode 100644 index 0614a6d..0000000 --- a/src/main/java/frc/robot/commands/MovePivotUp.java +++ /dev/null @@ -1,49 +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 MovePivotUp extends CommandBase { - public MovePivotUp() { - super("MovePivotUp"); - // Use requires() here to declare subsystem dependencies - requires(pivot); - } - - // Called just before this Command runs the first time - @Override - protected void initialize() { - System.out.println("Drive arm up init"); - } - - // Called repeatedly when this Command is scheduled to run - @Override - protected void execute() { - System.out.println("Pivot arm up..."); - pivot.driveArm(1); - } - - // 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() { - System.out.println("stop driving arm up"); - pivot.driveArm(0); // peaceful stop - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - @Override - protected void interrupted() { - pivot.driveArm(0); - } -} diff --git a/src/main/java/frc/robot/commands/VisionControl.java b/src/main/java/frc/robot/commands/VisionControl.java index da020eb..e33fa9b 100644 --- a/src/main/java/frc/robot/commands/VisionControl.java +++ b/src/main/java/frc/robot/commands/VisionControl.java @@ -15,6 +15,7 @@ public class VisionControl extends CommandBase { Joystick rightStick = OI.getRightDriveStick(); + Joystick auxJoystick = OI.getAuxStick(); int visionMode = 0; public VisionControl() { @@ -45,6 +46,18 @@ protected void execute() { if (rightStick.getRawButtonPressed(RobotMap.rightStick.cycleCamMode)) { vision.toggleCamMode(); } + + if (auxJoystick.getRawButtonPressed(4)) { + vision.camLow(); + } + + if (auxJoystick.getRawButtonPressed(2)) { + vision.camMid(); + } + + if (auxJoystick.getRawButtonPressed(5)) { + vision.camHigh(); + } } // Make this return true when this Command no longer needs to run execute() diff --git a/src/main/java/frc/robot/subsystems/ArmPivot.java b/src/main/java/frc/robot/subsystems/ArmPivot.java deleted file mode 100644 index 594069a..0000000 --- a/src/main/java/frc/robot/subsystems/ArmPivot.java +++ /dev/null @@ -1,37 +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.PWMTalonSRX; -import edu.wpi.first.wpilibj.command.Subsystem; -import frc.robot.RobotMap; -import frc.robot.commands.MovePivotDown; - -/** - * Add your docs here. - */ -public class ArmPivot extends Subsystem { - - PWMTalonSRX armPivot; - - public ArmPivot() { - armPivot = new PWMTalonSRX(RobotMap.arm.armTalonId); - } - // Put methods for controlling this subsystem - // here. Call these from Commands. - - public void driveArm(int num) { - armPivot.set(num); - } - - @Override - public void initDefaultCommand() { - // Set the default command for a subsystem here. - setDefaultCommand(new MovePivotDown()); - } -} diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 0a67167..09e7705 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -51,12 +51,12 @@ public Climber() { front.set(REVERSE); back.set(REVERSE); - armPID.setP(0.05); + armPID.setP(0.25); armPID.setI(0); - armPID.setD(1); + armPID.setD(0.20); armPID.setIZone(0); armPID.setFF(0); - armPID.setOutputRange(-0.15, 0.5); + armPID.setOutputRange(-0.15, 0.6); } public void setArmPos(double revs) { diff --git a/src/main/java/frc/robot/subsystems/Grabber.java b/src/main/java/frc/robot/subsystems/Grabber.java index c52d364..abfd781 100644 --- a/src/main/java/frc/robot/subsystems/Grabber.java +++ b/src/main/java/frc/robot/subsystems/Grabber.java @@ -29,7 +29,8 @@ public class Grabber extends Subsystem { public Grabber() { System.out.println("Grabber subsystem init"); - disk = new DoubleSolenoid(RobotMap.pcm.auxPcm, 0, 1); // The disk grabber solenoid is plugged into the main pcm (ID=1), 0 being the open forward channel and 1 being the reverse + disk = new DoubleSolenoid(RobotMap.pcm.mainPcm, 4, 5); // The disk grabber solenoid is plugged into the main pcm (ID=1), 0 being the open forward channel and 1 being the reverse + disk.set(REVERSE); } public void toggle() { diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index bccca2d..fa047f7 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -10,6 +10,7 @@ import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.command.Subsystem; import frc.robot.commands.VisionControl; @@ -20,9 +21,11 @@ public class Vision extends Subsystem { NetworkTable nTable; NetworkTableEntry tx, ty, ta, tv, ts; + Servo cameraServo; boolean driverMode; public Vision() { + cameraServo = new Servo(7); nTable = NetworkTableInstance.getDefault().getTable("limelight"); tx = nTable.getEntry("tx"); ty = nTable.getEntry("ty"); @@ -47,6 +50,21 @@ public void toggleCamMode() { nTable.getEntry("camMode").setDouble(0); } } + + public void camHigh() { + System.out.println("high"); + cameraServo.set(0.7); + } + + public void camMid() { + System.out.println("mid"); + cameraServo.set(0.5); + } + + public void camLow() { + System.out.println("low"); + cameraServo.set(0.25); + } // Put methods for controlling this subsystem // here. Call these from Commands. @Override