diff --git a/2018Robot1/build/org/usfirst/frc/team696/robot/Robot.class b/2018Robot1/build/org/usfirst/frc/team696/robot/Robot.class index bc8c49c..d6a71f6 100644 Binary files a/2018Robot1/build/org/usfirst/frc/team696/robot/Robot.class and b/2018Robot1/build/org/usfirst/frc/team696/robot/Robot.class differ diff --git a/2018Robot1/build/org/usfirst/frc/team696/robot/RobotMap.class b/2018Robot1/build/org/usfirst/frc/team696/robot/RobotMap.class index f87edad..44514b7 100644 Binary files a/2018Robot1/build/org/usfirst/frc/team696/robot/RobotMap.class and b/2018Robot1/build/org/usfirst/frc/team696/robot/RobotMap.class differ diff --git a/2018Robot1/build/org/usfirst/frc/team696/robot/RobotTesting.class b/2018Robot1/build/org/usfirst/frc/team696/robot/RobotTesting.class index 98d2793..1e93503 100644 Binary files a/2018Robot1/build/org/usfirst/frc/team696/robot/RobotTesting.class and b/2018Robot1/build/org/usfirst/frc/team696/robot/RobotTesting.class differ diff --git a/2018Robot1/build/org/usfirst/frc/team696/robot/commands/DriveCommand.class b/2018Robot1/build/org/usfirst/frc/team696/robot/commands/DriveCommand.class index 623dbf3..5d81c39 100644 Binary files a/2018Robot1/build/org/usfirst/frc/team696/robot/commands/DriveCommand.class and b/2018Robot1/build/org/usfirst/frc/team696/robot/commands/DriveCommand.class differ diff --git a/2018Robot1/build/org/usfirst/frc/team696/robot/subsystems/ClimberSubsystemPID.class b/2018Robot1/build/org/usfirst/frc/team696/robot/subsystems/ClimberSubsystemPID.class index bad423e..b553371 100644 Binary files a/2018Robot1/build/org/usfirst/frc/team696/robot/subsystems/ClimberSubsystemPID.class and b/2018Robot1/build/org/usfirst/frc/team696/robot/subsystems/ClimberSubsystemPID.class differ diff --git a/2018Robot1/build/org/usfirst/frc/team696/robot/subsystems/GreenLEDClimber.class b/2018Robot1/build/org/usfirst/frc/team696/robot/subsystems/GreenLEDClimber.class index bb3fdbe..a7495c5 100644 Binary files a/2018Robot1/build/org/usfirst/frc/team696/robot/subsystems/GreenLEDClimber.class and b/2018Robot1/build/org/usfirst/frc/team696/robot/subsystems/GreenLEDClimber.class differ diff --git a/2018Robot1/build/org/usfirst/frc/team696/robot/utilities/Constants.class b/2018Robot1/build/org/usfirst/frc/team696/robot/utilities/Constants.class index c7a56e9..d12ab74 100644 Binary files a/2018Robot1/build/org/usfirst/frc/team696/robot/utilities/Constants.class and b/2018Robot1/build/org/usfirst/frc/team696/robot/utilities/Constants.class differ diff --git a/2018Robot1/build/org/usfirst/frc/team696/robot/utilities/RGBSensor.class b/2018Robot1/build/org/usfirst/frc/team696/robot/utilities/RGBSensor.class deleted file mode 100644 index 802fb0f..0000000 Binary files a/2018Robot1/build/org/usfirst/frc/team696/robot/utilities/RGBSensor.class and /dev/null differ diff --git a/2018Robot1/dist/FRCUserProgram.jar b/2018Robot1/dist/FRCUserProgram.jar index b0a905c..7cd1792 100644 Binary files a/2018Robot1/dist/FRCUserProgram.jar and b/2018Robot1/dist/FRCUserProgram.jar differ diff --git a/2018Robot1/src/org/usfirst/frc/team696/robot/Robot.java b/2018Robot1/src/org/usfirst/frc/team696/robot/Robot.java index 7ef9b40..42d4da9 100644 --- a/2018Robot1/src/org/usfirst/frc/team696/robot/Robot.java +++ b/2018Robot1/src/org/usfirst/frc/team696/robot/Robot.java @@ -28,8 +28,9 @@ import org.usfirst.frc.team696.robot.utilities.Util; ======= import org.usfirst.frc.team696.robot.autonomousCommands.CenterPosition; +import org.usfirst.frc.team696.robot.commands.DriveCommand; import org.usfirst.frc.team696.robot.subsystems.*; -import org.usfirst.frc.team696.robot.utilities.RGBSensor; +//import org.usfirst.frc.team696.robot.utilities.RGBSensor; import org.usfirst.frc.team696.robot.utilities.Constants; >>>>>>> 303724418a6d61f39cd163bb002c7a8b6e25305d @@ -47,7 +48,7 @@ public class Robot extends TimedRobot { public static final ClimberSubsystem climberSubsystem = new ClimberSubsystem(RobotMap.leftClimber, RobotMap.rightClimber); public static final ClimberSubsystemPID climberSubsystemPID = new ClimberSubsystemPID(RobotMap.leftClimber, RobotMap.rightClimber, RobotMap.hookDeploy); public static final GreenLEDClimber greenLEDClimber = new GreenLEDClimber(RobotMap.greenLED); - public static final RGBSensor rgbSensorUtility = new RGBSensor(rgbSensor); +// public static final RGBSensor rgbSensorUtility = new RGBSensor(rgbSensor); public static final Constants constants = new Constants(); @@ -80,8 +81,8 @@ public class Robot extends TimedRobot { Drive variables and Objects */ - public static Encoder leftDriveEncoder = new Encoder(1, 1); - public static Encoder rightDriveEncoder = new Encoder(2, 2); +// public static Encoder leftDriveEncoder = new Encoder(1, 1); +// public static Encoder rightDriveEncoder = new Encoder(2, 2); double commandedTurn; double commandedDrive; @@ -110,6 +111,27 @@ public class Robot extends TimedRobot { public double climberSpeed = 0; + int testDriveMotors = 0; + boolean testElevator = false; + boolean testIntakeMotors = false; + + int leftRearCurrent = RobotMap.leftRearCurrent; + int leftMidCurrent = RobotMap.leftMidCurrent; + int leftFrontCurrent = RobotMap.leftFrontCurrent; + int rightRearCurrent = RobotMap.rightRearCurrent; + int rightMidCurrent = RobotMap.rightMidCurrent; + int rightFrontCurrent = RobotMap.rightFrontCurrent; + int leftIntakeCurrent = RobotMap.leftIntakeCurrent; + int rightIntakeCurrent = RobotMap.rightIntakeCurrent; + + int num = 1; + Timer time = new Timer(); + + double minCurrent = 50; + double halfSpeed = 0.5; + double noSpeed = 0; + + >>>>>>> 303724418a6d61f39cd163bb002c7a8b6e25305d @@ -327,7 +349,7 @@ public void teleopPeriodic() { leftDrive = speed + wheel; rightDrive = speed - wheel; - System.out.println(rgbSensorUtility.read16(0x18)); +// System.out.println(rgbSensorUtility.read16(0x18)); >>>>>>> 303724418a6d61f39cd163bb002c7a8b6e25305d @@ -343,5 +365,407 @@ public void teleopPeriodic() { @Override public void testPeriodic() { +<<<<<<< HEAD + /** + * Elevator PIDF Values + */ + + + SmartDashboard.putNumber("Elevator P Value", ElevatorSubsystem.kP); + SmartDashboard.putNumber("Elevator I Value", ElevatorSubsystem.kI); + SmartDashboard.putNumber("Elevator D Value", ElevatorSubsystem.kD); + SmartDashboard.putNumber("Elevator F Value", ElevatorSubsystem.kF); + + /** + * Autonomous Drive PID Values + */ + + SmartDashboard.putNumber("Distance P Value", DriveCommand.kPa); + SmartDashboard.putNumber("Distance I Value", DriveCommand.kIa); + SmartDashboard.putNumber("Distance D Value", DriveCommand.kDa); + SmartDashboard.putNumber("Distance F Value", DriveCommand.kFa); + + SmartDashboard.putNumber("Direction P Value", DriveCommand.kPb); + SmartDashboard.putNumber("Direction I Value", DriveCommand.kIb); + SmartDashboard.putNumber("Direction D Value", DriveCommand.kDb); + SmartDashboard.putNumber("Direction F Value", DriveCommand.kFb); + + SmartDashboard.putNumber("Target Distance (Testing)", DriveCommand.targetDistance); + SmartDashboard.putNumber("Target Direction (Testing)", DriveCommand.tempTargetDirection); + + SmartDashboard.putNumber("Test Drive Motors", testDriveMotors); + + + + + /** + * Testing Talons + */ + + //Drive Talons + + + // TODO Implement Automatic Test Intake Motors function + + if(testDriveMotors == 1){ + switch (num) { + case 1: + driveTrainSubsystem.leftRear.set(halfSpeed); + time.start(); + if (time.get() > 3 && Math.abs(driveTrainSubsystem.leftRear.get()) > 0 && getCurrent(leftRearCurrent) > minCurrent) { + driveTrainSubsystem.leftRear.set(noSpeed); + restartTimer(); + if (time.get() > 1) { + time.stop(); + time.reset(); + driveTrainSubsystem.leftRear.set(-halfSpeed); + time.start(); + } + if (time.get() > 3 && driveTrainSubsystem.leftRear.get() > 0 && getCurrent(leftRearCurrent) > minCurrent) { + driveTrainSubsystem.leftRear.set(noSpeed); + num++; + time.stop(); + time.reset(); +// break; + } else if (time.get() > 3 && !(Math.abs(driveTrainSubsystem.leftRear.get()) > 0) || time.get() > 3 && !(getCurrent(leftRearCurrent) > minCurrent)) { + System.out.println("Left Rear Drive Motor " + driveTrainSubsystem.leftRear.getDeviceID() + " is not functioning properly. "); + driveTrainSubsystem.leftRear.set(noSpeed); + time.stop(); + time.reset(); + testDriveMotors = 0; + resetNum(); + break; + } + } else if (time.get() > 3 && !(Math.abs(driveTrainSubsystem.leftRear.get()) > 0) || time.get() > 3 && !(getCurrent(leftRearCurrent) > minCurrent)) { + System.out.println("Left Rear Drive Motor " + driveTrainSubsystem.leftRear.getDeviceID() + " is not functioning properly."); + driveTrainSubsystem.leftRear.set(noSpeed); + time.stop(); + time.reset(); + testDriveMotors = 0; + resetNum(); + break; + } + + case 2: + driveTrainSubsystem.leftMid.set(halfSpeed); + time.start(); + if (time.get() > 3 && Math.abs(driveTrainSubsystem.leftRear.get()) > 0 && getCurrent(leftMidCurrent) > minCurrent) { + driveTrainSubsystem.leftMid.set(noSpeed); + restartTimer(); + if (time.get() > 1) { + time.stop(); + time.reset(); + driveTrainSubsystem.leftMid.set(-halfSpeed); + time.start(); + } + if (time.get() > 3 && Math.abs(driveTrainSubsystem.leftRear.get()) > 0 && getCurrent(leftMidCurrent) > minCurrent) { + driveTrainSubsystem.leftMid.set(noSpeed); + num++; + time.stop(); + time.reset(); +// break; + } else if (time.get() > 3 && !(Math.abs(driveTrainSubsystem.leftRear.get()) > 0)) { + System.out.println("Left Mid Drive Motor " + driveTrainSubsystem.leftMid.getDeviceID() + " is not functioning properly. "); + driveTrainSubsystem.leftMid.set(0); + time.stop(); + time.reset(); + testDriveMotors = 0; + resetNum(); + break; + } + } else if (time.get() > 3 && !(Math.abs(driveTrainSubsystem.leftRear.get()) > 0)) { + System.out.println("Left Mid Drive Motor " + driveTrainSubsystem.leftMid.getDeviceID() + " is not functioning properly."); + driveTrainSubsystem.leftMid.set(0); + time.stop(); + time.reset(); + testDriveMotors = 0; + resetNum(); + break; + } + + case 3: + driveTrainSubsystem.leftFront.set(0.5); + time.start(); + if (time.get() > 3 && driveTrainSubsystem.leftFront.get() > 0 && getCurrent(leftFrontCurrent) > minCurrent) { + driveTrainSubsystem.leftFront.set(0); + restartTimer(); + if (time.get() > 1) { + time.stop(); + time.reset(); + driveTrainSubsystem.leftFront.set(-0.5); + time.start(); + } + if (time.get() > 3 && driveTrainSubsystem.leftFront.get() > 0 && getCurrent(leftFrontCurrent) > minCurrent) { + driveTrainSubsystem.leftFront.set(0); + num++; + time.stop(); + time.reset(); +// break; + } else if (time.get() > 3 && !(driveTrainSubsystem.leftFront.get() > 0) || time.get() > 3 && !(getCurrent(leftFrontCurrent) > minCurrent)) { + System.out.println("Left Front Drive Motor " + driveTrainSubsystem.leftFront.getDeviceID() + " is not functioning properly. "); + driveTrainSubsystem.leftMid.set(0); + time.stop(); + time.reset(); + testDriveMotors = 0; + resetNum(); + break; + } + } else if (time.get() > 3 && !(driveTrainSubsystem.leftFront.get() > 0) || time.get() > 3 && !(getCurrent(leftFrontCurrent) > minCurrent)) { + System.out.println("Left Front Drive Motor " + driveTrainSubsystem.leftFront.getDeviceID() + " is not functioning properly."); + driveTrainSubsystem.leftFront.set(0); + time.stop(); + time.reset(); + testDriveMotors = 0; + resetNum(); + break; + } + + case 4: + driveTrainSubsystem.rightRear.set(0.5); + time.start(); + if (time.get() > 3 && driveTrainSubsystem.rightRear.get() > 0 && getCurrent(rightRearCurrent) > minCurrent) { + driveTrainSubsystem.rightRear.set(0); + restartTimer(); + if (time.get() > 1) { + time.stop(); + time.reset(); + driveTrainSubsystem.rightRear.set(-0.5); + time.start(); + } + if (time.get() > 3 && driveTrainSubsystem.rightRear.get() > 0 && getCurrent(rightRearCurrent) > minCurrent) { + driveTrainSubsystem.rightRear.set(0); + num++; + time.stop(); + time.reset(); +// break; + } else if (time.get() > 3 && !(driveTrainSubsystem.rightRear.get() > 0) || time.get() > 3 && getCurrent(rightRearCurrent) > minCurrent) { + System.out.println("Right Rear Drive Motor " + driveTrainSubsystem.rightRear.getDeviceID() + " is not functioning properly. "); + driveTrainSubsystem.rightRear.set(0); + time.stop(); + time.reset(); + testDriveMotors = 0; + resetNum(); + break; + } + } else if (time.get() > 3 && !(driveTrainSubsystem.rightRear.get() > 0) || time.get() > 3 && getCurrent(rightRearCurrent) > minCurrent) { + System.out.println("Right Rear Drive Motor " + driveTrainSubsystem.leftMid.getDeviceID() + " is not functioning properly."); + driveTrainSubsystem.rightRear.set(0); + time.stop(); + time.reset(); + testDriveMotors = 0; + resetNum(); + break; + } + + case 5: + driveTrainSubsystem.rightMid.set(0.5); + time.start(); + if (time.get() > 3 && driveTrainSubsystem.rightMid.get() > 0 && getCurrent(rightMidCurrent) > minCurrent) { + driveTrainSubsystem.rightMid.set(0); + restartTimer(); + if (time.get() > 1) { + time.stop(); + time.reset(); + driveTrainSubsystem.rightMid.set(-0.5); + time.start(); + } + if (time.get() > 3 && driveTrainSubsystem.rightMid.get() > 0 && getCurrent(rightMidCurrent) > minCurrent) { + driveTrainSubsystem.rightMid.set(0); + num++; + time.stop(); + time.reset(); +// break; + } else if (time.get() > 3 && !(driveTrainSubsystem.rightMid.get() > 0) || time.get() > 3 && getCurrent(rightMidCurrent) > minCurrent) { + System.out.println("Right Mid Drive Motor " + driveTrainSubsystem.leftMid.getDeviceID() + " is not functioning properly. "); + driveTrainSubsystem.rightMid.set(0); + time.stop(); + time.reset(); + testDriveMotors = 0; + resetNum(); + break; + } + } else if (time.get() > 3 && !(driveTrainSubsystem.rightMid.get() > 0) || time.get() > 3 && getCurrent(rightMidCurrent) > minCurrent) { + System.out.println("Right Mid Drive Motor " + driveTrainSubsystem.rightMid.getDeviceID() + " is not functioning properly."); + driveTrainSubsystem.rightMid.set(0); + time.stop(); + time.reset(); + testDriveMotors = 0; + resetNum(); + break; + } + + case 6: + time.start(); + driveTrainSubsystem.rightFront.set(0.5); + time.start(); + if (time.get() > 3 && driveTrainSubsystem.rightFront.get() > 0 && getCurrent(rightFrontCurrent) > minCurrent) { + driveTrainSubsystem.rightFront.set(0); + restartTimer(); + if (time.get() > 1) { + time.stop(); + time.reset(); + driveTrainSubsystem.rightFront.set(-0.5); + time.start(); + } + if (time.get() > 3 && driveTrainSubsystem.rightFront.get() > 0 && getCurrent(rightFrontCurrent) > minCurrent) { + driveTrainSubsystem.rightMid.set(0); + num++; + time.stop(); + time.reset(); +// break; + } else if (time.get() > 3 && !(driveTrainSubsystem.rightFront.get() > 0) || time.get() > 3 && getCurrent(rightFrontCurrent) > minCurrent) { + System.out.println("Right Front Drive Motor " + driveTrainSubsystem.rightFront.getDeviceID() + " is not functioning properly. "); + driveTrainSubsystem.rightFront.set(0); + time.stop(); + time.reset(); + testDriveMotors = 0; + resetNum(); + break; + } + } else if (time.get() > 3 && !(driveTrainSubsystem.rightFront.get() > 0) || time.get() > 3 && getCurrent(rightFrontCurrent) > minCurrent) { + System.out.println("Right Front Drive Motor " + driveTrainSubsystem.rightFront.getDeviceID() + " is not functioning properly."); + driveTrainSubsystem.rightFront.set(0); + time.stop(); + time.reset(); + testDriveMotors = 0; + resetNum(); + break; + } + + default: + System.out.println("All drive motors functional."); + resetNum(); + time.stop(); + time.reset(); + testDriveMotors = 0; + break; + } + } + + // Elevator Talons + + if(testElevator){ + + switch(num){ + + case 1: + + time.start(); + elevatorSubsystem.moveToPos("switch"); + if(elevatorSubsystem.checkError() < 2 && time.get() > 5){ + time.stop(); + time.reset(); + time.start(); + num++; + break; + }else if(!(elevatorSubsystem.checkError() < 2) && time.get() > 10){ + System.out.println("Move to Switch movement not functioning properly."); + time.stop(); + time.reset(); + num = 1; + testElevator = false; + break; + } + + case 2: + elevatorSubsystem.moveToPos("ground"); + if(elevatorSubsystem.checkError() < 2 && time.get() > 5){ + time.stop(); + time.reset(); + time.start(); + num++; + break; + }else if(!(elevatorSubsystem.checkError() < 2) && time.get() > 5){ + System.out.println("Move to Ground movement not functioning properly."); + time.stop(); + time.reset(); + num = 1; + testElevator = false; + break; + } + + case 3: + elevatorSubsystem.moveToPos("scale"); + if(elevatorSubsystem.checkError() < 2 && time.get() > 5){ + time.stop(); + time.reset(); + time.start(); + break; + }else if(!(elevatorSubsystem.checkError() < 2) && time.get() > 10){ + System.out.println("Move to Scale movement not functioning properly."); + time.stop(); + time.reset(); + num = 1; + testElevator = false; + break; + } + + case 4: + elevatorSubsystem.moveToPos("climb"); + if(elevatorSubsystem.checkError() < 2 && time.get() > 5){ + time.stop(); + time.reset(); + time.start(); + num++; + break; + }else if(!(elevatorSubsystem.checkError() < 2) && time.get() > 10){ + System.out.println("Move to Climb movement not functioning properly"); + time.stop(); + time.reset(); + num = 1; + testElevator = false; + break; + } + + default: + System.out.println("Elevator motion and control good to go!"); + time.stop(); + time.reset(); + num = 1; + testElevator = false; + + } + + } + + // Intake Motors + + if(testIntakeMotors){ + + switch(num){ + + case 1: + time.start(); + intakeSubsystem.runLeftIntake(0.5); + if(time.get() > 3 && getCurrent(leftIntakeCurrent) > 50){ + intakeSubsystem.runLeftIntake(0); + restartTimer(); + if(time.get() > 2){ + intakeSubsystem.runLeftIntake(-0.5); + restartTimer(); + } + }else if(time.get() > 5 && !(getCurrent(leftIntakeCurrent) > 50)){ + } + + } + } + +======= +>>>>>>> Testing_RGBSensor + } + + private double getCurrent(int currentPort){ + return Robot.PDP.getCurrent(currentPort); } + + private void resetNum() { + num = 1; + } + + private void restartTimer() { + time.stop(); + time.reset(); + time.start(); + } + } diff --git a/2018Robot1/src/org/usfirst/frc/team696/robot/RobotMap.java b/2018Robot1/src/org/usfirst/frc/team696/robot/RobotMap.java index 2616383..7232aaf 100644 --- a/2018Robot1/src/org/usfirst/frc/team696/robot/RobotMap.java +++ b/2018Robot1/src/org/usfirst/frc/team696/robot/RobotMap.java @@ -36,40 +36,40 @@ public class RobotMap { /* Drive Base */ - public static int leftFront; - public static int leftMid; - public static int leftRear; - public static int rightFront; - public static int rightMid; - public static int rightRear; + public static int leftFront = 14; + public static int leftMid = 15; + public static int leftRear = 16; + public static int rightFront = 3; + public static int rightMid = 2; + public static int rightRear = 1; - public static int leftRearCurrent; - public static int leftMidCurrent; - public static int leftFrontCurrent; - public static int rightRearCurrent; - public static int rightMidCurrent; - public static int rightFrontCurrent; + public static int leftRearCurrent = 13; + public static int leftMidCurrent = 14; + public static int leftFrontCurrent = 15; + public static int rightRearCurrent = 0; + public static int rightMidCurrent = 1; + public static int rightFrontCurrent = 2; /* Elevator */ - public static int leftIntake = 1; - public static int rightIntake = 2; - public static int leftElevator = 3; - public static int rightElevator = 4; + public static int leftIntake = 4; + public static int rightIntake = 5; + public static int leftElevator = 6; + public static int rightElevator = 8; - public static int leftIntakeCurrent = 1; - public static int rightIntakeCurrent = 2; + public static int leftIntakeCurrent = 3; + public static int rightIntakeCurrent = 4; /* Climber */ - public static int leftClimber = 1; - public static int rightClimber = 3; - public static int hookDeploy = 2; - public static int greenLED = 0; + public static int leftClimber = 9; + public static int rightClimber = 10; + public static int hookDeploy = 1; + public static int greenLED = 12; } diff --git a/2018Robot1/src/org/usfirst/frc/team696/robot/RobotTesting.java b/2018Robot1/src/org/usfirst/frc/team696/robot/RobotTesting.java index 87999fd..7982818 100644 --- a/2018Robot1/src/org/usfirst/frc/team696/robot/RobotTesting.java +++ b/2018Robot1/src/org/usfirst/frc/team696/robot/RobotTesting.java @@ -16,7 +16,7 @@ public class RobotTesting extends TimedRobot { DriveTrainSubsystem driveTrainSubsystem = new DriveTrainSubsystem(RobotMap.leftRear, RobotMap.leftMid, RobotMap.leftFront, - RobotMap.rightRear, RobotMap.rightMid, RobotMap.rightFront); + RobotMap.rightRear, RobotMap.rightMid, RobotMap.rightFront); ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem(RobotMap.leftElevator, RobotMap.rightElevator); IntakeSubsystem intakeSubsystem = new IntakeSubsystem(RobotMap.leftIntake, RobotMap.rightIntake); @@ -45,6 +45,74 @@ public class RobotTesting extends TimedRobot { public void testPeriodic() { +<<<<<<< HEAD +// /** +// * Elevator PIDF Values +// */ +// +// +// SmartDashboard.putNumber("Elevator P Value", ElevatorSubsystem.kP); +// SmartDashboard.putNumber("Elevator I Value", ElevatorSubsystem.kI); +// SmartDashboard.putNumber("Elevator D Value", ElevatorSubsystem.kD); +// SmartDashboard.putNumber("Elevator F Value", ElevatorSubsystem.kF); +// +// /** +// * Autonomous Drive PID Values +// */ +// +// SmartDashboard.putNumber("Distance P Value", DriveCommand.kPa); +// SmartDashboard.putNumber("Distance I Value", DriveCommand.kIa); +// SmartDashboard.putNumber("Distance D Value", DriveCommand.kDa); +// SmartDashboard.putNumber("Distance F Value", DriveCommand.kFa); +// +// SmartDashboard.putNumber("Direction P Value", DriveCommand.kPb); +// SmartDashboard.putNumber("Direction I Value", DriveCommand.kIb); +// SmartDashboard.putNumber("Direction D Value", DriveCommand.kDb); +// SmartDashboard.putNumber("Direction F Value", DriveCommand.kFb); +// +// SmartDashboard.putNumber("Target Distance (Testing)", DriveCommand.targetDistance); +// SmartDashboard.putNumber("Target Direction (Testing)", DriveCommand.tempTargetDirection); +// +// +// +// +// /** +// * Testing Talons +// */ +// +// //Drive Talons +// +// +// // TODO Implement Automatic Test Intake Motors function +// +// if(testDriveMotors){ +// switch (num) { +// case 1: +// driveTrainSubsystem.leftRear.set(halfSpeed); +// time.start(); +// if (time.get() > 3 && Math.abs(driveTrainSubsystem.leftRear.get()) > 0 && getCurrent(leftRearCurrent) > minCurrent) { +// driveTrainSubsystem.leftRear.set(noSpeed); +// restartTimer(); +// if (time.get() > 1) { +// time.stop(); +// time.reset(); +// driveTrainSubsystem.leftRear.set(-halfSpeed); +// time.start(); +// } +// if (time.get() > 3 && driveTrainSubsystem.leftRear.get() > 0 && getCurrent(leftRearCurrent) > minCurrent) { +// driveTrainSubsystem.leftRear.set(noSpeed); +// num++; +// time.stop(); +// time.reset(); +//// break; +// } else if (time.get() > 3 && !(Math.abs(driveTrainSubsystem.leftRear.get()) > 0) || time.get() > 3 && !(getCurrent(leftRearCurrent) > minCurrent)) { +// System.out.println("Left Rear Drive Motor " + driveTrainSubsystem.leftRear.getDeviceID() + " is not functioning properly. "); +// driveTrainSubsystem.leftRear.set(noSpeed); +// time.stop(); +// time.reset(); +// testDriveMotors = false; +// resetNum(); +======= /** * Elevator PIDF Values */ @@ -105,344 +173,339 @@ public void testPeriodic() { num++; time.stop(); time.reset(); +>>>>>>> Testing_RGBSensor // break; - } else if (time.get() > 3 && !(Math.abs(driveTrainSubsystem.leftRear.get()) > 0) || time.get() > 3 && !(getCurrent(leftRearCurrent) > minCurrent)) { - System.out.println("Left Rear Drive Motor " + driveTrainSubsystem.leftRear.getDeviceID() + " is not functioning properly. "); - driveTrainSubsystem.leftRear.set(noSpeed); - time.stop(); - time.reset(); - testDriveMotors = false; - resetNum(); - break; - } - } else if (time.get() > 3 && !(Math.abs(driveTrainSubsystem.leftRear.get()) > 0) || time.get() > 3 && !(getCurrent(leftRearCurrent) > minCurrent)) { - System.out.println("Left Rear Drive Motor " + driveTrainSubsystem.leftRear.getDeviceID() + " is not functioning properly."); - driveTrainSubsystem.leftRear.set(noSpeed); - time.stop(); - time.reset(); - testDriveMotors = false; - resetNum(); - break; - } - - case 2: - driveTrainSubsystem.leftMid.set(halfSpeed); - time.start(); - if (time.get() > 3 && Math.abs(driveTrainSubsystem.leftRear.get()) > 0 && getCurrent(leftMidCurrent) > minCurrent) { - driveTrainSubsystem.leftMid.set(noSpeed); - restartTimer(); - if (time.get() > 1) { - time.stop(); - time.reset(); - driveTrainSubsystem.leftMid.set(-halfSpeed); - time.start(); - } - if (time.get() > 3 && Math.abs(driveTrainSubsystem.leftRear.get()) > 0 && getCurrent(leftMidCurrent) > minCurrent) { - driveTrainSubsystem.leftMid.set(noSpeed); - num++; - time.stop(); - time.reset(); +// } +// } else if (time.get() > 3 && !(Math.abs(driveTrainSubsystem.leftRear.get()) > 0) || time.get() > 3 && !(getCurrent(leftRearCurrent) > minCurrent)) { +// System.out.println("Left Rear Drive Motor " + driveTrainSubsystem.leftRear.getDeviceID() + " is not functioning properly."); +// driveTrainSubsystem.leftRear.set(noSpeed); +// time.stop(); +// time.reset(); +// testDriveMotors = false; +// resetNum(); +// break; +// } +// +// case 2: +// driveTrainSubsystem.leftMid.set(halfSpeed); +// time.start(); +// if (time.get() > 3 && Math.abs(driveTrainSubsystem.leftRear.get()) > 0 && getCurrent(leftMidCurrent) > minCurrent) { +// driveTrainSubsystem.leftMid.set(noSpeed); +// restartTimer(); +// if (time.get() > 1) { +// time.stop(); +// time.reset(); +// driveTrainSubsystem.leftMid.set(-halfSpeed); +// time.start(); +// } +// if (time.get() > 3 && Math.abs(driveTrainSubsystem.leftRear.get()) > 0 && getCurrent(leftMidCurrent) > minCurrent) { +// driveTrainSubsystem.leftMid.set(noSpeed); +// num++; +// time.stop(); +// time.reset(); +//// break; +// } else if (time.get() > 3 && !(Math.abs(driveTrainSubsystem.leftRear.get()) > 0)) { +// System.out.println("Left Mid Drive Motor " + driveTrainSubsystem.leftMid.getDeviceID() + " is not functioning properly. "); +// driveTrainSubsystem.leftMid.set(0); +// time.stop(); +// time.reset(); +// testDriveMotors = false; +// resetNum(); // break; - } else if (time.get() > 3 && !(Math.abs(driveTrainSubsystem.leftRear.get()) > 0)) { - System.out.println("Left Mid Drive Motor " + driveTrainSubsystem.leftMid.getDeviceID() + " is not functioning properly. "); - driveTrainSubsystem.leftMid.set(0); - time.stop(); - time.reset(); - testDriveMotors = false; - resetNum(); - break; - } - } else if (time.get() > 3 && !(Math.abs(driveTrainSubsystem.leftRear.get()) > 0)) { - System.out.println("Left Mid Drive Motor " + driveTrainSubsystem.leftMid.getDeviceID() + " is not functioning properly."); - driveTrainSubsystem.leftMid.set(0); - time.stop(); - time.reset(); - testDriveMotors = false; - resetNum(); - break; - } - - case 3: - driveTrainSubsystem.leftFront.set(0.5); - time.start(); - if (time.get() > 3 && driveTrainSubsystem.leftFront.get() > 0 && getCurrent(leftFrontCurrent) > minCurrent) { - driveTrainSubsystem.leftFront.set(0); - restartTimer(); - if (time.get() > 1) { - time.stop(); - time.reset(); - driveTrainSubsystem.leftFront.set(-0.5); - time.start(); - } - if (time.get() > 3 && driveTrainSubsystem.leftFront.get() > 0 && getCurrent(leftFrontCurrent) > minCurrent) { - driveTrainSubsystem.leftFront.set(0); - num++; - time.stop(); - time.reset(); +// } +// } else if (time.get() > 3 && !(Math.abs(driveTrainSubsystem.leftRear.get()) > 0)) { +// System.out.println("Left Mid Drive Motor " + driveTrainSubsystem.leftMid.getDeviceID() + " is not functioning properly."); +// driveTrainSubsystem.leftMid.set(0); +// time.stop(); +// time.reset(); +// testDriveMotors = false; +// resetNum(); +// break; +// } +// +// case 3: +// driveTrainSubsystem.leftFront.set(0.5); +// time.start(); +// if (time.get() > 3 && driveTrainSubsystem.leftFront.get() > 0 && getCurrent(leftFrontCurrent) > minCurrent) { +// driveTrainSubsystem.leftFront.set(0); +// restartTimer(); +// if (time.get() > 1) { +// time.stop(); +// time.reset(); +// driveTrainSubsystem.leftFront.set(-0.5); +// time.start(); +// } +// if (time.get() > 3 && driveTrainSubsystem.leftFront.get() > 0 && getCurrent(leftFrontCurrent) > minCurrent) { +// driveTrainSubsystem.leftFront.set(0); +// num++; +// time.stop(); +// time.reset(); +//// break; +// } else if (time.get() > 3 && !(driveTrainSubsystem.leftFront.get() > 0) || time.get() > 3 && !(getCurrent(leftFrontCurrent) > minCurrent)) { +// System.out.println("Left Front Drive Motor " + driveTrainSubsystem.leftFront.getDeviceID() + " is not functioning properly. "); +// driveTrainSubsystem.leftMid.set(0); +// time.stop(); +// time.reset(); +// testDriveMotors = false; +// resetNum(); // break; - } else if (time.get() > 3 && !(driveTrainSubsystem.leftFront.get() > 0) || time.get() > 3 && !(getCurrent(leftFrontCurrent) > minCurrent)) { - System.out.println("Left Front Drive Motor " + driveTrainSubsystem.leftFront.getDeviceID() + " is not functioning properly. "); - driveTrainSubsystem.leftMid.set(0); - time.stop(); - time.reset(); - testDriveMotors = false; - resetNum(); - break; - } - } else if (time.get() > 3 && !(driveTrainSubsystem.leftFront.get() > 0) || time.get() > 3 && !(getCurrent(leftFrontCurrent) > minCurrent)) { - System.out.println("Left Front Drive Motor " + driveTrainSubsystem.leftFront.getDeviceID() + " is not functioning properly."); - driveTrainSubsystem.leftFront.set(0); - time.stop(); - time.reset(); - testDriveMotors = false; - resetNum(); - break; - } - - case 4: - driveTrainSubsystem.rightRear.set(0.5); - time.start(); - if (time.get() > 3 && driveTrainSubsystem.rightRear.get() > 0 && getCurrent(rightRearCurrent) > minCurrent) { - driveTrainSubsystem.rightRear.set(0); - restartTimer(); - if (time.get() > 1) { - time.stop(); - time.reset(); - driveTrainSubsystem.rightRear.set(-0.5); - time.start(); - } - if (time.get() > 3 && driveTrainSubsystem.rightRear.get() > 0 && getCurrent(rightRearCurrent) > minCurrent) { - driveTrainSubsystem.rightRear.set(0); - num++; - time.stop(); - time.reset(); +// } +// } else if (time.get() > 3 && !(driveTrainSubsystem.leftFront.get() > 0) || time.get() > 3 && !(getCurrent(leftFrontCurrent) > minCurrent)) { +// System.out.println("Left Front Drive Motor " + driveTrainSubsystem.leftFront.getDeviceID() + " is not functioning properly."); +// driveTrainSubsystem.leftFront.set(0); +// time.stop(); +// time.reset(); +// testDriveMotors = false; +// resetNum(); +// break; +// } +// +// case 4: +// driveTrainSubsystem.rightRear.set(0.5); +// time.start(); +// if (time.get() > 3 && driveTrainSubsystem.rightRear.get() > 0 && getCurrent(rightRearCurrent) > minCurrent) { +// driveTrainSubsystem.rightRear.set(0); +// restartTimer(); +// if (time.get() > 1) { +// time.stop(); +// time.reset(); +// driveTrainSubsystem.rightRear.set(-0.5); +// time.start(); +// } +// if (time.get() > 3 && driveTrainSubsystem.rightRear.get() > 0 && getCurrent(rightRearCurrent) > minCurrent) { +// driveTrainSubsystem.rightRear.set(0); +// num++; +// time.stop(); +// time.reset(); +//// break; +// } else if (time.get() > 3 && !(driveTrainSubsystem.rightRear.get() > 0) || time.get() > 3 && getCurrent(rightRearCurrent) > minCurrent) { +// System.out.println("Right Rear Drive Motor " + driveTrainSubsystem.rightRear.getDeviceID() + " is not functioning properly. "); +// driveTrainSubsystem.rightRear.set(0); +// time.stop(); +// time.reset(); +// testDriveMotors = false; +// resetNum(); // break; - } else if (time.get() > 3 && !(driveTrainSubsystem.rightRear.get() > 0) || time.get() > 3 && getCurrent(rightRearCurrent) > minCurrent) { - System.out.println("Right Rear Drive Motor " + driveTrainSubsystem.rightRear.getDeviceID() + " is not functioning properly. "); - driveTrainSubsystem.rightRear.set(0); - time.stop(); - time.reset(); - testDriveMotors = false; - resetNum(); - break; - } - } else if (time.get() > 3 && !(driveTrainSubsystem.rightRear.get() > 0) || time.get() > 3 && getCurrent(rightRearCurrent) > minCurrent) { - System.out.println("Right Rear Drive Motor " + driveTrainSubsystem.leftMid.getDeviceID() + " is not functioning properly."); - driveTrainSubsystem.rightRear.set(0); - time.stop(); - time.reset(); - testDriveMotors = false; - resetNum(); - break; - } - - case 5: - driveTrainSubsystem.rightMid.set(0.5); - time.start(); - if (time.get() > 3 && driveTrainSubsystem.rightMid.get() > 0 && getCurrent(rightMidCurrent) > minCurrent) { - driveTrainSubsystem.rightMid.set(0); - restartTimer(); - if (time.get() > 1) { - time.stop(); - time.reset(); - driveTrainSubsystem.rightMid.set(-0.5); - time.start(); - } - if (time.get() > 3 && driveTrainSubsystem.rightMid.get() > 0 && getCurrent(rightMidCurrent) > minCurrent) { - driveTrainSubsystem.rightMid.set(0); - num++; - time.stop(); - time.reset(); +// } +// } else if (time.get() > 3 && !(driveTrainSubsystem.rightRear.get() > 0) || time.get() > 3 && getCurrent(rightRearCurrent) > minCurrent) { +// System.out.println("Right Rear Drive Motor " + driveTrainSubsystem.leftMid.getDeviceID() + " is not functioning properly."); +// driveTrainSubsystem.rightRear.set(0); +// time.stop(); +// time.reset(); +// testDriveMotors = false; +// resetNum(); +// break; +// } +// +// case 5: +// driveTrainSubsystem.rightMid.set(0.5); +// time.start(); +// if (time.get() > 3 && driveTrainSubsystem.rightMid.get() > 0 && getCurrent(rightMidCurrent) > minCurrent) { +// driveTrainSubsystem.rightMid.set(0); +// restartTimer(); +// if (time.get() > 1) { +// time.stop(); +// time.reset(); +// driveTrainSubsystem.rightMid.set(-0.5); +// time.start(); +// } +// if (time.get() > 3 && driveTrainSubsystem.rightMid.get() > 0 && getCurrent(rightMidCurrent) > minCurrent) { +// driveTrainSubsystem.rightMid.set(0); +// num++; +// time.stop(); +// time.reset(); +//// break; +// } else if (time.get() > 3 && !(driveTrainSubsystem.rightMid.get() > 0) || time.get() > 3 && getCurrent(rightMidCurrent) > minCurrent) { +// System.out.println("Right Mid Drive Motor " + driveTrainSubsystem.leftMid.getDeviceID() + " is not functioning properly. "); +// driveTrainSubsystem.rightMid.set(0); +// time.stop(); +// time.reset(); +// testDriveMotors = false; +// resetNum(); // break; - } else if (time.get() > 3 && !(driveTrainSubsystem.rightMid.get() > 0) || time.get() > 3 && getCurrent(rightMidCurrent) > minCurrent) { - System.out.println("Right Mid Drive Motor " + driveTrainSubsystem.leftMid.getDeviceID() + " is not functioning properly. "); - driveTrainSubsystem.rightMid.set(0); - time.stop(); - time.reset(); - testDriveMotors = false; - resetNum(); - break; - } - } else if (time.get() > 3 && !(driveTrainSubsystem.rightMid.get() > 0) || time.get() > 3 && getCurrent(rightMidCurrent) > minCurrent) { - System.out.println("Right Mid Drive Motor " + driveTrainSubsystem.rightMid.getDeviceID() + " is not functioning properly."); - driveTrainSubsystem.rightMid.set(0); - time.stop(); - time.reset(); - testDriveMotors = false; - resetNum(); - break; - } - - case 6: - time.start(); - driveTrainSubsystem.rightFront.set(0.5); - time.start(); - if (time.get() > 3 && driveTrainSubsystem.rightFront.get() > 0 && getCurrent(rightFrontCurrent) > minCurrent) { - driveTrainSubsystem.rightFront.set(0); - restartTimer(); - if (time.get() > 1) { - time.stop(); - time.reset(); - driveTrainSubsystem.rightFront.set(-0.5); - time.start(); - } - if (time.get() > 3 && driveTrainSubsystem.rightFront.get() > 0 && getCurrent(rightFrontCurrent) > minCurrent) { - driveTrainSubsystem.rightMid.set(0); - num++; - time.stop(); - time.reset(); +// } +// } else if (time.get() > 3 && !(driveTrainSubsystem.rightMid.get() > 0) || time.get() > 3 && getCurrent(rightMidCurrent) > minCurrent) { +// System.out.println("Right Mid Drive Motor " + driveTrainSubsystem.rightMid.getDeviceID() + " is not functioning properly."); +// driveTrainSubsystem.rightMid.set(0); +// time.stop(); +// time.reset(); +// testDriveMotors = false; +// resetNum(); +// break; +// } +// +// case 6: +// time.start(); +// driveTrainSubsystem.rightFront.set(0.5); +// time.start(); +// if (time.get() > 3 && driveTrainSubsystem.rightFront.get() > 0 && getCurrent(rightFrontCurrent) > minCurrent) { +// driveTrainSubsystem.rightFront.set(0); +// restartTimer(); +// if (time.get() > 1) { +// time.stop(); +// time.reset(); +// driveTrainSubsystem.rightFront.set(-0.5); +// time.start(); +// } +// if (time.get() > 3 && driveTrainSubsystem.rightFront.get() > 0 && getCurrent(rightFrontCurrent) > minCurrent) { +// driveTrainSubsystem.rightMid.set(0); +// num++; +// time.stop(); +// time.reset(); +//// break; +// } else if (time.get() > 3 && !(driveTrainSubsystem.rightFront.get() > 0) || time.get() > 3 && getCurrent(rightFrontCurrent) > minCurrent) { +// System.out.println("Right Front Drive Motor " + driveTrainSubsystem.rightFront.getDeviceID() + " is not functioning properly. "); +// driveTrainSubsystem.rightFront.set(0); +// time.stop(); +// time.reset(); +// testDriveMotors = false; +// resetNum(); // break; - } else if (time.get() > 3 && !(driveTrainSubsystem.rightFront.get() > 0) || time.get() > 3 && getCurrent(rightFrontCurrent) > minCurrent) { - System.out.println("Right Front Drive Motor " + driveTrainSubsystem.rightFront.getDeviceID() + " is not functioning properly. "); - driveTrainSubsystem.rightFront.set(0); - time.stop(); - time.reset(); - testDriveMotors = false; - resetNum(); - break; - } - } else if (time.get() > 3 && !(driveTrainSubsystem.rightFront.get() > 0) || time.get() > 3 && getCurrent(rightFrontCurrent) > minCurrent) { - System.out.println("Right Front Drive Motor " + driveTrainSubsystem.rightFront.getDeviceID() + " is not functioning properly."); - driveTrainSubsystem.rightFront.set(0); - time.stop(); - time.reset(); - testDriveMotors = false; - resetNum(); - break; - } - - default: - System.out.println("All drive motors functional."); - resetNum(); - time.stop(); - time.reset(); - testDriveMotors = false; - break; - } - } - - // Elevator Talons - - if(testElevator){ +// } +// } else if (time.get() > 3 && !(driveTrainSubsystem.rightFront.get() > 0) || time.get() > 3 && getCurrent(rightFrontCurrent) > minCurrent) { +// System.out.println("Right Front Drive Motor " + driveTrainSubsystem.rightFront.getDeviceID() + " is not functioning properly."); +// driveTrainSubsystem.rightFront.set(0); +// time.stop(); +// time.reset(); +// testDriveMotors = false; +// resetNum(); +// break; +// } +// +// default: +// System.out.println("All drive motors functional."); +// resetNum(); +// time.stop(); +// time.reset(); +// testDriveMotors = false; +// break; +// } +// } +// +// // Elevator Talons +// +// if(testElevator){ +// +// switch(num){ +// +// case 1: +// +// time.start(); +// elevatorSubsystem.moveToPos("switch"); +// if(elevatorSubsystem.checkError() < 2 && time.get() > 5){ +// time.stop(); +// time.reset(); +// time.start(); +// num++; +// break; +// }else if(!(elevatorSubsystem.checkError() < 2) && time.get() > 10){ +// System.out.println("Move to Switch movement not functioning properly."); +// time.stop(); +// time.reset(); +// num = 1; +// testElevator = false; +// break; +// } +// +// case 2: +// elevatorSubsystem.moveToPos("ground"); +// if(elevatorSubsystem.checkError() < 2 && time.get() > 5){ +// time.stop(); +// time.reset(); +// time.start(); +// num++; +// break; +// }else if(!(elevatorSubsystem.checkError() < 2) && time.get() > 5){ +// System.out.println("Move to Ground movement not functioning properly."); +// time.stop(); +// time.reset(); +// num = 1; +// testElevator = false; +// break; +// } +// +// case 3: +// elevatorSubsystem.moveToPos("scale"); +// if(elevatorSubsystem.checkError() < 2 && time.get() > 5){ +// time.stop(); +// time.reset(); +// time.start(); +// break; +// }else if(!(elevatorSubsystem.checkError() < 2) && time.get() > 10){ +// System.out.println("Move to Scale movement not functioning properly."); +// time.stop(); +// time.reset(); +// num = 1; +// testElevator = false; +// break; +// } +// +// case 4: +// elevatorSubsystem.moveToPos("climb"); +// if(elevatorSubsystem.checkError() < 2 && time.get() > 5){ +// time.stop(); +// time.reset(); +// time.start(); +// num++; +// break; +// }else if(!(elevatorSubsystem.checkError() < 2) && time.get() > 10){ +// System.out.println("Move to Climb movement not functioning properly"); +// time.stop(); +// time.reset(); +// num = 1; +// testElevator = false; +// break; +// } +// +// default: +// System.out.println("Elevator motion and control good to go!"); +// time.stop(); +// time.reset(); +// num = 1; +// testElevator = false; +// +// } +// +// } +// +// // Intake Motors +// +// if(testIntakeMotors){ +// +// switch(num){ +// +// case 1: +// time.start(); +// intakeSubsystem.runLeftIntake(0.5); +// if(time.get() > 3 && getCurrent(leftIntakeCurrent) > 50){ +// intakeSubsystem.runLeftIntake(0); +// restartTimer(); +// if(time.get() > 2){ +// intakeSubsystem.runLeftIntake(-0.5); +// restartTimer(); +// } +// }else if(time.get() > 5 && !(getCurrent(leftIntakeCurrent) > 50)){ +// } +// +// } +// } +// +// } +// +// private double getCurrent(int currentPort){ +// return Robot.PDP.getCurrent(currentPort); +// } +// +// private void resetNum() { +// num = 1; +// } +// +// private void restartTimer() { +// time.stop(); +// time.reset(); +// time.start(); +// } - switch(num){ - - case 1: - - time.start(); - elevatorSubsystem.moveToPos("switch"); - if(elevatorSubsystem.checkError() < 2 && time.get() > 5){ - time.stop(); - time.reset(); - time.start(); - num++; - break; - }else if(!(elevatorSubsystem.checkError() < 2) && time.get() > 10){ - System.out.println("Move to Switch movement not functioning properly."); - time.stop(); - time.reset(); - num = 1; - testElevator = false; - break; - } - - case 2: - elevatorSubsystem.moveToPos("ground"); - if(elevatorSubsystem.checkError() < 2 && time.get() > 5){ - time.stop(); - time.reset(); - time.start(); - num++; - break; - }else if(!(elevatorSubsystem.checkError() < 2) && time.get() > 5){ - System.out.println("Move to Ground movement not functioning properly."); - time.stop(); - time.reset(); - num = 1; - testElevator = false; - break; - } - - case 3: - elevatorSubsystem.moveToPos("scale"); - if(elevatorSubsystem.checkError() < 2 && time.get() > 5){ - time.stop(); - time.reset(); - time.start(); - break; - }else if(!(elevatorSubsystem.checkError() < 2) && time.get() > 10){ - System.out.println("Move to Scale movement not functioning properly."); - time.stop(); - time.reset(); - num = 1; - testElevator = false; - break; - } - - case 4: - elevatorSubsystem.moveToPos("climb"); - if(elevatorSubsystem.checkError() < 2 && time.get() > 5){ - time.stop(); - time.reset(); - time.start(); - num++; - break; - }else if(!(elevatorSubsystem.checkError() < 2) && time.get() > 10){ - System.out.println("Move to Climb movement not functioning properly"); - time.stop(); - time.reset(); - num = 1; - testElevator = false; - break; - } - - default: - System.out.println("Elevator motion and control good to go!"); - time.stop(); - time.reset(); - num = 1; - testElevator = false; - - } - - } - - // Intake Motors - - if(testIntakeMotors){ - - switch(num){ - - case 1: - time.start(); - intakeSubsystem.runLeftIntake(0.5); - if(time.get() > 3 && getCurrent(leftIntakeCurrent) > 50){ - intakeSubsystem.runLeftIntake(0); - restartTimer(); - if(time.get() > 2){ - intakeSubsystem.runLeftIntake(-0.5); - restartTimer(); - } - }else if(time.get() > 5 && !(getCurrent(leftIntakeCurrent) > 50)){ - } - - } - } } - - private double getCurrent(int currentPort){ - return Robot.PDP.getCurrent(currentPort); - } - - private void resetNum() { - num = 1; - } - - private void restartTimer() { - time.stop(); - time.reset(); - time.start(); - } - -} +} \ No newline at end of file diff --git a/2018Robot1/src/org/usfirst/frc/team696/robot/commands/DriveCommand.java b/2018Robot1/src/org/usfirst/frc/team696/robot/commands/DriveCommand.java index 2070dff..bd8ebf3 100644 --- a/2018Robot1/src/org/usfirst/frc/team696/robot/commands/DriveCommand.java +++ b/2018Robot1/src/org/usfirst/frc/team696/robot/commands/DriveCommand.java @@ -62,8 +62,8 @@ public DriveCommand(double distance, double direction, double maxSpeed, double r */ @Override protected void initialize() { - Robot.leftDriveEncoder.reset(); - Robot.rightDriveEncoder.reset(); +// Robot.leftDriveEncoder.reset(); +// Robot.rightDriveEncoder.reset(); tempTargetDirection = Robot.targetDirection + Robot.navX.getYaw(); diff --git a/2018Robot1/src/org/usfirst/frc/team696/robot/subsystems/GreenLEDClimber.java b/2018Robot1/src/org/usfirst/frc/team696/robot/subsystems/GreenLEDClimber.java index 42de02f..159b24d 100644 --- a/2018Robot1/src/org/usfirst/frc/team696/robot/subsystems/GreenLEDClimber.java +++ b/2018Robot1/src/org/usfirst/frc/team696/robot/subsystems/GreenLEDClimber.java @@ -10,10 +10,10 @@ // Put methods for controlling this subsystem // here. Call these from Commands. - DigitalOutput greenLED; +// DigitalOutput greenLED; public GreenLEDClimber(int greenLED) { - this.greenLED = new DigitalOutput(greenLED); +// this.greenLED = new DigitalOutput(greenLED); } @@ -22,11 +22,14 @@ public void initDefaultCommand() { // setDefaultCommand(new MySpecialCommand()); } public void set(boolean state) { - greenLED.set(state); +// greenLED.set(state); } - public boolean get() { return greenLED.get(); } + public boolean get() { +// return greenLED.get(); + return true; + } } diff --git a/2018Robot1/src/org/usfirst/frc/team696/robot/utilities/Constants.java b/2018Robot1/src/org/usfirst/frc/team696/robot/utilities/Constants.java index 6d9909f..da7c60c 100644 --- a/2018Robot1/src/org/usfirst/frc/team696/robot/utilities/Constants.java +++ b/2018Robot1/src/org/usfirst/frc/team696/robot/utilities/Constants.java @@ -15,7 +15,7 @@ public class Constants { public int buttonY = 4; public int leftXAxis = 0; public int leftYAxis = 1; - public int rightXAxis = 4; + public int rightXAxis = 2; public int rightYAxis = 5; public int leftBumper = 5; public int rightBumper = 6; diff --git a/2018Robot1/src/org/usfirst/frc/team696/robot/utilities/RGBSensor.java b/2018Robot1/src/org/usfirst/frc/team696/robot/utilities/RGBSensor.java index a615f8a..8bea3e8 100644 --- a/2018Robot1/src/org/usfirst/frc/team696/robot/utilities/RGBSensor.java +++ b/2018Robot1/src/org/usfirst/frc/team696/robot/utilities/RGBSensor.java @@ -1,3 +1,39 @@ +<<<<<<< HEAD +//package org.usfirst.frc.team696.robot.utilities; +// +//import edu.wpi.first.wpilibj.I2C; +// +//public class RGBSensor { +// +// static double seconds; +// public static Timer timer = new Timer(seconds); +// +// I2C rgbSensor; +// byte sensorAddress = 0x29; +// byte sensorEnableAddress = 0x00; +// byte sensorEnable_PON = 0x01; +// byte sensorEnable_AEN = 0x02; +// byte sensorCommand_Bit = (byte) 0x80; +// public byte sensorClearL = 0x14; +// public byte sensorRedL = 0x16; +// public byte sensorGreenL = 0x18; +// public byte sensorBlueL = 0x1A; +// byte[] buffer1 = new byte[1]; +// byte[] buffer2 = new byte[2]; +// int[] RGBC = new int[4]; +// +// public RGBSensor(I2C rgbSensor) { +// this.rgbSensor = rgbSensor; +// +// RGBC[0] = read16(sensorClearL); +// RGBC[1] = read16(sensorRedL); +// RGBC[2] = read16(sensorGreenL); +// RGBC[3] = read16(sensorBlueL); +// } +// +// public void enable() { +// rgbSensor.write(sensorEnableAddress, sensorEnable_PON); +======= package org.usfirst.frc.team696.robot.utilities; import edu.wpi.first.wpilibj.I2C; @@ -51,18 +87,38 @@ public int read16(int reg){ int x; // rgbSensor.write(sensorCommand_Bit | reg, 2); +>>>>>>> Testing_RGBSensor // timer.runTimer(3); - rgbSensor.read(sensorCommand_Bit | reg, 2, buffer2); - - t = buffer2[0]; - x = buffer2[1]; - - x <<=8; - x |= t; - - return x; - - - } - -} +// rgbSensor.write(sensorEnableAddress, sensorEnable_PON | sensorEnable_AEN); +// +// } +// +// public byte read8(byte reg) { +//// rgbSensor.write(sensorCommand_Bit, reg); +// timer.runTimer(3); +// rgbSensor.read(sensorCommand_Bit | reg, 1, buffer1); +// +// return reg; +// } +// +// public int read16(int reg){ +// +// int t; +// int x; +// +//// rgbSensor.write(sensorCommand_Bit | reg, 2); +//// timer.runTimer(3); +// rgbSensor.read(sensorCommand_Bit | reg, 2, buffer2); +// +// t = buffer2[0]; +// x = buffer2[1]; +// +// x <<=8; +// x |= t; +// +// return x; +// +// +// } +// +//} diff --git a/2018Robot2/bin/com/kauailabs/nav6/IMUProtocol$GyroUpdate.class b/2018Robot2/bin/com/kauailabs/nav6/IMUProtocol$GyroUpdate.class new file mode 100644 index 0000000..7bf8d8c Binary files /dev/null and b/2018Robot2/bin/com/kauailabs/nav6/IMUProtocol$GyroUpdate.class differ diff --git a/2018Robot2/bin/com/kauailabs/nav6/IMUProtocol$QuaternionUpdate.class b/2018Robot2/bin/com/kauailabs/nav6/IMUProtocol$QuaternionUpdate.class new file mode 100644 index 0000000..b0fa58f Binary files /dev/null and b/2018Robot2/bin/com/kauailabs/nav6/IMUProtocol$QuaternionUpdate.class differ diff --git a/2018Robot2/bin/com/kauailabs/nav6/IMUProtocol$StreamCommand.class b/2018Robot2/bin/com/kauailabs/nav6/IMUProtocol$StreamCommand.class new file mode 100644 index 0000000..1861e7e Binary files /dev/null and b/2018Robot2/bin/com/kauailabs/nav6/IMUProtocol$StreamCommand.class differ diff --git a/2018Robot2/bin/com/kauailabs/nav6/IMUProtocol$StreamResponse.class b/2018Robot2/bin/com/kauailabs/nav6/IMUProtocol$StreamResponse.class new file mode 100644 index 0000000..bce5abd Binary files /dev/null and b/2018Robot2/bin/com/kauailabs/nav6/IMUProtocol$StreamResponse.class differ diff --git a/2018Robot2/bin/com/kauailabs/nav6/IMUProtocol$YPRUpdate.class b/2018Robot2/bin/com/kauailabs/nav6/IMUProtocol$YPRUpdate.class new file mode 100644 index 0000000..6170c26 Binary files /dev/null and b/2018Robot2/bin/com/kauailabs/nav6/IMUProtocol$YPRUpdate.class differ diff --git a/2018Robot2/bin/com/kauailabs/nav6/IMUProtocol.class b/2018Robot2/bin/com/kauailabs/nav6/IMUProtocol.class new file mode 100644 index 0000000..2032ea7 Binary files /dev/null and b/2018Robot2/bin/com/kauailabs/nav6/IMUProtocol.class differ diff --git a/2018Robot2/bin/com/kauailabs/nav6/frc/IMU.class b/2018Robot2/bin/com/kauailabs/nav6/frc/IMU.class new file mode 100644 index 0000000..5183041 Binary files /dev/null and b/2018Robot2/bin/com/kauailabs/nav6/frc/IMU.class differ diff --git a/2018Robot2/bin/com/kauailabs/nav6/frc/IMUAdvanced.class b/2018Robot2/bin/com/kauailabs/nav6/frc/IMUAdvanced.class new file mode 100644 index 0000000..fabdf05 Binary files /dev/null and b/2018Robot2/bin/com/kauailabs/nav6/frc/IMUAdvanced.class differ diff --git a/2018Robot2/bin/com/kauailabs/navx_mxp/AHRSProtocol$AHRSUpdate.class b/2018Robot2/bin/com/kauailabs/navx_mxp/AHRSProtocol$AHRSUpdate.class new file mode 100644 index 0000000..f9b4f30 Binary files /dev/null and b/2018Robot2/bin/com/kauailabs/navx_mxp/AHRSProtocol$AHRSUpdate.class differ diff --git a/2018Robot2/bin/com/kauailabs/navx_mxp/AHRSProtocol$AHRS_DATA_ACTION.class b/2018Robot2/bin/com/kauailabs/navx_mxp/AHRSProtocol$AHRS_DATA_ACTION.class new file mode 100644 index 0000000..42780ab Binary files /dev/null and b/2018Robot2/bin/com/kauailabs/navx_mxp/AHRSProtocol$AHRS_DATA_ACTION.class differ diff --git a/2018Robot2/bin/com/kauailabs/navx_mxp/AHRSProtocol$AHRS_DATA_TYPE.class b/2018Robot2/bin/com/kauailabs/navx_mxp/AHRSProtocol$AHRS_DATA_TYPE.class new file mode 100644 index 0000000..2df2841 Binary files /dev/null and b/2018Robot2/bin/com/kauailabs/navx_mxp/AHRSProtocol$AHRS_DATA_TYPE.class differ diff --git a/2018Robot2/bin/com/kauailabs/navx_mxp/AHRSProtocol$AHRS_TUNING_VAR_ID.class b/2018Robot2/bin/com/kauailabs/navx_mxp/AHRSProtocol$AHRS_TUNING_VAR_ID.class new file mode 100644 index 0000000..a872a9e Binary files /dev/null and b/2018Robot2/bin/com/kauailabs/navx_mxp/AHRSProtocol$AHRS_TUNING_VAR_ID.class differ diff --git a/2018Robot2/bin/com/kauailabs/navx_mxp/AHRSProtocol$BoardID.class b/2018Robot2/bin/com/kauailabs/navx_mxp/AHRSProtocol$BoardID.class new file mode 100644 index 0000000..b787466 Binary files /dev/null and b/2018Robot2/bin/com/kauailabs/navx_mxp/AHRSProtocol$BoardID.class differ diff --git a/2018Robot2/bin/com/kauailabs/navx_mxp/AHRSProtocol$DataSetResponse.class b/2018Robot2/bin/com/kauailabs/navx_mxp/AHRSProtocol$DataSetResponse.class new file mode 100644 index 0000000..2db84aa Binary files /dev/null and b/2018Robot2/bin/com/kauailabs/navx_mxp/AHRSProtocol$DataSetResponse.class differ diff --git a/2018Robot2/bin/com/kauailabs/navx_mxp/AHRSProtocol$MagCalData.class b/2018Robot2/bin/com/kauailabs/navx_mxp/AHRSProtocol$MagCalData.class new file mode 100644 index 0000000..147448a Binary files /dev/null and b/2018Robot2/bin/com/kauailabs/navx_mxp/AHRSProtocol$MagCalData.class differ diff --git a/2018Robot2/bin/com/kauailabs/navx_mxp/AHRSProtocol$TuningVar.class b/2018Robot2/bin/com/kauailabs/navx_mxp/AHRSProtocol$TuningVar.class new file mode 100644 index 0000000..6d2999a Binary files /dev/null and b/2018Robot2/bin/com/kauailabs/navx_mxp/AHRSProtocol$TuningVar.class differ diff --git a/2018Robot2/bin/com/kauailabs/navx_mxp/AHRSProtocol.class b/2018Robot2/bin/com/kauailabs/navx_mxp/AHRSProtocol.class new file mode 100644 index 0000000..8270448 Binary files /dev/null and b/2018Robot2/bin/com/kauailabs/navx_mxp/AHRSProtocol.class differ diff --git a/2018Robot2/bin/org/usfirst/frc/team696/robot/OI.class b/2018Robot2/bin/org/usfirst/frc/team696/robot/OI.class new file mode 100644 index 0000000..14ae18c Binary files /dev/null and b/2018Robot2/bin/org/usfirst/frc/team696/robot/OI.class differ diff --git a/2018Robot2/bin/org/usfirst/frc/team696/robot/Robot.class b/2018Robot2/bin/org/usfirst/frc/team696/robot/Robot.class new file mode 100644 index 0000000..6ed7fb9 Binary files /dev/null and b/2018Robot2/bin/org/usfirst/frc/team696/robot/Robot.class differ diff --git a/2018Robot2/bin/org/usfirst/frc/team696/robot/RobotMap.class b/2018Robot2/bin/org/usfirst/frc/team696/robot/RobotMap.class new file mode 100644 index 0000000..f87edad Binary files /dev/null and b/2018Robot2/bin/org/usfirst/frc/team696/robot/RobotMap.class differ diff --git a/2018Robot2/bin/org/usfirst/frc/team696/robot/commands/ExampleCommand.class b/2018Robot2/bin/org/usfirst/frc/team696/robot/commands/ExampleCommand.class new file mode 100644 index 0000000..96c4d68 Binary files /dev/null and b/2018Robot2/bin/org/usfirst/frc/team696/robot/commands/ExampleCommand.class differ diff --git a/2018Robot2/bin/org/usfirst/frc/team696/robot/subsystems/DriveTrainSubsystem.class b/2018Robot2/bin/org/usfirst/frc/team696/robot/subsystems/DriveTrainSubsystem.class new file mode 100644 index 0000000..93d14ff Binary files /dev/null and b/2018Robot2/bin/org/usfirst/frc/team696/robot/subsystems/DriveTrainSubsystem.class differ diff --git a/2018Robot2/bin/org/usfirst/frc/team696/robot/subsystems/ExampleSubsystem.class b/2018Robot2/bin/org/usfirst/frc/team696/robot/subsystems/ExampleSubsystem.class new file mode 100644 index 0000000..7bb7c0e Binary files /dev/null and b/2018Robot2/bin/org/usfirst/frc/team696/robot/subsystems/ExampleSubsystem.class differ diff --git a/ScorpiusCustom/.idea/misc.xml b/ScorpiusCustom/.idea/misc.xml index e208459..0d907f3 100644 --- a/ScorpiusCustom/.idea/misc.xml +++ b/ScorpiusCustom/.idea/misc.xml @@ -1,6 +1,10 @@ +<<<<<<< HEAD + +======= +>>>>>>> Testing_RGBSensor \ No newline at end of file diff --git a/ScorpiusCustom/.idea/workspace.xml b/ScorpiusCustom/.idea/workspace.xml index 28ba4a7..bb32b24 100644 --- a/ScorpiusCustom/.idea/workspace.xml +++ b/ScorpiusCustom/.idea/workspace.xml @@ -1,5 +1,11 @@ +<<<<<<< HEAD + + + + +======= @@ -19,6 +25,7 @@ >>>>>>> 447668491ee7c09143ce94bea6bd2dc9c6c416d3 +>>>>>>> Testing_RGBSensor +<<<<<<< HEAD + +======= @@ -191,6 +201,7 @@ +>>>>>>> Testing_RGBSensor +<<<<<<< HEAD +======= <<<<<<< HEAD +>>>>>>> Testing_RGBSensor @@ -263,9 +281,12 @@ +<<<<<<< HEAD +======= +>>>>>>> Testing_RGBSensor @@ -285,6 +306,8 @@ +<<<<<<< HEAD +======= @@ -316,10 +339,16 @@ +>>>>>>> Testing_RGBSensor