From cf5c116738ea0f1956168ab78a94ef3617fd41b8 Mon Sep 17 00:00:00 2001 From: Curtis Fowler Date: Thu, 14 Mar 2019 22:06:31 -0500 Subject: [PATCH 1/3] works but just barely --- src/main/java/frc/robot/OI.java | 10 +- src/main/java/frc/robot/Robot.java | 5 + .../robot/commands/AutoNavXBalanceClimb.java | 93 +++++++++++ .../java/frc/robot/commands/ClimberDrive.java | 33 ++-- .../frc/robot/commands/DriveBackClimberX.java | 59 +++++++ .../robot/commands/DriveClimberBackwards.java | 6 +- .../robot/commands/DriveClimberForward.java | 6 +- .../robot/commands/DriveFrontClimberX.java | 75 +++++++++ .../java/frc/robot/subsystems/Climber.java | 146 ++++++++++-------- vendordeps/Phoenix.json | 64 +++++++- vendordeps/REVRobotics.json | 33 +++- vendordeps/navx_frc.json | 6 +- 12 files changed, 437 insertions(+), 99 deletions(-) create mode 100644 src/main/java/frc/robot/commands/AutoNavXBalanceClimb.java create mode 100644 src/main/java/frc/robot/commands/DriveBackClimberX.java create mode 100644 src/main/java/frc/robot/commands/DriveFrontClimberX.java diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index 3fe5283..951fa0c 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -28,13 +28,15 @@ public static void init() { Button moveArmToPos = new JoystickButton(leftDriveStick, 9); Button moveArmToHighestRocket = new JoystickButton(leftDriveStick, 8); - Button driveClimberForward = new JoystickButton(rightDriveStick, 4); - Button driveClimberBackwards = new JoystickButton(rightDriveStick, 5); + Button navXClimb = new JoystickButton(rightDriveStick, 3); + Button driveFrontClimber = new JoystickButton(rightDriveStick, 8); + Button driveBackClimber = new JoystickButton(rightDriveStick, 9); + navXClimb.whileHeld(new AutoNavXBalanceClimb()); moveArmToHighestRocket.whileHeld(new MoveToHighRocket()); moveArmToPos.whileHeld(new MoveTo47()); - driveClimberForward.whileHeld(new DriveClimberForward()); - driveClimberBackwards.whileHeld(new DriveClimberBackwards()); + driveFrontClimber.toggleWhenPressed(new DriveFrontClimberX()); + driveBackClimber.toggleWhenPressed(new DriveBackClimberX()); } public static Joystick getRightDriveStick() { diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a9975dc..d1aab8d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -115,6 +115,11 @@ public void teleopInit() { @Override public void teleopPeriodic() { Scheduler.getInstance().run(); + SmartDashboard.putNumber("Motor #1 position", CommandBase.climber.getFrontClimbEncoder()); + SmartDashboard.putNumber("Motor #1 velocity", CommandBase.climber.getFrontClimbVoltage()); + SmartDashboard.putNumber("Motor #2 position", CommandBase.climber.getBackClimbEncoder()); + SmartDashboard.putNumber("Motor #2 velocity", CommandBase.climber.getBackClimbVoltage()); + SmartDashboard.putNumber("navx roll", CommandBase.climber.getRoll()); } /** diff --git a/src/main/java/frc/robot/commands/AutoNavXBalanceClimb.java b/src/main/java/frc/robot/commands/AutoNavXBalanceClimb.java new file mode 100644 index 0000000..9c02af9 --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoNavXBalanceClimb.java @@ -0,0 +1,93 @@ +/*----------------------------------------------------------------------------*/ +/* 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; + +import edu.wpi.first.wpilibj.Joystick; +import frc.robot.OI; + +public class AutoNavXBalanceClimb extends CommandBase { + + Joystick rightStick; + + public AutoNavXBalanceClimb() { + super("navxautobalance"); + // Use requires() here to declare subsystem dependencies + requires(climber); + rightStick = OI.getRightDriveStick(); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + System.out.println("auto balance navx started"); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + double roll = climber.getRoll(); + if (Math.abs(roll) < 2) + roll = 0; + if (roll == 0) { + if (climber.getBackClimbEncoder() > -65 && climber.getFrontClimbEncoder() > -65) { + climber.driveClimbBoth(-0.15); + } else { + climber.driveBackClimber(0); + } + } + ; + if (roll > 0 && Math.ceil(climber.getFrontClimbEncoder()) >= -65) { + System.out.println("I'm leaning forwards... adjusting"); + if (climber.getFrontClimbEncoder() > -65) { + climber.driveFrontClimber(-0.20); + climber.driveBackClimber(0); + } else { + climber.driveFrontClimber(0); + climber.driveBackClimber(0); + } + } else if (roll < 0) { + System.out.println("I'm leaning backwards... adjusting"); + if (climber.getBackClimbEncoder() > -65) { + climber.driveBackClimber(-0.20); + climber.driveFrontClimber(0); + } else { + climber.driveBackClimber(0); + climber.driveFrontClimber(0); + } + } + if (rightStick.getRawButton(4)) { + climber.driveBothClimbAxleWheels(-0.50); + } + if (rightStick.getRawButton(5)) { + climber.driveBothClimbAxleWheels(0.50); + } + } + + // Make this return true when this Command no longer needs to run execute() + @Override + public boolean isFinished() { + return Math.ceil(climber.getFrontClimbEncoder()) <= -65 && Math.ceil(climber.getBackClimbEncoder()) <= -65; + } + + // Called once after isFinished returns true + @Override + protected void end() { + climber.driveClimbBoth(0); + climber.driveBothClimbAxleWheels(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("auto balance interrupt"); + climber.driveClimbBoth(0); + climber.driveBackClimber(0); + climber.driveBothClimbAxleWheels(0); + } +} diff --git a/src/main/java/frc/robot/commands/ClimberDrive.java b/src/main/java/frc/robot/commands/ClimberDrive.java index fe2c300..2deea1b 100644 --- a/src/main/java/frc/robot/commands/ClimberDrive.java +++ b/src/main/java/frc/robot/commands/ClimberDrive.java @@ -27,29 +27,43 @@ public ClimberDrive() { // Called just before this Command runs the first time @Override protected void initialize() { + System.out.println("climberdrive init"); } // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - if (rightStick.getRawButtonPressed(6)) - climber.toggleFront(); - if (rightStick.getRawButtonPressed(7)) - climber.toggleBack(); - // if (rightStick.getRawButtonPressed(10)) climber.togglePiston(10); - if (rightStick.getRawButtonPressed(3)) - climber.toggleAll(); - if (auxJoystick.getRawButtonPressed(1)) { grabber.toggle(); System.out.println("Toggle grabber"); } - if (Math.abs(auxJoystick.getY()) <= 0.05) // Arm is disabled, just drives the climb motors here + if (rightStick.getRawButton(4)) { + climber.driveBothClimbAxleWheels(0.25); + } + + if (rightStick.getRawButton(5)) { + climber.driveBothClimbAxleWheels(-0.25); + } + + if (rightStick.getRawButtonPressed(7)) { + System.out.println("reset both climb encoders"); + climber.resetBothClimbEncoders(); + } + + if (Math.abs(auxJoystick.getY()) <= 0.05) climber.driveArm(0); else climber.driveArm(auxJoystick.getY()); + if (Math.abs(rightStick.getY()) <= 0.05) { + climber.driveClimbBoth(0); + }else { + climber.driveClimbBoth(rightStick.getY()); + } + + climber.driveBothClimbAxleWheels(0); + SmartDashboard.putNumber("Position", climber.getPosition()); SmartDashboard.putNumber("Velocity", climber.getVelocity()); } @@ -69,5 +83,6 @@ protected void end() { // subsystems is scheduled to run @Override protected void interrupted() { + System.out.println("climberdrive interrupt"); } } diff --git a/src/main/java/frc/robot/commands/DriveBackClimberX.java b/src/main/java/frc/robot/commands/DriveBackClimberX.java new file mode 100644 index 0000000..9ae9bc4 --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveBackClimberX.java @@ -0,0 +1,59 @@ +/*----------------------------------------------------------------------------*/ +/* 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; + +import edu.wpi.first.wpilibj.Joystick; +import frc.robot.OI; +import frc.robot.RobotMap.rightStick; + +public class DriveBackClimberX extends CommandBase { + + Joystick rightStick; + + public DriveBackClimberX() { + super("drivebackclimberx"); + // Use requires() here to declare subsystem dependencies + requires(climber); + rightStick = OI.getRightDriveStick(); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + System.out.println("back climber drive init"); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + if (Math.abs(rightStick.getY()) >= 0.05) { + climber.driveBackClimber(rightStick.getY()); + } else climber.driveBackClimber(0); + } + + // 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("front climber done"); + climber.driveBackClimber(0); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + climber.driveBackClimber(0); + System.out.println("back climber drive interrupt"); + } +} diff --git a/src/main/java/frc/robot/commands/DriveClimberBackwards.java b/src/main/java/frc/robot/commands/DriveClimberBackwards.java index 631a069..e67450d 100644 --- a/src/main/java/frc/robot/commands/DriveClimberBackwards.java +++ b/src/main/java/frc/robot/commands/DriveClimberBackwards.java @@ -23,7 +23,7 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - climber.driveBack(-0.5); + climber.driveBothClimbAxleWheels(-0.25); } // Make this return true when this Command no longer needs to run execute() @@ -36,7 +36,7 @@ public boolean isFinished() { @Override protected void end() { System.out.println("done driving the climber backwards"); - climber.driveBack(0); + climber.driveBothClimbAxleWheels(0); } // Called when another command which requires one or more of the same @@ -44,6 +44,6 @@ protected void end() { @Override protected void interrupted() { System.out.println("climber backwards drive interrupt"); - climber.driveBack(0); + climber.driveBothClimbAxleWheels(0); } } diff --git a/src/main/java/frc/robot/commands/DriveClimberForward.java b/src/main/java/frc/robot/commands/DriveClimberForward.java index cc46da4..392c109 100644 --- a/src/main/java/frc/robot/commands/DriveClimberForward.java +++ b/src/main/java/frc/robot/commands/DriveClimberForward.java @@ -23,7 +23,7 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - climber.driveBack(0.5); + climber.driveClimbBoth(0.25); } // Make this return true when this Command no longer needs to run execute() @@ -36,7 +36,7 @@ public boolean isFinished() { @Override protected void end() { System.out.println("done driving the climber forwards"); - climber.driveBack(0); + climber.driveClimbBoth(0); } // Called when another command which requires one or more of the same @@ -44,6 +44,6 @@ protected void end() { @Override protected void interrupted() { System.out.println("climber forward drive interrupted"); - climber.driveBack(0); + climber.driveClimbBoth(0); } } diff --git a/src/main/java/frc/robot/commands/DriveFrontClimberX.java b/src/main/java/frc/robot/commands/DriveFrontClimberX.java new file mode 100644 index 0000000..f218ef1 --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveFrontClimberX.java @@ -0,0 +1,75 @@ +/*----------------------------------------------------------------------------*/ +/* 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; + +import edu.wpi.first.wpilibj.Joystick; +import frc.robot.OI; +import frc.robot.RobotMap.rightStick; + +public class DriveFrontClimberX extends CommandBase { + + Joystick rightStick; + + public DriveFrontClimberX() { + super("drivefrontclimberx"); + // Use requires() here to declare subsystem dependencies + requires(climber); + rightStick = OI.getRightDriveStick(); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + System.out.println("front climber drive init"); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + if (Math.abs(rightStick.getY()) >= 0.05) { + climber.driveFrontClimber(rightStick.getY()); + } else climber.driveFrontClimber(0); + if (climber.getBackClimbEncoder() >= -65) { + climber.driveBackClimber(-0.15); + } else { + climber.driveBackClimber(0); + } + if (rightStick.getRawButton(5)) { + climber.driveBothClimbAxleWheels(-0.25); + } + if (rightStick.getRawButton(4)) { + climber.driveBothClimbAxleWheels(0.25); + } + if (!rightStick.getRawButton(4) && !rightStick.getRawButton(5)) { + climber.driveBothClimbAxleWheels(0); + } + } + + // 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("is finished front"); + climber.driveFrontClimber(0); + climber.driveBackClimber(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("front climber drive interrupt"); + climber.driveFrontClimber(0); + climber.driveBackClimber(0); + } +} diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 932e483..3ce7314 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -7,6 +7,7 @@ package frc.robot.subsystems; +import com.kauailabs.navx.frc.AHRS; import com.revrobotics.CANEncoder; import com.revrobotics.CANPIDController; import com.revrobotics.CANSparkMax; @@ -15,7 +16,8 @@ import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.PWMTalonSRX; -import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.SPI; +import edu.wpi.first.wpilibj.Ultrasonic; import edu.wpi.first.wpilibj.command.Subsystem; import frc.robot.RobotMap; import frc.robot.commands.ClimberDrive; @@ -28,31 +30,31 @@ public class Climber extends Subsystem { DoubleSolenoid.Value FORWARD = DoubleSolenoid.Value.kForward; DoubleSolenoid.Value REVERSE = DoubleSolenoid.Value.kReverse; DoubleSolenoid.Value OFF = DoubleSolenoid.Value.kReverse; - DoubleSolenoid front, back; PWMTalonSRX backDriveLeft, backDriveRight; - CANSparkMax arm; - CANEncoder armEncoder; + CANSparkMax arm, front, back; + AHRS navX; + CANEncoder armEncoder, frontClimbEncoder, backClimbEncoder; CANPIDController armPID; - boolean rightFrontExtended, rightBackExtended, leftFrontExtended, leftBackExtended, gg = false; + Ultrasonic frontSensor, backSensor; public Climber() { System.out.println("Climber subsystem init"); - front = new DoubleSolenoid(RobotMap.pcm.mainPcm, RobotMap.climber.rightFrontExtend, RobotMap.climber.rightFrontReverse); - back = new DoubleSolenoid(RobotMap.pcm.mainPcm, RobotMap.climber.rightBackExtend, RobotMap.climber.rightBackReverse); - - backDriveLeft = new PWMTalonSRX(8); - backDriveRight = new PWMTalonSRX(9); - - arm = new CANSparkMax(5, MotorType.kBrushless); - armEncoder = new CANEncoder(arm); - armPID = new CANPIDController(arm); - armEncoder.getVelocity(); - armEncoder.getPosition(); - - backDriveRight.setInverted(true); - - front.set(REVERSE); - back.set(REVERSE); + navX = new AHRS(SPI.Port.kMXP); + frontSensor = new Ultrasonic(1, 0); + backSensor = new Ultrasonic(3, 2); + backDriveLeft = new PWMTalonSRX(8); + backDriveRight = new PWMTalonSRX(9); + front = new CANSparkMax(6, MotorType.kBrushless); + back = new CANSparkMax(7, MotorType.kBrushless); + + arm = new CANSparkMax(5, MotorType.kBrushless); + armEncoder = new CANEncoder(arm); + frontClimbEncoder = new CANEncoder(front); + backClimbEncoder = new CANEncoder(back); + armPID = new CANPIDController(arm); + + backDriveRight.setInverted(true); + back.setInverted(true); armPID.setP(0.25); armPID.setI(0); @@ -62,6 +64,65 @@ public Climber() { armPID.setOutputRange(-0.15, 0.6); } + public void resetBothClimbEncoders() { + frontClimbEncoder.setPosition(0); + backClimbEncoder.setPosition(0); + } + + public double getFrontSensorInches() { + return frontSensor.getRangeInches(); + } + + public double getBackSensorInches() { + return backSensor.getRangeInches(); + } + + public void driveFrontClimber(double val) { + front.set(val); + } + + public void driveBackClimber(double val) { + back.set(val); + } + + public void driveClimbBoth(double val) { + front.set(val); + back.set(val); + } + + public double getRoll() { + return navX.getRoll(); + } + + public void driveLeftClimbAxle(double val) { + backDriveLeft.set(val); + } + + public void driveRightClimbAxle(double val) { + backDriveRight.set(val); + } + + public void driveBothClimbAxleWheels(double val) { + backDriveLeft.set(val); + backDriveRight.set(val); + } + + public double getFrontClimbEncoder() { + return frontClimbEncoder.getPosition(); + } + + public double getBackClimbEncoder() { + return backClimbEncoder.getPosition(); + } + + public double getFrontClimbVoltage() { + return front.getBusVoltage(); + } + + public double getBackClimbVoltage() { + return back.getBusVoltage(); + } + public void setArmPos(double revs) { armPID.setReference(revs, ControlType.kPosition); } @@ -74,53 +135,10 @@ public double getVelocity() { return armEncoder.getVelocity(); } - public void driveBack(double amnt) { - backDriveLeft.set(amnt); - backDriveRight.set(amnt); - } - - // public void driveIndividual(double left, double right) { - // backDriveLeft.set(left); - // backDriveRight.set(right); - // } - - 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; - } - public void driveArm(double num) { arm.set(num * RobotMap.speedLimiter); } - public void toggleBack() { - if (rightBackExtended && leftBackExtended) { - back.set(REVERSE); - } else { - back.set(FORWARD); - } - rightBackExtended = !rightBackExtended; - leftBackExtended = !leftBackExtended; - } - - public void toggleFront() { - if (rightFrontExtended && leftFrontExtended) { - front.set(REVERSE); - } else { - front.set(FORWARD); - } - rightFrontExtended = !rightFrontExtended; - leftFrontExtended = !leftFrontExtended; - } - @Override public void initDefaultCommand() { setDefaultCommand(new ClimberDrive()); diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json index a1654ec..f52fc06 100644 --- a/vendordeps/Phoenix.json +++ b/vendordeps/Phoenix.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix.json", "name": "CTRE-Phoenix", - "version": "5.12.1", + "version": "5.14.1", "uuid": "ab676553-b602-441f-a38d-f1296eff6537", "mavenUrls": [ "http://devsite.ctr-electronics.com/maven/release/" @@ -11,19 +11,19 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-java", - "version": "5.12.1" + "version": "5.14.1" }, { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-java", - "version": "5.12.1" + "version": "5.14.1" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.12.1", + "version": "5.14.1", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -31,13 +31,35 @@ "windowsx86-64", "linuxx86-64" ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "canutils", + "version": "5.14.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "platform-stub", + "version": "5.14.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] } ], "cppDependencies": [ { "groupId": "com.ctre.phoenix", "artifactId": "wpiapi-cpp", - "version": "5.12.1", + "version": "5.14.1", "libName": "CTRE_Phoenix_WPI", "headerClassifier": "headers", "sharedLibrary": false, @@ -51,7 +73,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "api-cpp", - "version": "5.12.1", + "version": "5.14.1", "libName": "CTRE_Phoenix", "headerClassifier": "headers", "sharedLibrary": false, @@ -65,7 +87,7 @@ { "groupId": "com.ctre.phoenix", "artifactId": "cci", - "version": "5.12.1", + "version": "5.14.1", "libName": "CTRE_PhoenixCCI", "headerClassifier": "headers", "sharedLibrary": false, @@ -76,10 +98,36 @@ "linuxx86-64" ] }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "canutils", + "version": "5.14.1", + "libName": "CTRE_PhoenixCanutils", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "platform-stub", + "version": "5.14.1", + "libName": "CTRE_PhoenixPlatform", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64" + ] + }, { "groupId": "com.ctre.phoenix", "artifactId": "core", - "version": "5.12.1", + "version": "5.14.1", "libName": "CTRE_PhoenixCore", "headerClassifier": "headers" } diff --git a/vendordeps/REVRobotics.json b/vendordeps/REVRobotics.json index d997798..a70dc13 100644 --- a/vendordeps/REVRobotics.json +++ b/vendordeps/REVRobotics.json @@ -1,8 +1,8 @@ { "fileName": "REVRobotics.json", "name": "REVRobotics", - "version": "1.0.26", - "uuid": "c16ed09f-23df-4beb-87e8-460bd7fa9924", + "version": "1.1.9", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ "http://www.revrobotics.com/content/sw/max/sdk/maven/" ], @@ -11,15 +11,26 @@ { "groupId": "com.revrobotics.frc", "artifactId": "SparkMax-java", - "version": "1.0.26" + "version": "1.1.9" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "SparkMax-driver", + "version": "1.1.9", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "linuxathena" + ] } ], - "jniDependencies": [], "cppDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "SparkMax-cpp", - "version": "1.0.26", + "version": "1.1.9", "libName": "libSparkMax", "headerClassifier": "headers", "sharedLibrary": false, @@ -27,6 +38,18 @@ "binaryPlatforms": [ "linuxathena" ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "SparkMax-driver", + "version": "1.1.9", + "libName": "libSparkMaxDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena" + ] } ] } \ No newline at end of file diff --git a/vendordeps/navx_frc.json b/vendordeps/navx_frc.json index ace2550..49c7d70 100644 --- a/vendordeps/navx_frc.json +++ b/vendordeps/navx_frc.json @@ -1,7 +1,7 @@ { "fileName": "navx_frc.json", "name": "KauaiLabs_navX_FRC", - "version": "3.1.339", + "version": "3.1.366", "uuid": "cb311d09-36e9-4143-a032-55bb2b94443b", "mavenUrls": [ "https://repo1.maven.org/maven2/" @@ -11,7 +11,7 @@ { "groupId": "com.kauailabs.navx.frc", "artifactId": "navx-java", - "version": "3.1.339" + "version": "3.1.366" } ], "jniDependencies": [], @@ -19,7 +19,7 @@ { "groupId": "com.kauailabs.navx.frc", "artifactId": "navx-cpp", - "version": "3.1.339", + "version": "3.1.366", "headerClassifier": "headers", "sourcesClassifier": "sources", "sharedLibrary": false, From f595837cbf332df5e241249ecdf8c7ca502ddcd5 Mon Sep 17 00:00:00 2001 From: Curtis Fowler Date: Fri, 15 Mar 2019 22:37:53 -0500 Subject: [PATCH 2/3] wip friday --- .../robot/commands/AutoNavXBalanceClimb.java | 4 ++ .../AutoApproachPlatform3Back.java | 63 +++++++++++++++++ .../AutoApproachPlatform3Front.java | 55 +++++++++++++++ .../Hab3AutoClimb/AutoRetractBackAxle.java | 68 +++++++++++++++++++ .../Hab3AutoClimb/AutoRetractFrontAxle.java | 56 +++++++++++++++ .../commands/Hab3AutoClimb/AutoToHab3.java | 34 ++++++++++ 6 files changed, 280 insertions(+) create mode 100644 src/main/java/frc/robot/commands/Hab3AutoClimb/AutoApproachPlatform3Back.java create mode 100644 src/main/java/frc/robot/commands/Hab3AutoClimb/AutoApproachPlatform3Front.java create mode 100644 src/main/java/frc/robot/commands/Hab3AutoClimb/AutoRetractBackAxle.java create mode 100644 src/main/java/frc/robot/commands/Hab3AutoClimb/AutoRetractFrontAxle.java create mode 100644 src/main/java/frc/robot/commands/Hab3AutoClimb/AutoToHab3.java diff --git a/src/main/java/frc/robot/commands/AutoNavXBalanceClimb.java b/src/main/java/frc/robot/commands/AutoNavXBalanceClimb.java index 9c02af9..5a986c4 100644 --- a/src/main/java/frc/robot/commands/AutoNavXBalanceClimb.java +++ b/src/main/java/frc/robot/commands/AutoNavXBalanceClimb.java @@ -30,6 +30,10 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { + balance(); + } + + public void balance() { double roll = climber.getRoll(); if (Math.abs(roll) < 2) roll = 0; diff --git a/src/main/java/frc/robot/commands/Hab3AutoClimb/AutoApproachPlatform3Back.java b/src/main/java/frc/robot/commands/Hab3AutoClimb/AutoApproachPlatform3Back.java new file mode 100644 index 0000000..5a8c67f --- /dev/null +++ b/src/main/java/frc/robot/commands/Hab3AutoClimb/AutoApproachPlatform3Back.java @@ -0,0 +1,63 @@ +/*----------------------------------------------------------------------------*/ +/* 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.Hab3AutoClimb; + +import frc.robot.commands.CommandBase; + +public class AutoApproachPlatform3Back extends CommandBase { + public AutoApproachPlatform3Back() { + super("approachplatformusingback"); + // Use requires() here to declare subsystem dependencies + requires(climber); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + System.out.println("Pushing onto the platform using back sensor"); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + double distance = climber.getBackSensorInches(); + if (distance > 1.1) { + climber.driveBothClimbAxleWheels(0.35); + } else { + climber.driveBothClimbAxleWheels(0); + } + if (climber.getBackClimbEncoder() > -65) { + climber.driveBackClimber(-0.20); + } else { + climber.driveBackClimber(0); + } + } + + // Make this return true when this Command no longer needs to run execute() + @Override + public boolean isFinished() { + return climber.getBackSensorInches() < 1.1; + } + + // Called once after isFinished returns true + @Override + protected void end() { + climber.driveBothClimbAxleWheels(0); + climber.driveBackClimber(0); + System.out.println("The back axle is 1.1\" away from the back platform. ending."); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + climber.driveBothClimbAxleWheels(0); + climber.driveBackClimber(0); + System.out.println("The automatic approach to the back platform was interrupted."); + } +} diff --git a/src/main/java/frc/robot/commands/Hab3AutoClimb/AutoApproachPlatform3Front.java b/src/main/java/frc/robot/commands/Hab3AutoClimb/AutoApproachPlatform3Front.java new file mode 100644 index 0000000..978e9f6 --- /dev/null +++ b/src/main/java/frc/robot/commands/Hab3AutoClimb/AutoApproachPlatform3Front.java @@ -0,0 +1,55 @@ +/*----------------------------------------------------------------------------*/ +/* 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.Hab3AutoClimb; + +import frc.robot.commands.AutoNavXBalanceClimb; + +public class AutoApproachPlatform3Front extends AutoNavXBalanceClimb { + public AutoApproachPlatform3Front() { + // Use requires() here to declare subsystem dependencies + requires(climber); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + System.out.println("Approaching the platform while auto balancing"); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + double distance = climber.getFrontSensorInches(); + System.out.println("F: " + distance); + if (distance > 1.1) { + climber.driveBothClimbAxleWheels(0.15); + } + this.balance(); // maintain balance while approaching + } + + // Make this return true when this Command no longer needs to run execute() + @Override + public boolean isFinished() { + return climber.getFrontSensorInches() < 1.1; + } + + // Called once after isFinished returns true + @Override + protected void end() { + climber.driveBothClimbAxleWheels(0); + System.out.println("The front axle is in optimal position, ended."); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + climber.driveBothClimbAxleWheels(0); + System.out.println("The front axle approach to the hab has been interrupted."); + } +} diff --git a/src/main/java/frc/robot/commands/Hab3AutoClimb/AutoRetractBackAxle.java b/src/main/java/frc/robot/commands/Hab3AutoClimb/AutoRetractBackAxle.java new file mode 100644 index 0000000..62eed67 --- /dev/null +++ b/src/main/java/frc/robot/commands/Hab3AutoClimb/AutoRetractBackAxle.java @@ -0,0 +1,68 @@ +/*----------------------------------------------------------------------------*/ +/* 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.Hab3AutoClimb; + +import edu.wpi.first.wpilibj.Joystick; +import frc.robot.OI; +import frc.robot.commands.CommandBase; + +public class AutoRetractBackAxle extends CommandBase { + + Joystick arcadeStick = OI.getLeftDriveStick(); // arcade stick + + public AutoRetractBackAxle() { + super("autoretractbackaxle"); + // Use requires() here to declare subsystem dependencies + requires(climber); + requires(driveTrain); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + System.out.println("going to retract the back climber."); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + if (Math.ceil(climber.getBackClimbEncoder()) < 0) { + climber.driveBackClimber(0.15); + } else { + climber.driveBackClimber(0); + } + if (Math.abs(arcadeStick.getY()) < 0.05) { + driveTrain.arcadeDrive(0, 0); + } else { + driveTrain.arcadeDrive(arcadeStick.getX(), arcadeStick.getY()); + } + } + + // Make this return true when this Command no longer needs to run execute() + @Override + public boolean isFinished() { + return Math.ceil(climber.getBackClimbEncoder()) >= 0; + } + + // Called once after isFinished returns true + @Override + protected void end() { + climber.driveBothClimbAxleWheels(0); + climber.driveClimbBoth(0); + System.out.println("done retracting the back axle."); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + climber.driveBothClimbAxleWheels(0); + climber.driveClimbBoth(0); + System.out.println("back axle retraction command has been interrupted."); + } +} diff --git a/src/main/java/frc/robot/commands/Hab3AutoClimb/AutoRetractFrontAxle.java b/src/main/java/frc/robot/commands/Hab3AutoClimb/AutoRetractFrontAxle.java new file mode 100644 index 0000000..954d253 --- /dev/null +++ b/src/main/java/frc/robot/commands/Hab3AutoClimb/AutoRetractFrontAxle.java @@ -0,0 +1,56 @@ +/*----------------------------------------------------------------------------*/ +/* 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.Hab3AutoClimb; + +import frc.robot.commands.CommandBase; + +public class AutoRetractFrontAxle extends CommandBase { + public AutoRetractFrontAxle() { + super("retractthebackaxle"); + // Use requires() here to declare subsystem dependencies + requires(climber); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + System.out.println("I am retracting the front axle"); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + climber.driveFrontClimber(0.15); // Retract front axle + if (climber.getBackClimbEncoder() > -65) { + climber.driveBackClimber(-0.20); // Correct itself + } else { + climber.driveBackClimber(0); // don't keep on correcting... + } + } + + // Make this return true when this Command no longer needs to run execute() + @Override + public boolean isFinished() { + return Math.ceil(climber.getFrontClimbEncoder()) == 0; + } + + // Called once after isFinished returns true + @Override + protected void end() { + climber.driveClimbBoth(0); + System.out.println("finished retracting the front axle"); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + climber.driveClimbBoth(0); + System.out.println("retracting the front axle was interrupted."); + } +} diff --git a/src/main/java/frc/robot/commands/Hab3AutoClimb/AutoToHab3.java b/src/main/java/frc/robot/commands/Hab3AutoClimb/AutoToHab3.java new file mode 100644 index 0000000..ddd437b --- /dev/null +++ b/src/main/java/frc/robot/commands/Hab3AutoClimb/AutoToHab3.java @@ -0,0 +1,34 @@ +/*----------------------------------------------------------------------------*/ +/* 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.Hab3AutoClimb; + +import edu.wpi.first.wpilibj.command.CommandGroup; + +public class AutoToHab3 extends CommandGroup { + /** + * Add your docs here. + */ + public AutoToHab3() { + // Add Commands here: + // e.g. addSequential(new Command1()); + // addSequential(new Command2()); + // these will run in order. + + // To run multiple commands at the same time, + // use addParallel() + // e.g. addParallel(new Command1()); + // addSequential(new Command2()); + // Command1 and Command2 will run in parallel. + + // A command group will require all of the subsystems that each member + // would require. + // e.g. if Command1 requires chassis, and Command2 requires arm, + // a CommandGroup containing them would require both the chassis and the + // arm. + } +} From 184c898b14a7b5be109e54a612189fe0bb6a3052 Mon Sep 17 00:00:00 2001 From: Curtis Fowler Date: Thu, 21 Mar 2019 19:33:20 -0500 Subject: [PATCH 3/3] after first comp, sorry for staying behind --- src/main/java/frc/robot/OI.java | 5 ++ .../java/frc/robot/commands/ArcadeDrive.java | 7 ++- .../frc/robot/commands/AutoDriveToTarget.java | 17 ++--- .../robot/commands/AutoNavXBalanceClimb.java | 16 ++--- .../java/frc/robot/commands/ClimberDrive.java | 20 +++--- .../frc/robot/commands/DriveBackClimberX.java | 24 +++++-- .../robot/commands/DriveFrontClimberX.java | 18 +++--- .../Hab3AutoClimb/AutoRetractBackAxle.java | 4 +- .../Hab3AutoClimb/AutoRetractFrontAxle.java | 2 +- .../commands/Hab3AutoClimb/AutoToHab3.java | 8 +++ .../java/frc/robot/commands/MoveTo47.java | 3 +- .../frc/robot/commands/MoveToHighRocket.java | 5 +- .../frc/robot/commands/ResetEncoders.java | 45 +++++++++++++ .../RetractFrontAxleNoAutoBalance.java | 63 +++++++++++++++++++ .../java/frc/robot/subsystems/Climber.java | 6 +- 15 files changed, 200 insertions(+), 43 deletions(-) create mode 100644 src/main/java/frc/robot/commands/ResetEncoders.java create mode 100644 src/main/java/frc/robot/commands/RetractFrontAxleNoAutoBalance.java diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index 951fa0c..ea8eaee 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj.buttons.Button; import edu.wpi.first.wpilibj.buttons.JoystickButton; import frc.robot.commands.*; +import frc.robot.commands.Hab3AutoClimb.AutoToHab3; /** * This class is the glue that binds the controls on the physical operator @@ -31,12 +32,16 @@ public static void init() { Button navXClimb = new JoystickButton(rightDriveStick, 3); Button driveFrontClimber = new JoystickButton(rightDriveStick, 8); Button driveBackClimber = new JoystickButton(rightDriveStick, 9); + Button autoLimelight = new JoystickButton(leftDriveStick, 4); + //Button autoNavxClimb = new JoystickButton(rightDriveStick, 2); navXClimb.whileHeld(new AutoNavXBalanceClimb()); moveArmToHighestRocket.whileHeld(new MoveToHighRocket()); moveArmToPos.whileHeld(new MoveTo47()); driveFrontClimber.toggleWhenPressed(new DriveFrontClimberX()); driveBackClimber.toggleWhenPressed(new DriveBackClimberX()); + autoLimelight.whileHeld(new AutoDriveToTarget()); + //autoNavxClimb.toggleWhenPressed(new AutoToHab3()); } public static Joystick getRightDriveStick() { diff --git a/src/main/java/frc/robot/commands/ArcadeDrive.java b/src/main/java/frc/robot/commands/ArcadeDrive.java index 7852218..1b44259 100644 --- a/src/main/java/frc/robot/commands/ArcadeDrive.java +++ b/src/main/java/frc/robot/commands/ArcadeDrive.java @@ -21,14 +21,14 @@ public class ArcadeDrive extends CommandBase { public ArcadeDrive() { super("ArcadeDrive"); - System.out.println("TankDrive command init"); + System.out.println("arcade command init"); requires(driveTrain); } // Called just before this Command runs the first time @Override protected void initialize() { - System.out.println("TankDrive command init 2"); + System.out.println("arcadedrive command init 2"); rightStick = OI.getRightDriveStick(); leftStick = OI.getLeftDriveStick(); } @@ -36,6 +36,9 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { + if (leftStick.getRawButtonPressed(1)) { + grabber.toggle(); + } double leftX = Math.abs(leftStick.getX()) < 0.05 ? 0 : leftStick.getX(); double leftY = Math.abs(leftStick.getY()) < 0.05 ? 0 : leftStick.getY(); // Handle deadband double rightX = Math.abs(rightStick.getX()) < 0.05 ? 0 : rightStick.getX(); diff --git a/src/main/java/frc/robot/commands/AutoDriveToTarget.java b/src/main/java/frc/robot/commands/AutoDriveToTarget.java index 9446716..5d7bd07 100644 --- a/src/main/java/frc/robot/commands/AutoDriveToTarget.java +++ b/src/main/java/frc/robot/commands/AutoDriveToTarget.java @@ -20,7 +20,6 @@ public AutoDriveToTarget() { super("AutoDrive"); requires(driveTrain); requires(vision); - requires(climber); limelight = NetworkTableInstance.getDefault().getTable("limelight"); } @@ -30,6 +29,7 @@ public AutoDriveToTarget() { protected void initialize() { System.out.println("Init"); xOffset = limelight.getEntry("tx").getDouble(0); + vision.setLedMode(3); } // Called repeatedly when this Command is scheduled to run @@ -40,18 +40,18 @@ protected void execute() { if (target == 0) { System.out.println("No target. Spinning aimlessly until I find something."); - // climber.driveIndividual(0.3, -0.3); + driveTrain.tankDrive(0.15, -0.15); } else if (target == 1) { double throttle = 0.07 * limelight.getEntry("tx").getDouble(0); - if (Math.abs(throttle) > 0.5) { + if (Math.abs(throttle) > 0.25) { int sign = 1; if (throttle < 0) { sign = -1; } - throttle = sign * 0.5; + throttle = sign * 0.25; } System.out.println(throttle); - // climber.driveIndividual(-throttle, throttle); + driveTrain.tankDrive(-throttle, throttle); } } @@ -64,13 +64,16 @@ public boolean isFinished() { @Override protected void end() { System.out.println("end"); - // climber.driveIndividual(0, 0); + driveTrain.tankDrive(0, 0); + vision.setLedMode(1); } // Called when another command which requires one or more of the same // subs*ystems is scheduled to run @Override protected void interrupted() { - System.out.println("interrupt auto"); + System.out.println("interrupt auto limelight target"); + driveTrain.tankDrive(0, 0); + vision.setLedMode(1); } } diff --git a/src/main/java/frc/robot/commands/AutoNavXBalanceClimb.java b/src/main/java/frc/robot/commands/AutoNavXBalanceClimb.java index 5a986c4..fd20c94 100644 --- a/src/main/java/frc/robot/commands/AutoNavXBalanceClimb.java +++ b/src/main/java/frc/robot/commands/AutoNavXBalanceClimb.java @@ -38,11 +38,12 @@ public void balance() { if (Math.abs(roll) < 2) roll = 0; if (roll == 0) { - if (climber.getBackClimbEncoder() > -65 && climber.getFrontClimbEncoder() > -65) { - climber.driveClimbBoth(-0.15); - } else { - climber.driveBackClimber(0); - } + if (climber.getFrontClimbEncoder() > -65) { + climber.driveFrontClimber(-0.35); + } else climber.driveFrontClimber(0); + if (climber.getBackClimbEncoder() > -65) { + climber.driveBackClimber(-0.35); + } else climber.driveBackClimber(0); } ; if (roll > 0 && Math.ceil(climber.getFrontClimbEncoder()) >= -65) { @@ -65,10 +66,10 @@ public void balance() { } } if (rightStick.getRawButton(4)) { - climber.driveBothClimbAxleWheels(-0.50); + climber.driveBothClimbAxleWheels(0.50); } if (rightStick.getRawButton(5)) { - climber.driveBothClimbAxleWheels(0.50); + climber.driveBothClimbAxleWheels(-0.50); } } @@ -81,6 +82,7 @@ public boolean isFinished() { // Called once after isFinished returns true @Override protected void end() { + System.out.println("auto balance is done."); climber.driveClimbBoth(0); climber.driveBothClimbAxleWheels(0); } diff --git a/src/main/java/frc/robot/commands/ClimberDrive.java b/src/main/java/frc/robot/commands/ClimberDrive.java index 2deea1b..0e68e53 100644 --- a/src/main/java/frc/robot/commands/ClimberDrive.java +++ b/src/main/java/frc/robot/commands/ClimberDrive.java @@ -39,11 +39,9 @@ protected void execute() { } if (rightStick.getRawButton(4)) { - climber.driveBothClimbAxleWheels(0.25); - } - - if (rightStick.getRawButton(5)) { - climber.driveBothClimbAxleWheels(-0.25); + climber.driveBothClimbAxleWheels(0.50); + } else { + climber.driveBothClimbAxleWheels(0); } if (rightStick.getRawButtonPressed(7)) { @@ -51,6 +49,14 @@ protected void execute() { climber.resetBothClimbEncoders(); } + if (rightStick.getRawButtonPressed(6)) { + System.out.println("F: " + climber.getFrontSensorInches() + "\nB: " + climber.getBackSensorInches()); + } + + if (auxJoystick.getRawButtonPressed(10)) { + climber.resetArm(); + } + if (Math.abs(auxJoystick.getY()) <= 0.05) climber.driveArm(0); else @@ -58,12 +64,10 @@ protected void execute() { if (Math.abs(rightStick.getY()) <= 0.05) { climber.driveClimbBoth(0); - }else { + } else { climber.driveClimbBoth(rightStick.getY()); } - climber.driveBothClimbAxleWheels(0); - SmartDashboard.putNumber("Position", climber.getPosition()); SmartDashboard.putNumber("Velocity", climber.getVelocity()); } diff --git a/src/main/java/frc/robot/commands/DriveBackClimberX.java b/src/main/java/frc/robot/commands/DriveBackClimberX.java index 9ae9bc4..9be92a7 100644 --- a/src/main/java/frc/robot/commands/DriveBackClimberX.java +++ b/src/main/java/frc/robot/commands/DriveBackClimberX.java @@ -13,13 +13,15 @@ public class DriveBackClimberX extends CommandBase { - Joystick rightStick; + Joystick rightStick, arcadeStick; public DriveBackClimberX() { super("drivebackclimberx"); // Use requires() here to declare subsystem dependencies requires(climber); + requires(driveTrain); rightStick = OI.getRightDriveStick(); + arcadeStick = OI.getLeftDriveStick(); } // Called just before this Command runs the first time @@ -31,9 +33,23 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - if (Math.abs(rightStick.getY()) >= 0.05) { - climber.driveBackClimber(rightStick.getY()); - } else climber.driveBackClimber(0); + if (rightStick.getRawButton(2)) { + if (climber.getBackClimbEncoder() < 0.5) { + climber.driveBackClimber(0.2); + } + } else { + climber.driveBackClimber(0); + } + if (rightStick.getRawButton(4)) { + climber.driveBothClimbAxleWheels(0.50); + } else { + climber.driveBothClimbAxleWheels(0); + } + if (Math.abs(arcadeStick.getY()) < 0.05) { + driveTrain.arcadeDrive(0, 0); + } else { + driveTrain.arcadeDrive(arcadeStick.getX(), arcadeStick.getY()); + } } // Make this return true when this Command no longer needs to run execute() diff --git a/src/main/java/frc/robot/commands/DriveFrontClimberX.java b/src/main/java/frc/robot/commands/DriveFrontClimberX.java index f218ef1..80fde7b 100644 --- a/src/main/java/frc/robot/commands/DriveFrontClimberX.java +++ b/src/main/java/frc/robot/commands/DriveFrontClimberX.java @@ -31,21 +31,21 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - if (Math.abs(rightStick.getY()) >= 0.05) { - climber.driveFrontClimber(rightStick.getY()); - } else climber.driveFrontClimber(0); + if (rightStick.getRawButton(2)) { + if (climber.getFrontClimbEncoder() < 0.5) { + climber.driveFrontClimber(0.2); + } + } else { + climber.driveFrontClimber(0); + } if (climber.getBackClimbEncoder() >= -65) { climber.driveBackClimber(-0.15); } else { climber.driveBackClimber(0); } - if (rightStick.getRawButton(5)) { - climber.driveBothClimbAxleWheels(-0.25); - } if (rightStick.getRawButton(4)) { - climber.driveBothClimbAxleWheels(0.25); - } - if (!rightStick.getRawButton(4) && !rightStick.getRawButton(5)) { + climber.driveBothClimbAxleWheels(0.50); + } else { climber.driveBothClimbAxleWheels(0); } } diff --git a/src/main/java/frc/robot/commands/Hab3AutoClimb/AutoRetractBackAxle.java b/src/main/java/frc/robot/commands/Hab3AutoClimb/AutoRetractBackAxle.java index 62eed67..f2ec750 100644 --- a/src/main/java/frc/robot/commands/Hab3AutoClimb/AutoRetractBackAxle.java +++ b/src/main/java/frc/robot/commands/Hab3AutoClimb/AutoRetractBackAxle.java @@ -35,7 +35,7 @@ protected void execute() { climber.driveBackClimber(0.15); } else { climber.driveBackClimber(0); - } + } if (Math.abs(arcadeStick.getY()) < 0.05) { driveTrain.arcadeDrive(0, 0); } else { @@ -46,7 +46,7 @@ protected void execute() { // Make this return true when this Command no longer needs to run execute() @Override public boolean isFinished() { - return Math.ceil(climber.getBackClimbEncoder()) >= 0; + return Math.ceil(climber.getBackClimbEncoder()) >= -2; } // Called once after isFinished returns true diff --git a/src/main/java/frc/robot/commands/Hab3AutoClimb/AutoRetractFrontAxle.java b/src/main/java/frc/robot/commands/Hab3AutoClimb/AutoRetractFrontAxle.java index 954d253..ae61f16 100644 --- a/src/main/java/frc/robot/commands/Hab3AutoClimb/AutoRetractFrontAxle.java +++ b/src/main/java/frc/robot/commands/Hab3AutoClimb/AutoRetractFrontAxle.java @@ -36,7 +36,7 @@ protected void execute() { // Make this return true when this Command no longer needs to run execute() @Override public boolean isFinished() { - return Math.ceil(climber.getFrontClimbEncoder()) == 0; + return Math.ceil(climber.getFrontClimbEncoder()) == -2; } // Called once after isFinished returns true diff --git a/src/main/java/frc/robot/commands/Hab3AutoClimb/AutoToHab3.java b/src/main/java/frc/robot/commands/Hab3AutoClimb/AutoToHab3.java index ddd437b..158f684 100644 --- a/src/main/java/frc/robot/commands/Hab3AutoClimb/AutoToHab3.java +++ b/src/main/java/frc/robot/commands/Hab3AutoClimb/AutoToHab3.java @@ -8,12 +8,20 @@ package frc.robot.commands.Hab3AutoClimb; import edu.wpi.first.wpilibj.command.CommandGroup; +import frc.robot.commands.AutoNavXBalanceClimb; public class AutoToHab3 extends CommandGroup { /** * Add your docs here. */ public AutoToHab3() { + System.out.println("starting hab3 auto climb"); + addSequential(new AutoNavXBalanceClimb()); + addSequential(new AutoApproachPlatform3Front()); + addSequential(new AutoRetractFrontAxle()); + addSequential(new AutoApproachPlatform3Back()); + addSequential(new AutoRetractBackAxle()); + System.out.println("done"); // Add Commands here: // e.g. addSequential(new Command1()); // addSequential(new Command2()); diff --git a/src/main/java/frc/robot/commands/MoveTo47.java b/src/main/java/frc/robot/commands/MoveTo47.java index 4585209..03c841e 100644 --- a/src/main/java/frc/robot/commands/MoveTo47.java +++ b/src/main/java/frc/robot/commands/MoveTo47.java @@ -7,6 +7,7 @@ package frc.robot.commands; +import edu.wpi.first.wpilibj.Joystick; import frc.robot.RobotMap; public class MoveTo47 extends CommandBase { @@ -22,7 +23,7 @@ public MoveTo47() { @Override protected void initialize() { System.out.println("Move to 47 started"); - climber.setArmPos(-7.6); + climber.setArmPos(-9.2); } // Called repeatedly when this Command is scheduled to run diff --git a/src/main/java/frc/robot/commands/MoveToHighRocket.java b/src/main/java/frc/robot/commands/MoveToHighRocket.java index 6b910fe..9a8cff0 100644 --- a/src/main/java/frc/robot/commands/MoveToHighRocket.java +++ b/src/main/java/frc/robot/commands/MoveToHighRocket.java @@ -7,6 +7,8 @@ package frc.robot.commands; +import edu.wpi.first.wpilibj.Joystick; +import frc.robot.OI; import frc.robot.RobotMap; public class MoveToHighRocket extends CommandBase { @@ -22,7 +24,7 @@ public MoveToHighRocket() { @Override protected void initialize() { System.out.println("Move to high rocket started"); - climber.setArmPos(-15); + climber.setArmPos(-16.8); } // Called repeatedly when this Command is scheduled to run @@ -46,5 +48,6 @@ protected void end() { @Override protected void interrupted() { System.out.println("Move to high rocket interrupted"); + climber.driveArm(0); } } diff --git a/src/main/java/frc/robot/commands/ResetEncoders.java b/src/main/java/frc/robot/commands/ResetEncoders.java new file mode 100644 index 0000000..5faae7a --- /dev/null +++ b/src/main/java/frc/robot/commands/ResetEncoders.java @@ -0,0 +1,45 @@ +/*----------------------------------------------------------------------------*/ +/* 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 ResetEncoders extends CommandBase { + public ResetEncoders() { + super("resetencoders"); + // Use requires() here to declare subsystem dependencies + requires(climber); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + climber.resetBothClimbEncoders(); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + } + + // Make this return true when this Command no longer needs to run execute() + @Override + public boolean isFinished() { + return true; + } + + // Called once after isFinished returns true + @Override + protected void end() { + System.out.println("reset encoders command"); + } + + // 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/RetractFrontAxleNoAutoBalance.java b/src/main/java/frc/robot/commands/RetractFrontAxleNoAutoBalance.java new file mode 100644 index 0000000..17ff3ca --- /dev/null +++ b/src/main/java/frc/robot/commands/RetractFrontAxleNoAutoBalance.java @@ -0,0 +1,63 @@ +/*----------------------------------------------------------------------------*/ +/* 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; + +import edu.wpi.first.wpilibj.Joystick; +import frc.robot.OI; + +public class RetractFrontAxleNoAutoBalance extends CommandBase { + + Joystick rightStick; + + public RetractFrontAxleNoAutoBalance() { + super("retractfrontnoauto"); + // Use requires() here to declare subsystem dependencies + requires(climber); + rightStick = OI.getRightDriveStick(); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + System.out.println("doing a level 2 yeehaw"); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + if (rightStick.getRawButton(2)) { + if (climber.getFrontClimbEncoder() < 0.5) { + climber.driveFrontClimber(0.2); + } + } else { + climber.driveFrontClimber(0); + } + if (rightStick.getRawButton(4)) { + climber.driveBothClimbAxleWheels(0.50); + } else { + climber.driveBothClimbAxleWheels(0); + } + } + + // 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() { + } + + // 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/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 3ce7314..9811ad8 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -61,7 +61,11 @@ public Climber() { armPID.setD(0.20); armPID.setIZone(0); armPID.setFF(0); - armPID.setOutputRange(-0.15, 0.6); + armPID.setOutputRange(-0.25, 0.6); + } + + public void resetArm() { + armEncoder.setPosition(0); } public void resetBothClimbEncoders() {