diff --git a/PowerUp/README.md b/PowerUp/README.md new file mode 100644 index 0000000..3736aad --- /dev/null +++ b/PowerUp/README.md @@ -0,0 +1 @@ +# PowerUp \ No newline at end of file diff --git a/PowerUp/build/AutoCommands/ArmGoUp.class b/PowerUp/build/AutoCommands/ArmGoUp.class index 42d59ea..0f17889 100644 Binary files a/PowerUp/build/AutoCommands/ArmGoUp.class and b/PowerUp/build/AutoCommands/ArmGoUp.class differ diff --git a/PowerUp/build/AutoCommands/DT_DriveDistance.class b/PowerUp/build/AutoCommands/DT_DriveDistance.class new file mode 100644 index 0000000..579c698 Binary files /dev/null and b/PowerUp/build/AutoCommands/DT_DriveDistance.class differ diff --git a/PowerUp/build/AutoCommands/DT_TurnToAngle.class b/PowerUp/build/AutoCommands/DT_TurnToAngle.class new file mode 100644 index 0000000..cf9621d Binary files /dev/null and b/PowerUp/build/AutoCommands/DT_TurnToAngle.class differ diff --git a/PowerUp/build/AutoCommands/DriveDistance.class b/PowerUp/build/AutoCommands/DriveDistance.class index d7c9e9b..0079344 100644 Binary files a/PowerUp/build/AutoCommands/DriveDistance.class and b/PowerUp/build/AutoCommands/DriveDistance.class differ diff --git a/PowerUp/build/AutoCommands/DropCube.class b/PowerUp/build/AutoCommands/DropCube.class index 8d2ec2b..9f8780a 100644 Binary files a/PowerUp/build/AutoCommands/DropCube.class and b/PowerUp/build/AutoCommands/DropCube.class differ diff --git a/PowerUp/build/AutoCommands/TurnToAngle.class b/PowerUp/build/AutoCommands/TurnToAngle.class index 82113ea..1cbb865 100644 Binary files a/PowerUp/build/AutoCommands/TurnToAngle.class and b/PowerUp/build/AutoCommands/TurnToAngle.class differ diff --git a/PowerUp/build/AutoCommands/armGoDown.class b/PowerUp/build/AutoCommands/armGoDown.class new file mode 100644 index 0000000..4dc6388 Binary files /dev/null and b/PowerUp/build/AutoCommands/armGoDown.class differ diff --git a/PowerUp/build/AutoModes/AutoLine.class b/PowerUp/build/AutoModes/AutoLine.class new file mode 100644 index 0000000..fb348ba Binary files /dev/null and b/PowerUp/build/AutoModes/AutoLine.class differ diff --git a/PowerUp/build/AutoModes/AutoMode.class b/PowerUp/build/AutoModes/AutoMode.class index e4b7976..9619adb 100644 Binary files a/PowerUp/build/AutoModes/AutoMode.class and b/PowerUp/build/AutoModes/AutoMode.class differ diff --git a/PowerUp/build/AutoModes/DriveStraight.class b/PowerUp/build/AutoModes/DriveStraight.class index 6f97b40..8705847 100644 Binary files a/PowerUp/build/AutoModes/DriveStraight.class and b/PowerUp/build/AutoModes/DriveStraight.class differ diff --git a/PowerUp/build/AutoModes/LeftCross.class b/PowerUp/build/AutoModes/LeftCross.class new file mode 100644 index 0000000..ea3164c Binary files /dev/null and b/PowerUp/build/AutoModes/LeftCross.class differ diff --git a/PowerUp/build/AutoModes/LeftScaleSwitchOutside.class b/PowerUp/build/AutoModes/LeftScaleSwitchOutside.class new file mode 100644 index 0000000..038a330 Binary files /dev/null and b/PowerUp/build/AutoModes/LeftScaleSwitchOutside.class differ diff --git a/PowerUp/build/AutoModes/RightCross.class b/PowerUp/build/AutoModes/RightCross.class new file mode 100644 index 0000000..69c44ea Binary files /dev/null and b/PowerUp/build/AutoModes/RightCross.class differ diff --git a/PowerUp/build/AutoModes/RightScaleSwitchOutside.class b/PowerUp/build/AutoModes/RightScaleSwitchOutside.class new file mode 100644 index 0000000..cb9d78b Binary files /dev/null and b/PowerUp/build/AutoModes/RightScaleSwitchOutside.class differ diff --git a/PowerUp/build/AutoModes/RightSwitchRightScale.class b/PowerUp/build/AutoModes/RightSwitchRightScale.class new file mode 100644 index 0000000..174645d Binary files /dev/null and b/PowerUp/build/AutoModes/RightSwitchRightScale.class differ diff --git a/PowerUp/build/AutoModes/SwitchAuto.class b/PowerUp/build/AutoModes/SwitchAuto.class index 7cd5899..e60a202 100644 Binary files a/PowerUp/build/AutoModes/SwitchAuto.class and b/PowerUp/build/AutoModes/SwitchAuto.class differ diff --git a/PowerUp/build/AutoModes/SwitchAuto1.class b/PowerUp/build/AutoModes/SwitchAuto1.class new file mode 100644 index 0000000..870888b Binary files /dev/null and b/PowerUp/build/AutoModes/SwitchAuto1.class differ diff --git a/PowerUp/build/AutoModes/SwitchAuto2.class b/PowerUp/build/AutoModes/SwitchAuto2.class index 335c9b0..a285d96 100644 Binary files a/PowerUp/build/AutoModes/SwitchAuto2.class and b/PowerUp/build/AutoModes/SwitchAuto2.class differ diff --git a/PowerUp/build/AutoModes/SwitchPriorityLeft.class b/PowerUp/build/AutoModes/SwitchPriorityLeft.class new file mode 100644 index 0000000..10da96a Binary files /dev/null and b/PowerUp/build/AutoModes/SwitchPriorityLeft.class differ diff --git a/PowerUp/build/AutoModes/SwitchPriorityRight.class b/PowerUp/build/AutoModes/SwitchPriorityRight.class new file mode 100644 index 0000000..a18237a Binary files /dev/null and b/PowerUp/build/AutoModes/SwitchPriorityRight.class differ diff --git a/PowerUp/build/org/usfirst/frc/team548/robot/Climber.class b/PowerUp/build/org/usfirst/frc/team548/robot/Climber.class index bcd4b25..6029ae8 100644 Binary files a/PowerUp/build/org/usfirst/frc/team548/robot/Climber.class and b/PowerUp/build/org/usfirst/frc/team548/robot/Climber.class differ diff --git a/PowerUp/build/org/usfirst/frc/team548/robot/Constants.class b/PowerUp/build/org/usfirst/frc/team548/robot/Constants.class index b9680bb..3b907d6 100644 Binary files a/PowerUp/build/org/usfirst/frc/team548/robot/Constants.class and b/PowerUp/build/org/usfirst/frc/team548/robot/Constants.class differ diff --git a/PowerUp/build/org/usfirst/frc/team548/robot/DriveTrain.class b/PowerUp/build/org/usfirst/frc/team548/robot/DriveTrain.class index 426e550..bfec03e 100644 Binary files a/PowerUp/build/org/usfirst/frc/team548/robot/DriveTrain.class and b/PowerUp/build/org/usfirst/frc/team548/robot/DriveTrain.class differ diff --git a/PowerUp/build/org/usfirst/frc/team548/robot/Elevator.class b/PowerUp/build/org/usfirst/frc/team548/robot/Elevator.class index 401935c..32abab0 100644 Binary files a/PowerUp/build/org/usfirst/frc/team548/robot/Elevator.class and b/PowerUp/build/org/usfirst/frc/team548/robot/Elevator.class differ diff --git a/PowerUp/build/org/usfirst/frc/team548/robot/Ingestor.class b/PowerUp/build/org/usfirst/frc/team548/robot/Ingestor.class index ba7ee6e..d194ef4 100644 Binary files a/PowerUp/build/org/usfirst/frc/team548/robot/Ingestor.class and b/PowerUp/build/org/usfirst/frc/team548/robot/Ingestor.class differ diff --git a/PowerUp/build/org/usfirst/frc/team548/robot/Robot.class b/PowerUp/build/org/usfirst/frc/team548/robot/Robot.class index 94afffc..7499aeb 100644 Binary files a/PowerUp/build/org/usfirst/frc/team548/robot/Robot.class and b/PowerUp/build/org/usfirst/frc/team548/robot/Robot.class differ diff --git a/PowerUp/build/org/usfirst/frc/team548/robot/TeleOp.class b/PowerUp/build/org/usfirst/frc/team548/robot/TeleOp.class index 196ab55..4020916 100644 Binary files a/PowerUp/build/org/usfirst/frc/team548/robot/TeleOp.class and b/PowerUp/build/org/usfirst/frc/team548/robot/TeleOp.class differ diff --git a/PowerUp/build/org/usfirst/frc/team548/robot/USBLED$1.class b/PowerUp/build/org/usfirst/frc/team548/robot/USBLED$1.class new file mode 100644 index 0000000..b057ad1 Binary files /dev/null and b/PowerUp/build/org/usfirst/frc/team548/robot/USBLED$1.class differ diff --git a/PowerUp/build/org/usfirst/frc/team548/robot/USBLED.class b/PowerUp/build/org/usfirst/frc/team548/robot/USBLED.class new file mode 100644 index 0000000..216042a Binary files /dev/null and b/PowerUp/build/org/usfirst/frc/team548/robot/USBLED.class differ diff --git a/PowerUp/build/org/usfirst/frc/team548/robot/XBoxController.class b/PowerUp/build/org/usfirst/frc/team548/robot/XBoxController.class index 5e4e5f8..8c2ec19 100644 Binary files a/PowerUp/build/org/usfirst/frc/team548/robot/XBoxController.class and b/PowerUp/build/org/usfirst/frc/team548/robot/XBoxController.class differ diff --git a/PowerUp/dist/FRCUserProgram.jar b/PowerUp/dist/FRCUserProgram.jar index 2fc50c4..9e59a65 100644 Binary files a/PowerUp/dist/FRCUserProgram.jar and b/PowerUp/dist/FRCUserProgram.jar differ diff --git a/PowerUp/src/AutoCommands/ArmGoUp.java b/PowerUp/src/AutoCommands/ArmGoUp.java index 30633b0..80cc615 100644 --- a/PowerUp/src/AutoCommands/ArmGoUp.java +++ b/PowerUp/src/AutoCommands/ArmGoUp.java @@ -16,7 +16,6 @@ public void init() { // TODO Auto-generated method stub Elevator.resetEncoder(); - } @@ -41,4 +40,4 @@ protected String getCommandName() { return null; } -} +} \ No newline at end of file diff --git a/PowerUp/src/AutoCommands/DT_DriveDistance.java b/PowerUp/src/AutoCommands/DT_DriveDistance.java new file mode 100644 index 0000000..7739b18 --- /dev/null +++ b/PowerUp/src/AutoCommands/DT_DriveDistance.java @@ -0,0 +1,64 @@ +package AutoCommands; + +import org.usfirst.frc.team548.robot.DriveTrain; +import org.usfirst.frc.team548.robot.Elevator; +import org.usfirst.frc.team548.robot.Ingestor; + +public class DT_DriveDistance extends AutoCommandBase{ + private double power; + private double elevatorSetPoint; + private double ingestorPower; + private double distance; + private final double kP = 0.03; + + public DT_DriveDistance(double timeOut, double power, double distance, double elevatorSetPoint, double ingestorPower) { + super(timeOut); + + this.power = power; + this.elevatorSetPoint = elevatorSetPoint; + this.ingestorPower = ingestorPower; + // TODO Auto-generated constructor stub + } + + @Override + public void init() { + // TODO Auto-generated method stub + DriveTrain.resetEncoder(); + DriveTrain.resetGyro(); + } + + @Override + protected void run() { + // TODO Auto-generated method stub + if(Math.abs(Math.abs(DriveTrain.getEncoderAverage())) < Math.abs(distance)){ + double angle = DriveTrain.getAngle(); + DriveTrain.pidDriveStraight(0.5); + }else{ + DriveTrain.stop(); + Elevator.setPosition(elevatorSetPoint); + Ingestor.bothControl(ingestorPower); + } + + } + + @Override + public void end() { + // TODO Auto-generated method stub + DriveTrain.stop(); + DriveTrain.resetGyro(); + DriveTrain.resetEncoder(); + DriveTrain.disablePID(); + + Elevator.setPosition(elevatorSetPoint); + Ingestor.bothControl(ingestorPower); + + } + + @Override + protected String getCommandName() { + // TODO Auto-generated method stub + return "Better Drive Distance"; + } + + +} diff --git a/PowerUp/src/AutoCommands/DT_TurnToAngle.java b/PowerUp/src/AutoCommands/DT_TurnToAngle.java new file mode 100644 index 0000000..ee9242b --- /dev/null +++ b/PowerUp/src/AutoCommands/DT_TurnToAngle.java @@ -0,0 +1,50 @@ +package AutoCommands; + +import org.usfirst.frc.team548.robot.DriveTrain; +import org.usfirst.frc.team548.robot.Elevator; +import org.usfirst.frc.team548.robot.Ingestor; + +public class DT_TurnToAngle extends AutoCommandBase{ + + private double angle = 0; + private double power; + private double elevatorSetPoint; + private double ingestorPower; + + public DT_TurnToAngle(double timeOut, double angle, double power, double elevatorSetPoint, double ingestorPower){ + super(timeOut); + this.angle = angle; + this.power = power; + this.elevatorSetPoint = elevatorSetPoint; + this.ingestorPower = ingestorPower; + } + + @Override + public void init() { + // TODO Auto-generated method stub + DriveTrain.resetGyro(); + } + + @Override + protected void run() { + // TODO Auto-generated method stub + DriveTrain.turnToAngle(angle); + Ingestor.bothControl(ingestorPower); + } + + @Override + public void end() { + // TODO Auto-generated method stub + DriveTrain.disablePID(); + System.out.println(DriveTrain.getAngle()); + DriveTrain.resetGyro(); + Elevator.setPosition(elevatorSetPoint); + Ingestor.bothControl(ingestorPower); + } + + @Override + protected String getCommandName() { + // TODO Auto-generated method stub + return "Accurate Turn To Angle"; + } +} diff --git a/PowerUp/src/AutoCommands/DriveDistance.java b/PowerUp/src/AutoCommands/DriveDistance.java index 615eff0..24d53e4 100644 --- a/PowerUp/src/AutoCommands/DriveDistance.java +++ b/PowerUp/src/AutoCommands/DriveDistance.java @@ -17,8 +17,7 @@ public DriveDistance(double timeOut, double power, double distance, double eleva public void init(){ DriveTrain.resetEncoder(); - //DriveTrain.resetGyro(); - //Elevator.resetEncoder(); + DriveTrain.resetGyro(); } @Override @@ -41,8 +40,8 @@ protected void run() { public void end() { // TODO Auto-generated method stub DriveTrain.stop(); - //DriveTrain.resetGyro(); - //DriveTrain.resetEncoder(); + DriveTrain.resetGyro(); + DriveTrain.resetEncoder(); Elevator.setPosition(elevatorSetPoint); Ingestor.bothControl(ingestorPower); } @@ -53,4 +52,4 @@ protected String getCommandName() { return "Drive Distance"; } -} +} \ No newline at end of file diff --git a/PowerUp/src/AutoCommands/DropCube.java b/PowerUp/src/AutoCommands/DropCube.java index ef1d3a0..8faa5e9 100644 --- a/PowerUp/src/AutoCommands/DropCube.java +++ b/PowerUp/src/AutoCommands/DropCube.java @@ -2,6 +2,7 @@ import org.usfirst.frc.team548.robot.Elevator; import org.usfirst.frc.team548.robot.Ingestor; +import org.usfirst.frc.team548.robot.TeleOp; public class DropCube extends AutoCommandBase { private double powerLeft, powerRight, elevatorSetPoint; @@ -33,6 +34,8 @@ protected void run() { public void end() { // TODO Auto-generated method stub Ingestor.stop(); + + //Elevator.setElevatorOut(); } @@ -43,4 +46,4 @@ protected String getCommandName() { return "Drop Cube"; } -} +} \ No newline at end of file diff --git a/PowerUp/src/AutoCommands/ElevatorSetPoint.java b/PowerUp/src/AutoCommands/ElevatorSetPoint.java index 863d695..cbcd841 100644 --- a/PowerUp/src/AutoCommands/ElevatorSetPoint.java +++ b/PowerUp/src/AutoCommands/ElevatorSetPoint.java @@ -38,4 +38,4 @@ protected String getCommandName() { return null; } -} +} \ No newline at end of file diff --git a/PowerUp/src/AutoCommands/TurnToAngle.java b/PowerUp/src/AutoCommands/TurnToAngle.java index 8cdb842..8812555 100644 --- a/PowerUp/src/AutoCommands/TurnToAngle.java +++ b/PowerUp/src/AutoCommands/TurnToAngle.java @@ -38,7 +38,7 @@ protected void run() { DriveTrain.arcadeDrive(0, power); else if(DriveTrain.getAngle() < angle && DriveTrain.getAngle() > angle - angleOffSet) DriveTrain.arcadeDrive(0, .15); - Elevator.setPosition(elevatorSetPoint); + // Elevator.setPosition(elevatorSetPoint); } else{ if(DriveTrain.getAngle() > (angle + angleOffSet)) diff --git a/PowerUp/src/AutoCommands/armGoDown.java b/PowerUp/src/AutoCommands/armGoDown.java new file mode 100644 index 0000000..0840213 --- /dev/null +++ b/PowerUp/src/AutoCommands/armGoDown.java @@ -0,0 +1,43 @@ +package AutoCommands; + +import org.usfirst.frc.team548.robot.Elevator; + +public class armGoDown extends AutoCommandBase { + public static boolean position; + + public armGoDown(double timeOut, boolean pos) { + super(timeOut); + this.position = pos; + // TODO Auto-genearmGoDownrated constructor stub + } + + @Override + public void init() { + // TODO Auto-generated method stub + + Elevator.resetEncoder(); + + } + + @Override + protected void run() { + // TODO Auto-generated method stub + Elevator.setElevatorIn(); + ////Elevator.setPosition(0); + } + + + @Override + public void end() { + // TODO Auto-generated method stub + Elevator.setElevatorIn(); + Elevator.setPosition(0); + } + + @Override + protected String getCommandName() { + // TODO Auto-generated method stub + return null; + } + +} \ No newline at end of file diff --git a/PowerUp/src/AutoModes/AutoLine.java b/PowerUp/src/AutoModes/AutoLine.java new file mode 100644 index 0000000..5040573 --- /dev/null +++ b/PowerUp/src/AutoModes/AutoLine.java @@ -0,0 +1,17 @@ +package AutoModes; + +import org.usfirst.frc.team548.robot.Ingestor; + +public class AutoLine extends AutoMode { + + @Override + protected void run() { + // TODO Auto-generated method stub + driveDistance(1, -.3, (260 * 8), 10, 0, .3); + driveDistance(8, -.7, (32500), 10, 0, .3); + turnToAngle(2, 45, 8, .5, 0); + Ingestor.closeIngestor(); + armGoUp(1, true); + } + +} diff --git a/PowerUp/src/AutoModes/AutoMode.java b/PowerUp/src/AutoModes/AutoMode.java index 0250d9c..c44d45a 100644 --- a/PowerUp/src/AutoModes/AutoMode.java +++ b/PowerUp/src/AutoModes/AutoMode.java @@ -2,17 +2,17 @@ import AutoCommands.ArmGoUp; import AutoCommands.AutoCommandBase; +//import AutoCommands.DT_DriveDistance; +import AutoCommands.DT_TurnToAngle; import AutoCommands.DriveDistance; import AutoCommands.DropCube; import AutoCommands.ElevatorSetPoint; -import AutoCommands.OneSideTurn; +//import AutoCommands.OneSideTurn; import AutoCommands.TurnToAngle; -import AutoCommands.Wait; +//import AutoCommands.Wait; public abstract class AutoMode { - private String autoName; - public void start(){ run(); } @@ -31,14 +31,13 @@ protected void turnToAngle(double seconds, double angle, double offset, double p runCommand(new TurnToAngle(seconds, angle, offset, power, setPoint)); } - protected void doubleSidePower(double seconds, double leftPower, double rightPower){ - runCommand(new OneSideTurn(seconds, leftPower, rightPower)); + protected void dtTurnToAngle(double seconds, double angle, double power, double elevatorSetPoint, double ingestorPower){ + runCommand(new DT_TurnToAngle(seconds, angle, power, elevatorSetPoint, ingestorPower)); } - protected void waitTime(double seconds){ - runCommand(new Wait(seconds)); + protected void dtDriveDistance(double seconds, double power, double distance, double elevatorSetPoint, double ingestorSetPoint){ + runCommand(new DriveDistance(seconds, power, distance, elevatorSetPoint, ingestorSetPoint)); } - protected void armGoUp(double seconds, boolean pos){ runCommand(new ArmGoUp(seconds, pos)); } @@ -47,11 +46,16 @@ protected void elevatotSet(double seconds, double angle){ runCommand(new ElevatorSetPoint(seconds, angle)); } + protected void armGoDown(double seconds, boolean pos){ + runCommand(new AutoCommands.armGoDown(seconds, pos)); + } + + protected void DT_DriveDistance(double timeOut, double power, double distance, double elevatorSetPoint, double ingestorPower){ + runCommand(new AutoCommands.DT_DriveDistance(timeOut, power, distance, elevatorSetPoint, ingestorPower)); + } private void runCommand(AutoCommandBase command) { - // TODO Auto-generated method stub + //TODO Auto-generated method stub command.execute(); } - } - diff --git a/PowerUp/src/AutoModes/DriveStraight.java b/PowerUp/src/AutoModes/DriveStraight.java index 79592c4..f72a0f2 100644 --- a/PowerUp/src/AutoModes/DriveStraight.java +++ b/PowerUp/src/AutoModes/DriveStraight.java @@ -1,8 +1,10 @@ package AutoModes; import org.usfirst.frc.team548.robot.DriveTrain; +import org.usfirst.frc.team548.robot.Ingestor; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class DriveStraight extends AutoMode { @@ -15,6 +17,7 @@ protected void run() { String gameData = ""; gameData = DriverStation.getInstance().getGameSpecificMessage(); + // if(gameData==null) { for(int i = 0; i < 20; i++) { @@ -29,66 +32,103 @@ protected void run() { } } - - + SmartDashboard.putNumber("Auto Enc", DriveTrain.getEncoderAverage()); + //driveDistance(.5, -.2, (800), 10, 0, 0); driveDistance(.5, -.2, (800), 10, 0, 0); - driveDistance(1.2, -.6, (7510), 10, 0, 0); + if(gameData != null && gameData.charAt(0) == 'L'){ if(DriveTrain.isConnected()){ - - turnToAngle(1, -45, 30, .5, 0); - driveDistance(2, -.7, (20057), 10, 0, 0); - turnToAngle(2, 70, 45, .5, 0); + //double timeOut, double angle, double power, double elevatorSetPoint + Ingestor.closeIngestor(); + driveDistance(1.2, -.6, (7510), 10, 0, 0); + Ingestor.closeIngestor(); + dtTurnToAngle(1, -45, .5, 0, 0); + // 6-18-2018 + driveDistance(2, -.7, (20000), 10, 0, 0); + //turnToAngle(2, -35, 25, .5, 0); + dtTurnToAngle(2, 50, .5, 0, 0); driveDistance(1, -.5, (260 * 28), 10, 0, 0); - dropCube(1, .7, .7, 0); - armGoUp(.1, true); - driveDistance(2, .5, (260 * 8), 10, 0, 0); - turnToAngle(1, 90, 13, .5, 0); + dropCube(.5, .7, .6, 0); + driveDistance(2, .5, (260 * 12), 10, 0, 0); + //turnToAngle(1, -90, 13, .5, 0); + dtTurnToAngle(1, 90, .5, 0, 0); + armGoUp(1, true); + + + + + ////driveDistance(1.2, -.6, (7510), 10, 0, 0); + //driveDistance(1, -.6, (7510), 10, 0, 0); + ////turnToAngle(2, -45, 30, .5, 0); + //dtTurnToAngle(2, -45, .5, 0); + + ////driveDistance(2, -.7, (20057 - 2928), 10, 0, 0); + //driveDistance(2, -.7, (20057 - 2928), 10, 0, 0); + ////turnToAngle(2, 45, 20, .5, 0); + //dtTurnToAngle(2, 50, .5, 0); + //System.out.println("3"); + ////driveDistance(1, -.5, (260 * 28), 10, 0, 0); + //driveDistance(1, -.5, (260*28), 10, 0, 0); + //System.out.println("4"); + + ////power goes from 0-1 + //dropCube(1, 0.875, 0.875, 0); + //System.out.println("5"); + //armGoUp(.1, true); + ////driveDistance(2, .5, (260 * 8), 10, 0, 0); + //driveDistance(2, .5, (260*8), 10, 0, 0); + //System.out.println("6"); + + ////armGoUp(1, true); + ////turnToAngle(1, -90, 13, .5, 0); + //dtTurnToAngle(2, -90, .5, 0); + //System.out.println("7"); + + ////armGoUp(1, true); + System.out.println("8"); } else{ - turnToAngle(.4, -90, 13, .5, 0); - driveDistance(1, -.5, (260 * 20), 10, 0, 0); + System.out.println("NO GYRO"); + //turnToAngle(.4, -90, 13, .5, 0); + dtTurnToAngle(.4, -90, .5, 0, 0); + //driveDistance(1, -.5, (260 * 20), 10, 0, 0); + driveDistance(1, 0.5, (260 * 20), 10, 0, 0); + armGoUp(1, true); + } //uirkj } else { - if(DriveTrain.isConnected()){ - turnToAngle(1, 60, 30, .5, 0); - - driveDistance(2, -.7, (20057 - (260 * 10)), 10, 0, 0); - turnToAngle(1., -35, 25, .5, 0); + if(DriveTrain.isConnected() && gameData.charAt(0) == 'R' && gameData != null){ + //double timeOut, double angle, double power, double elevatorSetPoint + Ingestor.closeIngestor(); + driveDistance(1.2, -.6, (9000), 10, 0, 0); + Ingestor.closeIngestor(); + dtTurnToAngle(1.5, 45, .5, 0, 0); + // 6-18-2018 + driveDistance(2, -.7, (10000), 10, 0, 0); + //turnToAngle(2, -35, 25, .5, 0); + dtTurnToAngle(2, -35, .5, 0, 0); driveDistance(1, -.5, (260 * 28), 10, 0, 0); - dropCube(.5, .7, .7, 0); - //armGoUp(.1, true); + dropCube(.5, .7, .6, 0); driveDistance(2, .5, (260 * 12), 10, 0, 0); //turnToAngle(1, -90, 13, .5, 0); + dtTurnToAngle(1, 90, .5, 0, 0); + armGoUp(1, true); } else{ - turnToAngle(.4, -90, 13, .5, 0); + System.out.println("NO GYRO"); + //turnToAngle(.4, -90, 13, .5, 0); + dtTurnToAngle(.4, -90, .5, 0, 0); driveDistance(1, -.5, (260 * 20), 10, 0, 0); - } - //armGoUp(2, true); - - //armGoUp(1, true); + armGoUp(1, true); + + } } } -} - -//260 encoderclicks = 1 in - - -//write down autos and where they go and other stuff -//understand what is happening -//know how I am adjusting to failure and stuff - //Deal with failure -//alows us with something to start with in the next year - - - //drop off at back - //drop one off at switch, then attempt to pick one up and go to scale - //most autons should dump cube and then go towards middle of field \ No newline at end of file +} \ No newline at end of file diff --git a/PowerUp/src/AutoModes/LeftCross.java b/PowerUp/src/AutoModes/LeftCross.java new file mode 100644 index 0000000..bbc8212 --- /dev/null +++ b/PowerUp/src/AutoModes/LeftCross.java @@ -0,0 +1,90 @@ +package AutoModes; + +import org.usfirst.frc.team548.robot.DriveTrain; +import org.usfirst.frc.team548.robot.Ingestor; + +import edu.wpi.first.wpilibj.DriverStation; + +public class LeftCross extends AutoMode { + + @Override + protected void run() { + // TODO Auto-generated method stub + + + String gameData = ""; + gameData = DriverStation.getInstance().getGameSpecificMessage(); + + if(gameData==null) { + for(int i = 0; i < 20; i++) { + gameData = DriverStation.getInstance().getGameSpecificMessage(); + if(gameData!=null) break; + try { + Thread.sleep(5); + } catch (InterruptedException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + } + } + + + + if(gameData != null && gameData.charAt(1) == 'L'){ + + //driveDistance(.5, -.5, (260 * 8), 10, 0, .3); + Ingestor.closeIngestor(); + armGoUp(.001, true); + driveDistance(4.25, -.9, 72080, 10, 0, .3); + //turnToAngle(1.2, -120, 15, .5, 18750); + dtTurnToAngle(1.25, -115, .7, 22000, .3); + driveDistance(1.25, .85, 10000, 10, 22000, .3); + dropCube(.3, .7, .7, 22000); + //driveDistance(.6, -.85, (-2500), 20, -50000, 0); + dtTurnToAngle(1.25, 95, 0.6, -55000, 0); + Ingestor.openIngest(); + driveDistance(1.5, .9, (22500), 20, -50000, 1); + driveDistance(0.75, 0, 0, 0, -50000, 1); + Ingestor.closeIngestor(); + driveDistance(0.3, 0, 0, 0, -50000, 1); + driveDistance(1.5, -.9, 22000, 20, 0, 0); + dtTurnToAngle(1.1, -70, 0.9, 22000, 1); + driveDistance(1.5, .85, (5000), 20, 22000, 0); + dropCube(.5, 1, 1, 22000); + //OLD driveDistance(1, -.5, 2080 + 9 * 260, 20, 10000, 0); + //turnToAngle(1, 110, 25, .5, 0); + //driveDistance(1, .5, 2080 + 20 * 260, 20, 0, 0); + + + } + + + else if(gameData != null && gameData.charAt(1) == 'R'){ + Ingestor.closeIngestor(); + armGoUp(.001, true); + driveDistance(3.8, -.9, 65000, 10, 0, .3); + dtTurnToAngle(1.5, 95, .35, 0, .3); + driveDistance(4.25, -.9, 51000, 10, -30000, .3); + dtTurnToAngle(1.5, 87, .35, 22000, .3); + driveDistance(1.5, .7, 15000, 10, 22000, .3); + dropCube(.5, 1, 1, 22000); + driveDistance(1.5, -.7, 10000, 10, 22000, .3); + dtTurnToAngle(1.5, 180, .35, -50000, .3); + //dtTurnToAngle(2,-52.5,50,.5); + //driveDistance(1, -.6, (260 * 25), 10, 0, 0); + //driveDistance(2, 0, 0, -50000, 0, 0); + + } + else{ + driveDistance(1, -.3, (260 * 8), 10, 0, .3); + driveDistance(8, -.7, (72280 - (260 * 80)), 10, 0, .3); + turnToAngle(2, 45, 8, .5, 0); + Ingestor.closeIngestor(); + armGoUp(1, true); + } + + + + } + +} \ No newline at end of file diff --git a/PowerUp/src/AutoModes/LeftScaleSwitchOutside.java b/PowerUp/src/AutoModes/LeftScaleSwitchOutside.java new file mode 100644 index 0000000..7d735b9 --- /dev/null +++ b/PowerUp/src/AutoModes/LeftScaleSwitchOutside.java @@ -0,0 +1,76 @@ +package AutoModes; + +import org.usfirst.frc.team548.robot.DriveTrain; +import org.usfirst.frc.team548.robot.Ingestor; + +import edu.wpi.first.wpilibj.DriverStation; + +public class LeftScaleSwitchOutside extends AutoMode { + + @Override + protected void run() { + // TODO Auto-generated method stub + + + String gameData = ""; + gameData = DriverStation.getInstance().getGameSpecificMessage(); + + if(gameData==null) { + for(int i = 0; i < 20; i++) { + gameData = DriverStation.getInstance().getGameSpecificMessage(); + if(gameData!=null) break; + try { + Thread.sleep(5); + } catch (InterruptedException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + } + } + + + + if(gameData != null && gameData.charAt(1) == 'L'){ + + //driveDistance(.5, -.5, (260 * 8), 10, 0, .3); + Ingestor.closeIngestor(); + armGoUp(.001, true); + driveDistance(4.25, -.9, 72080, 10, 0, .3); + //turnToAngle(1.2, -120, 15, .5, 18750); + dtTurnToAngle(1.25, -115, .7, 22000, .3); + driveDistance(1.25, .85, 10000, 10, 22000, .3); + dropCube(.7, .7, .7, 22000); + driveDistance(3, -.6, (-20000), 20, -50000, 0); + + + } + + + else if(gameData != null && gameData.charAt(0) == 'L'){ + Ingestor.closeIngestor(); + armGoDown(.1, true); + driveDistance(.7, -.3, (260 * 8), 10, 0, 0); + driveDistance(4, -.6, (40000), 10, 0, 0); + dtTurnToAngle(2, 80, 5, 0, 0); + driveDistance(1, -.6, (1500 * 50), 10, 0, 0); + dropCube(1, .9, .9, 0); + driveDistance(1, .6, 3000, 10, 0, 0); + //dtTurnToAngle(2,-52.5,50,.5); + //driveDistance(1, -.6, (260 * 25), 10, 0, 0); + armGoUp(3, true); + driveDistance(2, 0, 0, -50000, 0, 0); + + } + else{ + driveDistance(1, -.3, (260 * 8), 10, 0, .3); + driveDistance(8, -.7, (72280 - (260 * 80)), 10, 0, .3); + turnToAngle(2, 45, 8, .5, 0); + Ingestor.closeIngestor(); + armGoUp(1, true); + } + + + + } + +} \ No newline at end of file diff --git a/PowerUp/src/AutoModes/RightCross.java b/PowerUp/src/AutoModes/RightCross.java new file mode 100644 index 0000000..9181761 --- /dev/null +++ b/PowerUp/src/AutoModes/RightCross.java @@ -0,0 +1,113 @@ +package AutoModes; + +import org.usfirst.frc.team548.robot.DriveTrain; +import org.usfirst.frc.team548.robot.Ingestor; + +import edu.wpi.first.wpilibj.DriverStation; + +public class RightCross extends AutoMode { + + @Override + protected void run() { + // TODO Auto-generated method stub + + + String gameData = ""; + gameData = DriverStation.getInstance().getGameSpecificMessage(); + + if(gameData==null) { + for(int i = 0; i < 20; i++) { + gameData = DriverStation.getInstance().getGameSpecificMessage(); + if(gameData!=null) break; + try { + Thread.sleep(5); + } catch (InterruptedException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + } + } + + + +if(gameData != null && gameData.charAt(1) == 'R'){ + + //driveDistance(.5, -.5, (260 * 8), 10, 0, .3); + Ingestor.closeIngestor(); + armGoUp(.001, true); + driveDistance(4.25, -.9, 72080, 10, 0, .3); + //turnToAngle(1.2, -120, 15, .5, 18750); + dtTurnToAngle(1.25, 115, .7, 22000, .3); + driveDistance(1.25, .85, 10000, 10, 22000, .3); + dropCube(.3, .7, .7, 22000); + //driveDistance(.6, -.85, (-2500), 20, -50000, 0); + dtTurnToAngle(1.25, -98, 0.6, -55000, 0); + Ingestor.openIngest(); + driveDistance(1.5, .9, (22500), 20, -50000, 1); + driveDistance(0.75, .3, 500, 0, -50000, 1); + Ingestor.closeIngestor(); + driveDistance(0.3, 0, 0, 0, -50000, 1); + driveDistance(1.5, -.9, 15000, 20, 0, 0); + dtTurnToAngle(1.1, 90, 0.9, 22000, 1); + driveDistance(1.5, .85, (9000), 20, 22000, 0); + dropCube(.5, 1, 1, 22000); + //OLD driveDistance(1, -.5, 2080 + 9 * 260, 20, 10000, 0); + //turnToAngle(1, 110, 25, .5, 0); + //driveDistance(1, .5, 2080 + 20 * 260, 20, 0, 0); + + + } + + + else if(gameData != null && gameData.charAt(1) == 'L'){ + + Ingestor.closeIngestor(); + armGoUp(0.001, true); + driveDistance(3.8, -.9, 57000, 10, 0, .3); + dtTurnToAngle(1.5, -80, .35, 0, .3); + driveDistance(4.25, -.9, 51000, 10, -30000, .3); + dtTurnToAngle(1.5, -82, .35, 22000, .3); + driveDistance(1.5, .7, 15000, 10, 22000, .3); + dropCube(.5, 1, 1, 22000); + driveDistance(1.5, -.7, 10000, 10, 22000, .3); + dtTurnToAngle(1.5, 180, .35, -50000, .3); + +/* + * //driveDistance(.5, -.5, (260 * 8), 10, 0, .3); + Ingestor.closeIngestor(); + armGoUp(.001, true); + driveDistance(4.25, -.9, 72080, 10, 0, .3); + //turnToAngle(1.2, -120, 15, .5, 18750); + dtTurnToAngle(1.25, 115, .7, 22000, .3); + driveDistance(1.25, .85, 10000, 10, 22000, .3); + dropCube(.3, .7, .7, 22000); + //driveDistance(.6, -.85, (-2500), 20, -50000, 0); + dtTurnToAngle(1.25, -98, 0.6, -55000, 0); + Ingestor.openIngest(); + driveDistance(1.5, .9, (22500), 20, -50000, 1); + driveDistance(0.75, .3, 500, 0, -50000, 1); + Ingestor.closeIngestor(); + driveDistance(0.3, 0, 0, 0, -50000, 1); + driveDistance(1.5, -.9, 15000, 20, 0, 0); + dtTurnToAngle(1.1, 90, 0.9, 22000, 1); + driveDistance(1.5, .85, (9000), 20, 22000, 0); + dropCube(.5, 1, 1, 22000); + //OLD driveDistance(1, -.5, 2080 + 9 * 260, 20, 10000, 0); + //turnToAngle(1, 110, 25, .5, 0); + //driveDistance(1, .5, 2080 + 20 * 260, 20, 0, 0); + * + */ + } + else{ + driveDistance(1, -.3, (260 * 8), 10, 0, .3); + driveDistance(8, -.7, (72280 - (260 * 80)), 10, 0, .3); + turnToAngle(2, -45, 8, .5, 0); + Ingestor.closeIngestor(); + armGoUp(1, true); + } + + + + } + +} diff --git a/PowerUp/src/AutoModes/RightScaleSwitchOutside.java b/PowerUp/src/AutoModes/RightScaleSwitchOutside.java new file mode 100644 index 0000000..b90d279 --- /dev/null +++ b/PowerUp/src/AutoModes/RightScaleSwitchOutside.java @@ -0,0 +1,76 @@ +package AutoModes; + +import org.usfirst.frc.team548.robot.DriveTrain; +import org.usfirst.frc.team548.robot.Ingestor; + +import edu.wpi.first.wpilibj.DriverStation; + +public class RightScaleSwitchOutside extends AutoMode { + + @Override + protected void run() { + // TODO Auto-generated method stub + + + String gameData = ""; + gameData = DriverStation.getInstance().getGameSpecificMessage(); + + if(gameData==null) { + for(int i = 0; i < 20; i++) { + gameData = DriverStation.getInstance().getGameSpecificMessage(); + if(gameData!=null) break; + try { + Thread.sleep(5); + } catch (InterruptedException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + } + } + + + + if(gameData != null && gameData.charAt(1) == 'R'){ + + //driveDistance(.5, -.5, (260 * 8), 10, 0, .3); + Ingestor.closeIngestor(); + armGoUp(.001, true); + driveDistance(4.25, -.9, 72080, 10, 0, .3); + //turnToAngle(1.2, -120, 15, .5, 18750); + dtTurnToAngle(1.25, 115, .7, 22000, .3); + driveDistance(1.25, .85, 10000, 10, 22000, .3); + dropCube(.7, .7, .7, 22000); + driveDistance(3, -.6, (-20000), 20, -50000, 0); + + + } + + + else if(gameData != null && gameData.charAt(0) == 'R'){ + Ingestor.closeIngestor(); + armGoDown(.1, true); + driveDistance(.7, -.3, (260 * 8), 10, 0, 0); + driveDistance(4, -.6, (40000), 10, 0, 0); + dtTurnToAngle(2, -80, 5, 0, 0); + driveDistance(1, -.6, (1500 * 50), 10, 0, 0); + dropCube(1, .9, .9, 0); + driveDistance(1, .6, 3000, 10, 0, 0); + //dtTurnToAngle(2,-52.5,50,.5); + //driveDistance(1, -.6, (260 * 25), 10, 0, 0); + armGoUp(3, true); + driveDistance(2, 0, 0, -50000, 0, 0); + + } + else{ + driveDistance(1, -.3, (260 * 8), 10, 0, .3); + driveDistance(8, -.7, (72280 - (260 * 80)), 10, 0, .3); + turnToAngle(2, -45, 8, .5, 0); + Ingestor.closeIngestor(); + armGoUp(1, true); + } + + + + } + +} \ No newline at end of file diff --git a/PowerUp/src/AutoModes/RightSwitchRightScale.java b/PowerUp/src/AutoModes/RightSwitchRightScale.java new file mode 100644 index 0000000..f803182 --- /dev/null +++ b/PowerUp/src/AutoModes/RightSwitchRightScale.java @@ -0,0 +1,89 @@ +package AutoModes; + +import org.usfirst.frc.team548.robot.DriveTrain; +import org.usfirst.frc.team548.robot.Ingestor; + +import edu.wpi.first.wpilibj.DriverStation; + +public class RightSwitchRightScale extends AutoMode { + + @Override + protected void run() { + // TODO Auto-generated method stub + + + String gameData = ""; + gameData = DriverStation.getInstance().getGameSpecificMessage(); + + if(gameData==null) { + for(int i = 0; i < 20; i++) { + gameData = DriverStation.getInstance().getGameSpecificMessage(); + if(gameData!=null) break; + try { + Thread.sleep(5); + } catch (InterruptedException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + } + } + + + +if(gameData != null && gameData.charAt(1) == 'R'){ + + //driveDistance(.5, -.5, (260 * 8), 10, 0, .3); + Ingestor.closeIngestor(); + armGoUp(.001, true); + driveDistance(4.25, -.9, 72080, 10, 0, .3); + //turnToAngle(1.2, -120, 15, .5, 18750); + dtTurnToAngle(1.25, 115, .7, 22000, .3); + driveDistance(1.25, .85, 10000, 10, 22000, .3); + dropCube(.3, .7, .7, 22000); + //driveDistance(.6, -.85, (-2500), 20, -50000, 0); + dtTurnToAngle(1.25, -98, 0.6, -55000, 0); + Ingestor.openIngest(); + driveDistance(1.5, .9, (22500), 20, -50000, 1); + driveDistance(0.75, .3, 500, 0, -50000, 1); + Ingestor.closeIngestor(); + driveDistance(0.3, 0, 0, 0, -50000, 1); + driveDistance(1.5, -.9, 15000, 20, 0, 0); + dtTurnToAngle(1.1, 90, 0.9, 22000, 1); + driveDistance(1.5, .85, (9000), 20, 22000, 0); + dropCube(.5, 1, 1, 22000); + //OLD driveDistance(1, -.5, 2080 + 9 * 260, 20, 10000, 0); + //turnToAngle(1, 110, 25, .5, 0); + //driveDistance(1, .5, 2080 + 20 * 260, 20, 0, 0); + + + } + + + else if(gameData != null && gameData.charAt(0) == 'R'){ + Ingestor.closeIngestor(); + armGoDown(.1, true); + driveDistance(.7, -.3, (260 * 8), 10, 0, 0); + driveDistance(4, -.6, (37000), 10, 0, 0); + dtTurnToAngle(2, -80, 5, 0, 0); + driveDistance(1, -.6, (1500 * 50), 10, 0, 0); + dropCube(1, .9, .9, 0); + driveDistance(1, .6, 3000, 10, 0, 0); + //dtTurnToAngle(2,-52.5,50,.5); + //driveDistance(1, -.6, (260 * 25), 10, 0, 0); + armGoUp(3, true); + driveDistance(2, 0, 0, -50000, 0, 0); + + } + else{ + driveDistance(1, -.3, (260 * 8), 10, 0, .3); + driveDistance(8, -.7, (72280 - (260 * 80)), 10, 0, .3); + turnToAngle(2, -45, 8, .5, 0); + Ingestor.closeIngestor(); + armGoUp(1, true); + } + + + + } + +} diff --git a/PowerUp/src/AutoModes/SwitchAuto.java b/PowerUp/src/AutoModes/SwitchAuto.java index de10f66..6f3a5f7 100644 --- a/PowerUp/src/AutoModes/SwitchAuto.java +++ b/PowerUp/src/AutoModes/SwitchAuto.java @@ -1,6 +1,7 @@ package AutoModes; import org.usfirst.frc.team548.robot.DriveTrain; +import org.usfirst.frc.team548.robot.Ingestor; import edu.wpi.first.wpilibj.DriverStation; @@ -31,13 +32,25 @@ protected void run() { if(gameData != null && gameData.charAt(1) == 'L'){ + //driveDistance(.5, -.5, (260 * 8), 10, 0, .3); + Ingestor.closeIngestor(); armGoUp(.001, true); - driveDistance(1, .3, (260 * 8), 10, 61000, .3); - driveDistance(6, .7, (72280 - (15 * 260)), 10, 61000, .3); - turnToAngle(1.2, 30, 15, .5, 61000); - driveDistance(.5, .4, 8 * 260, 10, 61000, .3); - dropCube(.5, 1, 1, 61000); - driveDistance(2, -.5, 2080 + (12 * 260), 20, 61000, 0); + driveDistance(4.25, -.9, 72080, 10, 0, .3); + //turnToAngle(1.2, -120, 15, .5, 18750); + dtTurnToAngle(1.25, -115, .7, 22000, .3); + driveDistance(1.25, .85, 10000, 10, 22000, .3); + dropCube(.3, .7, .7, 22000); + //driveDistance(.6, -.85, (-2500), 20, -50000, 0); + dtTurnToAngle(1.25, 95, 0.6, -55000, 0); + Ingestor.openIngest(); + driveDistance(1.5, .9, (22500), 20, -50000, 1); + driveDistance(0.75, 0, 0, 0, -50000, 1); + Ingestor.closeIngestor(); + driveDistance(0.3, 0, 0, 0, -50000, 1); + driveDistance(1.5, -.9, 22000, 20, 0, 0); + dtTurnToAngle(1.1, -70, 0.9, 22000, 1); + driveDistance(1.5, .85, (5000), 20, 22000, 0); + dropCube(.5, 1, 1, 22000); //OLD driveDistance(1, -.5, 2080 + 9 * 260, 20, 10000, 0); //turnToAngle(1, 110, 25, .5, 0); //driveDistance(1, .5, 2080 + 20 * 260, 20, 0, 0); @@ -47,18 +60,25 @@ protected void run() { else if(gameData != null && gameData.charAt(0) == 'L'){ - driveDistance(.7, .3, (260 * 8), 10, 0, 0); - driveDistance(4, .6, (260*144), 10, 0, 0); - turnToAngle(2, -90, 45, .5, 0); - driveDistance(1, -.6, (260 * 30), 10, 0, 0); + Ingestor.closeIngestor(); + armGoDown(.1, true); + driveDistance(.7, -.3, (260 * 8), 10, 0, 0); + driveDistance(4, -.6, (40000), 10, 0, 0); + dtTurnToAngle(2, 80, 5, 0, 0); + driveDistance(1, -.6, (1500 * 50), 10, 0, 0); dropCube(1, .9, .9, 0); - driveDistance(1, .6, (260 * 25), 10, 0, 0); - armGoUp(.1, true); + driveDistance(1, .6, 3000, 10, 0, 0); + //dtTurnToAngle(2,-52.5,50,.5); + //driveDistance(1, -.6, (260 * 25), 10, 0, 0); + armGoUp(3, true); + driveDistance(2, 0, 0, -50000, 0, 0); + } else{ - driveDistance(1, .3, (260 * 8), 10, 0, .3); - driveDistance(8, .7, (72280 - (260 * 80)), 10, 0, .3); + driveDistance(1, -.3, (260 * 8), 10, 0, .3); + driveDistance(8, -.7, (72280 - (260 * 80)), 10, 0, .3); turnToAngle(2, 45, 8, .5, 0); + Ingestor.closeIngestor(); armGoUp(1, true); } diff --git a/PowerUp/src/AutoModes/SwitchAuto1.java b/PowerUp/src/AutoModes/SwitchAuto1.java new file mode 100644 index 0000000..e1c1627 --- /dev/null +++ b/PowerUp/src/AutoModes/SwitchAuto1.java @@ -0,0 +1,93 @@ +package AutoModes; + +import org.usfirst.frc.team548.robot.DriveTrain; + +import edu.wpi.first.wpilibj.DriverStation; + +public class SwitchAuto1 extends AutoMode { + + @Override + protected void run() { + // TODO Auto-generated method stub + + + String gameData = ""; + gameData = DriverStation.getInstance().getGameSpecificMessage(); + + if(gameData==null) { + for(int i = 0; i < 20; i++) { + gameData = DriverStation.getInstance().getGameSpecificMessage(); + if(gameData!=null) break; + try { + Thread.sleep(5); + } catch (InterruptedException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + } + } + + + /* + if(gameData != null && gameData.charAt(1) == 'L'){ + /* + armGoUp(.001, true); + driveDistance(1, .3, (260 * 8), 10, 0, .3); + driveDistance(7, .7, (72280 - (29 * 260)), 10, 0, .3); + turnToAngle(1, 30, 25, .5, 10000); + driveDistance(2, .4, 15 * 260, 10, 10000, .3); + dropCube(.5, .9, .9, 10000); + driveDistance(1, -.5, 2080 + 12 * 260, 20, 0, 0); + //OLD driveDistance(1, -.5, 2080 + 9 * 260, 20, 10000, 0); + turnToAngle(1, 110, 25, .5, 0); + driveDistance(1, .5, 2080 + 20 * 260, 20, 0, 0); + + armGoUp(.001, true); + driveDistance(1, .3, (260 * 8), 10, 0, .3); + driveDistance(6, .7, (72280 - (15 * 260)), 10, 15000, .3); + turnToAngle(1.2, 30, 15, .5, 15000); + driveDistance(.5, .4, 8 * 260, 10, 15000, .3); + dropCube(.5, 1, 1, 15000); + driveDistance(2, -.5, 2080 + (10 * 260), 20, 15000, 0); + + } + */ + + + if(gameData != null && gameData.charAt(0) == 'L'){ + driveDistance(.7, -.3, (260 * 8), 10, 0, 0); + driveDistance(4, -.6, (260*144), 10, 0, 0); + turnToAngle(2, 90, 45, .5, 0); + driveDistance(1, -.6, (260 * 30), 10, 0, 0); + dropCube(1, .9, .9, 0); + driveDistance(1, -.6, (260 * 10), 10, 0, 0); + //turnToAngle(2, 90, 45, .5, 0); + //armGoUp(.1, true); + } + else{ + driveDistance(.7, -.3, (260 * 8), 10, 0, 0); + driveDistance(4, -.6, (260*144), 10, 0, 0); + //armGoUp(1, true); + } + + +// if(gameData != null && gameData.charAt(0) == 'L'){ +// driveDistance(.7, -.3, (260 * 8), 10, 0, 0); +// driveDistance(4, -.6, (260*140), 10, 0, 0); +// +// //double seconds, double angle, double power, double elevatorSetPoint +// turnToAngle(2, 90, 50, .5, 0); +// driveDistance(1, -.6, (260 * 30), 10, 0, 0); +// dropCube(1, .9, .9, 0); +// driveDistance(1, .6, (260 * 10), 10, 0, 0); +// +// }else{ +// driveDistance(.7, -.3, (260 * 8), 10, 0, 0); +// driveDistance(4, -.6, (260*144), 10, 0, 0); +// //armGoUp(1, true); +// } +// + + } + +} \ No newline at end of file diff --git a/PowerUp/src/AutoModes/SwitchAuto2.java b/PowerUp/src/AutoModes/SwitchAuto2.java index 81b8dd7..58f402c 100644 --- a/PowerUp/src/AutoModes/SwitchAuto2.java +++ b/PowerUp/src/AutoModes/SwitchAuto2.java @@ -23,65 +23,51 @@ protected void run() { } } - - if(gameData != null && gameData.charAt(1) == 'R'){ - // - //test 1 - Increase turn, was -15, now -45 - //test 2 change to -60 - //change to -50 - + /* + if(gameData != null && gameData.charAt(1) == 'R'){ + //The gyro seems to be undocumented armGoUp(.001, true); - driveDistance(1, .3, (260 * 8), 10, 61000, .3); - driveDistance(5.5, .7, (72280 - (15 * 260)), 10, 61000, .3); - turnToAngle(1, -50, 35, .5, 61000); - driveDistance(1, .5, (260 * 8), 10, 61000, .3); - dropCube(.5, 1, 1, 61000); - driveDistance(1, -.5, 2080 + 12 * 260, 20, 61000, 0); - - /* - //armGoUp(.001, true); - driveDistance(1, .3, (260 * 8), 10, 0, .3); - driveDistance(8, .7, (72280 - (26 * 260)), 10, 0, .3); - // - //turnToAngle(1, -90, 8, .5, 20000); - ///driveDistance(2, -.5, 2080 + 3 * 260, 10, 20000, .3); - //dropCube(2, 1, 1, 15000); - //driveDistance(1, .5, 2080 + 9 * 260, 20, 20000, 0); - */ - - } + driveDistance(1, .3, (260 * 8), 10, 0, .3); + driveDistance(7, .7, (72280 - (29 * 260)), 10, 0, .3); + turnToAngle(1, -30, 25, .5, 10000); + driveDistance(2, .4, 15 * 260, 10, 10000, .3); + dropCube(.5, .9, .9, 10000); + driveDistance(1, -.5, 2080 + 12 * 260, 20, 0, 0); + //OLD driveDistance(1, -.5, 2080 + 9 * 260, 20, 10000, 0); + turnToAngle(1, -110, 25, .5, 0); + driveDistance(1, .5, 2080 + 20 * 260, 20, 0, 0); - else if(gameData != null && gameData.charAt(0) == 'R'){ + armGoUp(.001, true); + driveDistance(1, .3, (260 * 8), 10, 0, .3); + driveDistance(5.5, .7, (72280 - (15 * 260)), 10, 15000, .3); + turnToAngle(1, -50, 35, .5, 15000); + driveDistance(1, .5, (260 * 8), 10, 15000, .3); + dropCube(.5, 1, 1, 15000); + driveDistance(1, -.5, 12 * 260, 20, 15000, 0); + } + */ + + if(gameData != null && gameData.charAt(0) == 'R'){ driveDistance(.7, .3, (260 * 8), 10, 0, 0); driveDistance(4, .6, (260*144), 10, 0, 0); turnToAngle(2, 90, 45, .5, 0); - driveDistance(1, -.6, (260 * 30), 10, 0, 0); dropCube(1, .9, .9, 0); - driveDistance(1, .6, (260 * 25), 10, 0, 0); - armGoUp(.1, true); + driveDistance(1, .6, (260 * 15), 10, 0, 0); + turnToAngle(2, -90, 45, .5, 0); + //armGoUp(.1, true); - /* + } + else{ driveDistance(.7, .3, (260 * 8), 10, 0, 0); driveDistance(4, .6, (260*144), 10, 0, 0); //turnToAngle(2, 90, 8, .5, 0); - turnToAngle(2, 60, 8, .5, 0); - driveDistance(1, -.6, (260 * 30), 10, 0, 0); - dropCube(1, .9, .9, 0); - driveDistance(1, .6, (260 * 25), 10, 0, 0); //armGoUp(1, true); - */ - } - else{ - driveDistance(1, .3, (260 * 8), 10, 0, .3); - driveDistance(8, .7, (72280 - (260 * 80)), 10, 0, .3); - turnToAngle(2, -45, 8, .5, 0); - armGoUp(1, true); } diff --git a/PowerUp/src/AutoModes/SwitchPriorityLeft.java b/PowerUp/src/AutoModes/SwitchPriorityLeft.java new file mode 100644 index 0000000..8e2bb08 --- /dev/null +++ b/PowerUp/src/AutoModes/SwitchPriorityLeft.java @@ -0,0 +1,69 @@ +package AutoModes; + +import org.usfirst.frc.team548.robot.Ingestor; + +import edu.wpi.first.wpilibj.DriverStation; + +public class SwitchPriorityLeft extends AutoMode { + + @Override + protected void run() { + // TODO Auto-generated method stub + + + String gameData = ""; + gameData = DriverStation.getInstance().getGameSpecificMessage(); + + if(gameData==null) { + for(int i = 0; i < 20; i++) { + gameData = DriverStation.getInstance().getGameSpecificMessage(); + if(gameData!=null) break; + try { + Thread.sleep(5); + } catch (InterruptedException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + } + } + // + if(gameData != null && gameData.charAt(0) == 'L'){ + driveDistance(.7, -.3, (260 * 8), 10, 0, 0); + driveDistance(4, -.6, (260*10), 10, 0, 0); + //turnToAngle(2, -90, 45, .5, 0); + turnToAngle(2, 40, 0, .5, 0); + /*driveDistance(1, -.6, (260 * 30), 10, 0, 0); + dropCube(1, .9, .9, 0); + driveDistance(1, -.6, (260 * 15), 10, 0, 0); + turnToAngle(2, 90, 45, .5, 0); + armGoUp(.1, true); + */ + } + /* + else if(gameData != null && gameData.charAt(1) == 'L'){ + armGoUp(.001, true); + driveDistance(1, .3, (260 * 8), 10, 0, .3); + driveDistance(7, .7, (72280 - (29 * 260)), 10, 0, .3); + + turnToAngle(1, 30, 25, .5, 10000); + driveDistance(2, .4, 15 * 260, 10, 10000, .3); + dropCube(.5, .9, .9, 10000); + driveDistance(1, -.5, 2080 + 12 * 260, 20, 0, 0); + //OLD driveDistance(1, -.5, 2080 + 9 * 260, 20, 10000, 0); + turnToAngle(1, 110, 25, .5, 0); + driveDistance(1, .5, 2080 + 20 * 260, 20, 0, 0); + } + */ + else{ + driveDistance(1, -.3, (260 * 8), 10, 0, .3); + driveDistance(8, -.7, (72280 - (260 * 80)), 10, 0, .3); + turnToAngle(2, 45, 8, .5, 0); + Ingestor.closeIngestor(); + armGoUp(1, true); + } + + + + } + +} \ No newline at end of file diff --git a/PowerUp/src/AutoModes/SwitchPriorityRight.java b/PowerUp/src/AutoModes/SwitchPriorityRight.java new file mode 100644 index 0000000..4d2a5c5 --- /dev/null +++ b/PowerUp/src/AutoModes/SwitchPriorityRight.java @@ -0,0 +1,68 @@ +package AutoModes; + +import org.usfirst.frc.team548.robot.Ingestor; + +import edu.wpi.first.wpilibj.DriverStation; + +public class SwitchPriorityRight extends AutoMode { + + @Override + protected void run() { + // TODO Auto-generated method stub + String gameData = ""; + gameData = DriverStation.getInstance().getGameSpecificMessage(); + + if(gameData==null) { + for(int i = 0; i < 20; i++) { + gameData = DriverStation.getInstance().getGameSpecificMessage(); + if(gameData!=null) break; + try { + Thread.sleep(5); + } catch (InterruptedException e) { + // TODO Auto-generated catch block + e.printStackTrace(); + } + } + //sdfgsdfg + } + + if(gameData != null && gameData.charAt(0) == 'R'){ + driveDistance(.7, -.3, (260 * 8), 10, 0, 0); + driveDistance(4, -.6, (260*142), 10, 0, 0); + turnToAngle(2, 90, 8, .5, 0); + driveDistance(1, -.6, (260 * 30), 10, 0, 0); + dropCube(1, .9, .9, 0); + driveDistance(1, -.6, (260 * 15), 10, 0, 0); + turnToAngle(2, -90, 8, .5, 0); + armGoUp(1, true); + + + } + /* + else if(gameData != null && gameData.charAt(1) == 'R'){ + armGoUp(.001, true); + driveDistance(1, .3, (260 * 8), 10, 0, .3); + driveDistance(7, .7, (72280 - (29 * 260)), 10, 0, .3); + turnToAngle(1, -30, 25, .5, 10000); + driveDistance(2, .4, 15 * 260, 10, 10000, .3); + dropCube(.5, .9, .9, 10000); + driveDistance(1, -.5, 2080 + 12 * 260, 20, 0, 0); + //OLD driveDistance(1, -.5, 2080 + 9 * 260, 20, 10000, 0); + turnToAngle(1, -110, 25, .5, 0); + driveDistance(1, .5, 2080 + 20 * 260, 20, 0, 0); + + + } + */ + else{ + driveDistance(1, .3, (260 * 8), 10, 0, .3); + driveDistance(8, .7, (72280 - (260 * 80)), 10, 0, .3); + turnToAngle(2, -45, 8, .5, 0); + Ingestor.closeIngestor(); + armGoUp(1, true); + } + + + } + +} \ No newline at end of file diff --git a/PowerUp/src/org/usfirst/frc/team548/robot/Climber.java b/PowerUp/src/org/usfirst/frc/team548/robot/Climber.java index fd1657d..6374b94 100644 --- a/PowerUp/src/org/usfirst/frc/team548/robot/Climber.java +++ b/PowerUp/src/org/usfirst/frc/team548/robot/Climber.java @@ -16,14 +16,13 @@ public static Climber getInstance(){ private Climber(){ talonUn = new TalonSRX(Constants.CLIMBER_TALON_UN); //Master talonDeux = new TalonSRX(Constants.CLIMBER_TALON_DEUX); //Slave - talonTrois = new TalonSRX(Constants.CLIMBER_TALON_TROIS); //Slave - + talonTrois = new TalonSRX(Constants.CLIMBER_TALON_TROIS); //Master + talonDeux.set(ControlMode.Follower, talonUn.getDeviceID()); + talonTrois.set(ControlMode.Follower, talonUn.getDeviceID()); } public static void climb(double power){ talonUn.set(ControlMode.PercentOutput, power); - talonDeux.set(ControlMode.PercentOutput, power); - talonTrois.set(ControlMode.PercentOutput, power); } diff --git a/PowerUp/src/org/usfirst/frc/team548/robot/Constants.java b/PowerUp/src/org/usfirst/frc/team548/robot/Constants.java index a1ab0e2..1d1e89d 100644 --- a/PowerUp/src/org/usfirst/frc/team548/robot/Constants.java +++ b/PowerUp/src/org/usfirst/frc/team548/robot/Constants.java @@ -4,21 +4,22 @@ public class Constants { //Drive Train - public static final int DT_TALON_RIGHTFRONT = 1; // has encoder + public static final int DT_TALON_RIGHTFRONT = 3; // has encoder public static final int DT_TALON_RIGHTBACK = 2; public static final int DT_TALON_LEFTFRONT = 13; public static final int DT_TALON_LEFTBACK = 14; // has encoder - public static final int DT_SOLENOID_SHIFTER = 4; + + public static final int DT_SOLENOID_SHIFTER = 1; public static final boolean DT_SOLENOID_BOOLEAN = true; - public static final double DT_PID_P = 0.0105d; - public static final double DT_PID_I = 0.0016; - public static final double DT_PID_D = 0.0; + public static final double DT_PID_P = 0.025d; + public static final double DT_PID_I = 0; + public static final double DT_PID_D = 0.06d; public static final double DT_DRIVE_STRAIGHT = 1; //Elevator public static final int ELEVATOR_TOP_LIMITSWITCH = 1; public static final int ELEVATOR_BOTTOM_LIMITSWITCH = 2; - public static final int ELEVATOR_TALON_RGIHT = 7; + public static final int ELEVATOR_TALON_RGIHT = 12; // has encoder public static final int ELEVATOR_TALON_LEFT = 8; public static final double ELEVATOR_DOWNPOWER = -.1; public static final double ELEVATOR_DOWN = 0; @@ -30,24 +31,25 @@ public class Constants { public static final double ELEAVTOR_PID_I = 0; public static final double ELEAVTOR_PID_D = 0; public static final double ELEAVTOR_PID_F = 0; - public static final int ELEVATOR_SOLENOID1 = 7; - public static final int ELEVATOR_SOLENOID2 = 6; + public static final int ELEVATOR_SOLENOID1 = 3; //going up + public static final int ELEVATOR_SOLENOID2 = 2; //going down + public static final int ELEVATOR_SOLENOID3 = 7; //Ingestor - public static final int INGESTOR_TALON_RIGHT = 5; - public static final int INGESTOR_TALONG_LEFT = 9; + public static final int INGESTOR_TALON_RIGHT = 6; + public static final int INGESTOR_TALONG_LEFT = 7; public static final int INGESTOR_CURRENT_CONSTANT = 4; public static final int INGESTOR_CURRENT_TIMER = 15; - //1 is closed, 0 is open + public static final int INGESTOR_SOLENOID_CLOSE_HEAVY = 0; + public static final int INGESTOR_SOLENOID_CLOSE_LIGHT = 5; //Climber - public static final int CLIMBER_TALON_UN = 3; - public static final int CLIMBER_TALON_DEUX = 11; - public static final int CLIMBER_TALON_TROIS = 12; + ;public static final int CLIMBER_TALON_UN = 5; + public static final int CLIMBER_TALON_DEUX = 4; + public static final int CLIMBER_TALON_TROIS = 11; //TeleOp public static final int XB_POS_DRIVER = 0; public static final int XB_POS_MANIP = 1; } - \ No newline at end of file diff --git a/PowerUp/src/org/usfirst/frc/team548/robot/DriveTrain.java b/PowerUp/src/org/usfirst/frc/team548/robot/DriveTrain.java index 8e49a3d..71879c2 100644 --- a/PowerUp/src/org/usfirst/frc/team548/robot/DriveTrain.java +++ b/PowerUp/src/org/usfirst/frc/team548/robot/DriveTrain.java @@ -7,9 +7,10 @@ import edu.wpi.first.wpilibj.PIDController; import edu.wpi.first.wpilibj.PIDOutput; +import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj.SerialPort; +import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.Solenoid; -import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.interfaces.Gyro; public class DriveTrain implements PIDOutput { @@ -25,21 +26,25 @@ public static DriveTrain getInstance(){ private static Solenoid sol; private static AHRS hyro; private static PIDController pid; + private static Servo tip, ramp; private DriveTrain(){ + //ramp = new Servo(1); + tip = new Servo(1); rightFront = new TalonSRX(Constants.DT_TALON_RIGHTFRONT); // has encoder rightBack = new TalonSRX(Constants.DT_TALON_RIGHTBACK); leftFront = new TalonSRX(Constants.DT_TALON_LEFTFRONT); leftBack = new TalonSRX(Constants.DT_TALON_LEFTBACK); // has encoder sol = new Solenoid(Constants.DT_SOLENOID_SHIFTER); - hyro = new AHRS(SerialPort.Port.kUSB1); + //hyro = new AHRS(SerialPort.Port.kUSB2); + hyro = new AHRS(SPI.Port.kMXP); rightFront.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 10); leftBack.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 10); pid = new PIDController(Constants.DT_PID_P, Constants.DT_PID_I, Constants.DT_PID_D, hyro, this); pid.setInputRange(-180.0f, 180.0f); - pid.setOutputRange(-0.5f, 0.5f); + pid.setOutputRange(-0.7f, 0.7f); pid.setAbsoluteTolerance(2f); - + pid.setContinuous(true); } public static void drive(double leftPower, double rightPower){ @@ -47,7 +52,6 @@ public static void drive(double leftPower, double rightPower){ rightBack.set(ControlMode.PercentOutput, -rightPower); leftBack.set(ControlMode.PercentOutput, leftPower); leftFront.set(ControlMode.PercentOutput, leftPower); - //DriveTrain.turnToAngle(90); } public static void arcadeDrive(double fwd, double tur) { @@ -59,7 +63,7 @@ public static void arcadeDrive(double fwd, double tur) { } public static void setHighGear(boolean b){ - sol.set(!b); + sol.set(b); } public static double getAngle(){ @@ -75,22 +79,35 @@ public static double getLeftEncoderDistance(){ return -(leftBack.getSelectedSensorPosition(0)); } - public static double getPIDError(){ - return pid.getError(); - } - public static void resetEncoder(){ leftBack.setSelectedSensorPosition(0, 0, 10); rightFront.setSelectedSensorPosition(0, 0, 10); } + public static void rampOut(){ + ramp.set(1); + } + + public static void rampIn(){ + ramp.set(.5); + } + + public static void tipIn(){ + tip.set(.5); + } + + public static void tipOut(){ + tip.set(0); + } public static double getEncoderAverage(){ + /* if(getLeftEncoderDistance() > getRightEncoderDistance()){ - return getRightEncoderDistance(); + return getLeftEncoderDistance(); } else + */ return getRightEncoderDistance(); } @@ -102,21 +119,21 @@ public static void turnToAngle(double angle){ } } + + public static void drivePID(double angle){ + + } public static void disablePID() { pid.disable(); } - public static void resetPID(){ - pid.reset(); - } - public void pidWrite(double output) { - if(Math.abs(pid.getError()) < 90d){ - pid.setPID(pid.getP(), pid.getI(), pid.getD()); + if(Math.abs(pid.getError()) < 5d){ + pid.setPID(pid.getP(), .001, 0); } else - pid.setPID(pid.getP(), 0.001, 0.0); - drive(output, -output); + pid.setPID(pid.getP(), 0, 0); + drive(output, -output); } public static void resetGyro(){ @@ -147,14 +164,13 @@ else if(getAngle() < -Constants.DT_DRIVE_STRAIGHT) } - - public static boolean isConnected(){ - return hyro.isConnected(); - } - - - public static double getRightPower(){ - return rightFront.getMotorOutputPercent(); + public static void pidDriveStraight(double power){ + pid.setSetpoint(0); + if(!pid.isEnabled()){ + pid.reset(); + pid.enable(); + drive(power, power); + } } public static double getPitch(){ @@ -168,6 +184,10 @@ public static void stop(){ leftFront.set(ControlMode.PercentOutput, 0); } + public static boolean isConnected(){ + return hyro.isConnected(); + } + public static void preventTip(){ while(Math.abs(DriveTrain.getRoll()) > 10){ if(DriveTrain.getRoll() > 10){ diff --git a/PowerUp/src/org/usfirst/frc/team548/robot/Elevator.java b/PowerUp/src/org/usfirst/frc/team548/robot/Elevator.java index 25ae406..9b27f61 100644 --- a/PowerUp/src/org/usfirst/frc/team548/robot/Elevator.java +++ b/PowerUp/src/org/usfirst/frc/team548/robot/Elevator.java @@ -16,7 +16,7 @@ public class Elevator { private static Elevator instance; private static DigitalInput bottomLimitSwitch, topLimitSwitch; private static PIDController pid; - private static DoubleSolenoid whip1; + private static DoubleSolenoid whip1, bars; @@ -30,28 +30,30 @@ public static Elevator getInstance(){ } private Elevator(){ - left = new TalonSRX(Constants.ELEVATOR_TALON_LEFT); // master - right = new TalonSRX(Constants.ELEVATOR_TALON_RGIHT); // slave + + + right = new TalonSRX(Constants.ELEVATOR_TALON_LEFT); // slave + left = new TalonSRX(Constants.ELEVATOR_TALON_RGIHT); // master bottomLimitSwitch = new DigitalInput(Constants.ELEVATOR_BOTTOM_LIMITSWITCH); topLimitSwitch = new DigitalInput(Constants.ELEVATOR_TOP_LIMITSWITCH); right.set(ControlMode.Follower, left.getDeviceID()); left.configSelectedFeedbackSensor(FeedbackDevice.CTRE_MagEncoder_Relative, 0, 10); - whip1 = new DoubleSolenoid(2, 3); + whip1 = new DoubleSolenoid(Constants.ELEVATOR_SOLENOID2, Constants.ELEVATOR_SOLENOID1); left.config_kP(0, Constants.ELEAVTOR_PID_P, 0); left.config_kI(0, Constants.ELEAVTOR_PID_I, 0); left.config_kD(0, Constants.ELEAVTOR_PID_D, 0); left.config_kF(0, Constants.ELEAVTOR_PID_F, 0); - left.configPeakOutputForward(.5, 10); - left.configPeakOutputReverse(-.5, 10); - - //left.setInverted(false); + bars = new DoubleSolenoid(6,7); //right.setInverted(true); left.setSensorPhase(true); + left.configPeakOutputForward(.5, 10); + left.configPeakOutputReverse(-.5, 10); } public static void setPower(double power){ + if(Math.abs(power) < .1) Elevator.stop(); else @@ -65,6 +67,14 @@ public static void setAlwaysPower(){ } */ + + public static void setOutputLimit(){ + left.configPeakOutputForward(1, 10); + left.configPeakOutputReverse(-1, 10); + } + + + public static double getPosition(){ return left.getSelectedSensorPosition(0); } @@ -129,6 +139,15 @@ public static void setElevatorIn(){ whip1.set(DoubleSolenoid.Value.kReverse); } + + public static void setBarsOut(){ + bars.set(DoubleSolenoid.Value.kForward); + } + + public static void setBarsIn(){ + bars.set(DoubleSolenoid.Value.kReverse); + } + public static boolean checkLimitSwitches(boolean limitSwitch){ if(limitSwitch && !limitSwitch){ //This means that there is something that has gone wrong diff --git a/PowerUp/src/org/usfirst/frc/team548/robot/Ingestor.java b/PowerUp/src/org/usfirst/frc/team548/robot/Ingestor.java index 6303e08..e864f94 100644 --- a/PowerUp/src/org/usfirst/frc/team548/robot/Ingestor.java +++ b/PowerUp/src/org/usfirst/frc/team548/robot/Ingestor.java @@ -4,6 +4,7 @@ import com.ctre.phoenix.motorcontrol.can.TalonSRX; import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.Solenoid; import edu.wpi.first.wpilibj.Timer; public class Ingestor { @@ -12,7 +13,8 @@ public class Ingestor { private static TalonSRX right, left; private static boolean currentLimiting = false, startedTimer = false; private static Timer currentTimer; - private static DoubleSolenoid ingestHer; + //private static Solenoid closeHeavy, closeLight; + private static DoubleSolenoid ingestSol; public static Ingestor getInstance(){ if(instance == null) @@ -23,7 +25,9 @@ public static Ingestor getInstance(){ private Ingestor(){ right = new TalonSRX(Constants.INGESTOR_TALON_RIGHT); left = new TalonSRX(Constants.INGESTOR_TALONG_LEFT); - ingestHer = new DoubleSolenoid(1, 0); + // closeHeavy = new Solenoid(Constants.INGESTOR_SOLENOID_CLOSE_HEAVY); + // closeLight = new Solenoid(4); + ingestSol = new DoubleSolenoid(4,0); currentTimer = new Timer(); } @@ -47,7 +51,7 @@ else if(currentTimer.get() < .15 && startedTimer){ } else{ right.set(ControlMode.PercentOutput, .25); - left.set(ControlMode.PercentOutput, .25); + left.set(ControlMode.PercentOutput, .7); if(Robot.PDP.getCurrent(Constants.INGESTOR_CURRENT_CONSTANT) < 1); currentLimiting = false; } @@ -58,35 +62,38 @@ public static void bothControl(double power){ stop(); } else{ - right.set(ControlMode.PercentOutput, -power); - left.set(ControlMode.PercentOutput, power); + right.set(ControlMode.PercentOutput, power); + left.set(ControlMode.PercentOutput, -power); } } - public static void stop(){ - right.set(ControlMode.PercentOutput, 0); - left.set(ControlMode.PercentOutput, 0); - } - public static void leftControl(double power){ - left.set(ControlMode.PercentOutput, power); - } public static void openIngest(){ - ingestHer.set(DoubleSolenoid.Value.kForward); + ingestSol.set(DoubleSolenoid.Value.kForward); } public static void closeIngestor(){ - ingestHer.set(DoubleSolenoid.Value.kReverse); + ingestSol.set(DoubleSolenoid.Value.kReverse); } - public static void rightControl(double power){ - right.set(ControlMode.PercentOutput, -power); + + + + public static void stop(){ + right.set(ControlMode.PercentOutput, 0); + left.set(ControlMode.PercentOutput, 0); } - public static boolean isCubeInIngestor() { - return currentLimiting; + public static void leftControl(double power){ + left.set(ControlMode.PercentOutput, -power); } + public static void rightControl(double power){ + right.set(ControlMode.PercentOutput, power); + } + public static boolean isGearInIngestor() { + return currentLimiting; + } } diff --git a/PowerUp/src/org/usfirst/frc/team548/robot/JSONConstants.java b/PowerUp/src/org/usfirst/frc/team548/robot/JSONConstants.java new file mode 100644 index 0000000..721b2ca --- /dev/null +++ b/PowerUp/src/org/usfirst/frc/team548/robot/JSONConstants.java @@ -0,0 +1,171 @@ +package frc.robot; + +import java.io.FileNotFoundException; +import java.io.FileReader; +import java.io.IOException; +import java.util.HashMap; +import java.util.Set; + +import org.json.simple.JSONObject; +import org.json.simple.parser.JSONParser; +import org.json.simple.parser.ParseException; + +/** + * + * @author Anurag Kompalli + * + * Method for saving, retrieving and updating config values + * without having to re-deploy code every time. Reads and Parses a JSON + * File and maps its keys and values to the configValues HashMap + * + * + * + * TODO: Add Error Handling + */ +public class JSONConstants { + + // configValues is the map in which the keys and values will be stored + private static HashMap configValues = new HashMap(); + + // defines the file path and name of the config file + private static final String FILE_PATH = "config/config.json"; + + /** + * Tester Class: Tests whether one is able to read and update the + * values within the configValues map + */ + public static void test() { + // Start the call timer + double firstPopulate = System.nanoTime(); + + // Run the function + int status = JSONConstants.populateMap(); + + // End the call timer + double firstPopulateEND = System.nanoTime(); + + // end minus start time is the duration the call took + System.out.println("First Population Call Took: " + ((firstPopulateEND - firstPopulate) / 1000000) + " ms"); + + // Start the second call timer + double secondPopulate = System.nanoTime(); + + // Run the function + int status2 = JSONConstants.populateMap(); + + // End the call timer + double secondPopulateEND = System.nanoTime(); + + // What did we see? + System.out.println("Second Population Call Took: " + ((secondPopulateEND - secondPopulate) / 1000000) + " ms"); + System.out.println("First populateMap() returned: " + status); + System.out.println("Second populateMap() returned: " + status2); + } + + /** + * The populateMap() method takes a JSON file and reads + * the keys and values. + * + * Uses these keys and values to populate the configValues HashMap with the + * relevant values needed for the match + */ + public static int populateMap() { + + // Defines a JSON parser + JSONParser parser = new JSONParser( + + ); + + try { + // Open and Assign the file to a JSON Object + Object fileObj = parser.parse(new FileReader(FILE_PATH)); + JSONObject configJSONObj = (JSONObject) fileObj; + + // Get the key set for the JSON Object + Set entries = configJSONObj.keySet(); + + // Make sure there isn't any leftover values in the map before we write + System.out.println("====== CLEARING HASHMAP ======"); + configValues.clear(); + + // Foreach loop to put each key and value in the HashMap + for (String entry : entries) { + configValues.put(entry, configJSONObj.get(entry)); + } + + // Make sure we tell the user what keys we have added (will also show up on + // SmartDashboard) + for (String key : configValues.keySet()) { + System.out.println("Added KEY: [" + key + "] and VALUE: [" + configValues.get(key) + "] " + + "Value is of TYPE: " + configValues.get(key).getClass()); + } + + } catch (FileNotFoundException notFoundEx) { + + // System did not find the file in the specified path + // Check if you defined the file correctly (spelling and path) + System.err.println("The file " + FILE_PATH + " was not found at the specified path"); + notFoundEx.printStackTrace(); + + // return FAIL + return -1; + } catch (IOException ioe) { + + // System was not able to perform I/O Operations + // Check if the file is marked as read only or is protected in any way + // (encrypted) + System.err.println("An IOException Occured"); + ioe.printStackTrace(); + + // return FAIL + return -1; + } catch (ParseException pe) { + + // JSON Simple was not able to parse the JSON + // Make sure the file extension is actually .json and json.simple jar is not + // corrupted + System.err.println("JSON Simple was not able to parse the file: " + FILE_PATH); + pe.printStackTrace(); + + // return FAIL + return -1; + } + + // return SUCCESS + return 0; + } + + /* + * The get() method allows easy access to the configValues HashMap without + * providing direct access and does a bunch of checks before delivering the + * value + * + * @return Object + */ + public static Object get(String key) { + // Making sure there's something there to search for + if (configValues.size() > 0) { + + // get the value from the configValues HashMap + Object value = configValues.get(key); + + // making sure object is there + if (value != null) { + + // Safely Converts a Long to an Integer + // if (value instanceof Long) { + // return ((Long) value).intValue(); + // } + + // return the value as a generic Object (user can cast as necessary) + return value; + } + + // if not, return -1 (fail) + return -1; + } + + // In case size is 0, we return FAIL + return -1; + } +} diff --git a/PowerUp/src/org/usfirst/frc/team548/robot/Robot.java b/PowerUp/src/org/usfirst/frc/team548/robot/Robot.java index e242fd8..8e73f21 100644 --- a/PowerUp/src/org/usfirst/frc/team548/robot/Robot.java +++ b/PowerUp/src/org/usfirst/frc/team548/robot/Robot.java @@ -7,27 +7,39 @@ package org.usfirst.frc.team548.robot; -import org.opencv.core.Mat; -import org.opencv.imgproc.Imgproc; +import com.kauailabs.navx.frc.AHRS; +import AutoModes.AutoLine; import AutoModes.AutoMode; import AutoModes.DriveStraight; -import AutoModes.LeftGoAround; +import AutoModes.RightSwitchRightScale; import AutoModes.SwitchAuto; import AutoModes.SwitchAuto2; -import AutoModes.TestStuff; -import edu.wpi.cscore.CvSink; -import edu.wpi.cscore.CvSource; -import edu.wpi.cscore.UsbCamera; -import edu.wpi.first.wpilibj.CameraServer; -import edu.wpi.first.wpilibj.DriverStation; +import AutoModes.SwitchPriorityLeft; +import AutoModes.SwitchPriorityRight; +import AutoModes.LeftScaleSwitchOutside; +import AutoModes.RightScaleSwitchOutside; +import AutoModes.RightCross; +import AutoModes.LeftCross; +//import AutoModes.Beesswervecode; +//import AutoModes.BeesPID; +//import AutoModes.5cubeauto; +//import edu.wpi.first.wpilibj.CameraServer; import edu.wpi.first.wpilibj.IterativeRobot; import edu.wpi.first.wpilibj.PowerDistributionPanel; +import edu.wpi.first.wpilibj.SerialPort; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; public class Robot extends IterativeRobot { + public Robot(){ + + AHRS ahrs = new AHRS(SerialPort.Port.kUSB); + /* Alternatives: SPI.Port.kMXP, I2C.Port.kMXP or SerialPort.Port.kUSB */ + + } + AutoMode autonCommand; SendableChooser autoChooser; @@ -40,43 +52,45 @@ public void robotInit() { Elevator.getInstance(); Ingestor.getInstance(); Climber.getInstance(); + //USBLED.getInstance(); + //CameraServer.getInstance().startAutomaticCapture(); PDP = new PowerDistributionPanel(); autoChooser = new SendableChooser(); - autoChooser.addDefault("Decision Middle", new DriveStraight()); - autoChooser.addDefault("left goaround",new LeftGoAround()); + autoChooser.addDefault("Center Switch", new DriveStraight()); + autoChooser.addDefault("Cross Baseline", new AutoLine()); + //autoChooser.addDefault("Scale/Switch Left", new SwitchAuto1()); autoChooser.addDefault("Scale/Switch Left", new SwitchAuto()); - autoChooser.addDefault("Scale/Switch Right", new SwitchAuto2()); - autoChooser.addDefault("Test Stuff", new TestStuff()); - try{ - UsbCamera camera = CameraServer.getInstance().startAutomaticCapture(); - camera.setResolution(480, 320); - SmartDashboard.putNumber("Brightness", camera.getBrightness()); - }catch (Exception e){ - e.printStackTrace(); - } - + autoChooser.addDefault("Scale/Switch Right", new RightSwitchRightScale()); + autoChooser.addDefault("Switch/Scale Left", new SwitchPriorityLeft()); + autoChooser.addDefault("Switch/Scale Right", new SwitchPriorityRight()); + autoChooser.addDefault("Left Scale for alliance two cube/switch", new LeftScaleSwitchOutside()); + autoChooser.addDefault("Right Scale for alliance two cube/switch", new RightScaleSwitchOutside()); + autoChooser.addDefault("Cross Scale Right", new RightCross()); + autoChooser.addDefault("Cross Scale Left", new LeftCross()); + autoChooser.addDefault("Pick your auto", new SwitchAuto2()); SmartDashboard.putData("Auto mode", autoChooser); - //SmartDashboard.putNumber("Match Time:", DriverStation.getInstance().getMatchTime()); - + //SmartDashboard.putData("Is Gyro Connected?", DriveTrain.isConnected()); + SmartDashboard.putBoolean("Gyro Connection", DriveTrain.isConnected()); + //SmarasdftDashboard.putNumber("Match Time:", DriverStation.getInstance().getMatchTime()); + DriveTrain.resetGyro(); + DriveTrain.resetEncoder(); } - - - @Override public void autonomousInit() { - DriveTrain.resetGyro(); - DriveTrain.resetEncoder(); DriveTrain.resetGyro(); autoChooser.getSelected().start(); + Ingestor.openIngest(); } @Override public void autonomousPeriodic() { SmartDashboard.putNumber("Drive Train", DriveTrain.getEncoderAverage()); SmartDashboard.putNumber("AUTO GYRO", DriveTrain.getAngle()); - //SmartDashboard.putNumber("PID Error", DriveTrain.getPIDError()); + System.out.println("Gyro: " + DriveTrain.isConnected()); + System.out.println(DriveTrain.isConnected()); + //SmartDashboard.putBoolean("Is Gyro Connected", DriveTrain.isConnected()); } @Override diff --git a/PowerUp/src/org/usfirst/frc/team548/robot/TeleOp.java b/PowerUp/src/org/usfirst/frc/team548/robot/TeleOp.java index 39a5847..619f3b9 100644 --- a/PowerUp/src/org/usfirst/frc/team548/robot/TeleOp.java +++ b/PowerUp/src/org/usfirst/frc/team548/robot/TeleOp.java @@ -5,7 +5,7 @@ public class TeleOp { private static XBoxController driver, manip; private static TeleOp instance; - + public static TeleOp getInstance() { if (instance == null) instance = new TeleOp(); @@ -25,62 +25,40 @@ public static void init(){ public static void run(){ //DriveTrain.preventTip(); + Elevator.setOutputLimit(); //Driver DriveTrain.arcadeDrive(driver.getRightStickYAxis(), Utils.negPowTwo(driver.getLeftStickXAxis())); + //Two Drivers + if(driver.getRightBumper()){ - DriveTrain.setHighGear(false); + DriveTrain.setHighGear(true); } else - DriveTrain.setHighGear(true); - - //Manip - - - + DriveTrain.setHighGear(false); + //Manip if(manip.getRightBumper()) Climber.climb(manip.getRightStickYAxis()); - else{ + else { Elevator.setPower(manip.getRightStickYAxis()); Climber.climb(0); } + //gkj - /* - if(manip.getLeftStickYAxis() > .5){ - Ingestor.ingestCurentLimiting(); - if (Ingestor.isCubeInIngestor()) { - manip.setLeftRumble(1); - driver.setLeftRumble(1); - - } else { - manip.setLeftRumble(0); - driver.setLeftRumble(0); - } - } - */ - if(driver.getAButton()) - DriveTrain.resetEncoder(); - if(driver.getBButton()) - DriveTrain.resetGyro(); - if(driver.getXButton()) - Elevator.resetEncoder();; - if(manip.getYButton()) - Elevator.setElevatorIn(); - else - Elevator.setElevatorOut(); + if(manip.getLeftBumper()) Ingestor.closeIngestor(); - else + else{ Ingestor.openIngest(); + } + if(manip.getYButton()) + Elevator.setElevatorIn(); + else + Elevator.setElevatorOut(); - //else //Jason uncommented lines 56 and 57 on 3/10/18 after kurt left because elevator wasn't unfolding. - // Elevator.setElevatorOut(); - //else - // Elevator.setElevatorOut(); - //Ingestor.ingestCurentLimiting(); @@ -98,33 +76,87 @@ else if(manip.getLeftJoystickButton()){ } else Ingestor.bothControl(-manip.getLeftStickYAxis()); - - //Ingestor.ingestCurentLimiting(); - if(Ingestor.isCubeInIngestor()){ - manip.setLeftRumble(1); - driver.setLeftRumble(1); - } - else{ - manip.setLeftRumble(0); - driver.setLeftRumble(0); - } - //asdfj} - if(driver.getBButton()){ + + if(manip.getBButton()) + Elevator.setBarsOut(); + else + Elevator.setBarsIn(); + + + if(driver.getAButton()){ Elevator.resetEncoder(); } if(driver.getAButton()){ - - Elevator.setPosition(55000); + Elevator.setPosition(20000); } - //Ingestor.rightControl(manip.getRightTriggerAxis()); - //Ingestor.leftControl(manip.getLeftTriggerAxis()); - - //SmartDashboard + //one Driver + +/* if(driver.getYButton()){ +* DriveTrain.setHighGear(true); +* } +* else +* DriveTrain.setHighGear(false); +* +* +* +* if(driver.getXButton()) +* Climber.climb(driver.getRightStickYAxis()); +* else { +* Elevator.setPower(driver.getRightTriggerAxis()-driver.getLeftTriggerAxis()); +* Climber.climb(0); +* } +* //gkj +* +* +* +* if(driver.getLeftJoystickButton()) +* Ingestor.closeIngestor(); +* else{ +* Ingestor.openIngest(); +* } +* +* +* if(driver.getStartButton()) +* Elevator.setElevatorIn(); +* else +* Elevator.setElevatorOut(); +* +* +* +* +* if(driver.getRightBumper()){ +* Ingestor.bothControl(-.7); +* } +* else if(driver.getLeftBumper()){ +* Ingestor.bothControl(.7); +* } +* else +* Ingestor.bothControl(0.1); +*/ + + + + /*if(manip.getBButton()) + Elevator.setBarsOut(); + else + Elevator.setBarsIn(); + + + if(driver.getAButton()){ + Elevator.resetEncoder(); + } + + if(driver.getAButton()){ + Elevator.setPosition(20000); + } + */ + //SmartDashboard + SmartDashboard.putNumber("DT Gryo", DriveTrain.getAngle()); SmartDashboard.putNumber("DT Encoder", DriveTrain.getEncoderAverage()); //SmartDashboard.putNumber("Pitch", DriveTrain.getRoll()); SmartDashboard.putNumber("eleavtor encoder", Elevator.getPosition()); @@ -132,8 +164,8 @@ else if(manip.getLeftJoystickButton()){ //SmartDashboard.putBoolean("Limit Switch Error", Elevator.checkLimitSwitches(Elevator.getBottomLimitSwitch())); SmartDashboard.putBoolean("Switch", Elevator.getBottomLimitSwitch()); SmartDashboard.putNumber("xbox", manip.getLeftStickYAxis()); - SmartDashboard.putNumber("Elevator power", Elevator.getAmountPower()); - SmartDashboard.putBoolean("Gyro", DriveTrain.isConnected()); + SmartDashboard.putNumber("Elevator power", Elevator.getAmountPower()); + SmartDashboard.putBoolean("GYRO", DriveTrain.isConnected()); } } diff --git a/PowerUp/src/org/usfirst/frc/team548/robot/USBLED.java b/PowerUp/src/org/usfirst/frc/team548/robot/USBLED.java new file mode 100644 index 0000000..37174b8 --- /dev/null +++ b/PowerUp/src/org/usfirst/frc/team548/robot/USBLED.java @@ -0,0 +1,149 @@ +package org.usfirst.frc.team548.robot; + +import java.util.Arrays; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.SerialPort; +import edu.wpi.first.wpilibj.DriverStation.Alliance; + +/* + * See https://docs.google.com/spreadsheets/d/1tdbu5kxECmHi2MyT0dEW17oEaT4cRJXpTU9CM4r0fAo/edit?usp=sharing for bit reference + * BIT ZERO IS LEAST SIGNIFICANT BIT, LIKE ARDUINO + */ +public class USBLED { + private static short elevatorPosition; + private static short oldPosition; + private static USBLED instance = null; + private static SerialPort serial; + private static byte status; + private static boolean[] statusBools; + private static boolean[] oldBools; + private USBLED() { + try { + serial = new SerialPort(115200, SerialPort.Port.kUSB1); + } catch (Exception e) { + e.printStackTrace(); + } + status = 0; + statusBools = new boolean[8]; + oldBools = new boolean[8]; + elevatorPosition=0; + oldPosition=0; + new Thread(new Runnable() { + public void run() { + while (true) { + try { + + sync(); + //System.out.println("memes"); + Thread.sleep(1000); + } catch (Exception e) { + e.printStackTrace(); + } + } + } + }).start(); + } + + public static USBLED getInstance() { + if (instance == null) { + instance = new USBLED(); + } + return instance; + } + + private static void updateByte() { + status = (byte) ((statusBools[7] ? 1 << 7 : 0) + (statusBools[6] ? 1 << 6 : 0) + (statusBools[5] ? 1 << 5 : 0) + + (statusBools[4] ? 1 << 4 : 0) + (statusBools[3] ? 1 << 3 : 0) + (statusBools[2] ? 1 << 2 : 0) + + (statusBools[1] ? 1 << 1 : 0) + (statusBools[0] ? 1 : 0)); + } + + private static void sync() { + //System.out.println(serial.readString()); + updateDsValues(); + updateByte(); + try { + for (int i = 0; i < 5; i++){ + serial.write(new byte[] {(byte)0x0f,(byte) 0xf0, status,(byte)(elevatorPosition&0xff),(byte)((elevatorPosition>>8)&0xff) }, 5); + } + //System.out.println(Arrays.toString(statusBools)); + } catch (Exception e) { + //e.printStackTrace(); + try { + serial = new SerialPort(9600, SerialPort.Port.kUSB); + } catch (Exception ex) { + //ex.printStackTrace(); + } + //e.printStackTrace(); + } + } + + private static void updateDsValues() { + if (DriverStation.getInstance().isBrownedOut()) { + statusBools[0] = true; + statusBools[1] = true; + } else if (!DriverStation.getInstance().isDSAttached() + || DriverStation.getInstance().getAlliance() == Alliance.Invalid) { + statusBools[0] = false; + statusBools[1] = false; + } else if (DriverStation.getInstance().getAlliance() == Alliance.Red) { + statusBools[0] = false; + statusBools[1] = true; + } else { + statusBools[0] = true; + statusBools[1] = false; + } + if (DriverStation.getInstance().isAutonomous()&&DriverStation.getInstance().isEnabled()) { + statusBools[2] = true; + statusBools[3] = false; + } else if (DriverStation.getInstance().isOperatorControl()&&DriverStation.getInstance().isEnabled()) { + statusBools[2] = true; + statusBools[3] = true; + } else if (DriverStation.getInstance().isFMSAttached()) { + statusBools[2] = false; + statusBools[3] = true; + } else { + statusBools[2] = false; + statusBools[3] = false; + } + } + + public static void isHasCube(boolean hasCube) { + statusBools[4] = hasCube; + if(statusBools[4]!=oldBools[4]){ + sync(); + oldBools[4]=statusBools[4]; + } + } + + public static void isWantCube(boolean wantCube) { + statusBools[5] = !wantCube; + if(statusBools[5]!=oldBools[5]){ + sync(); + oldBools[5]=statusBools[5]; + } + } + + public static void isWombo(boolean wombo) { + statusBools[6] = wombo; + if(statusBools[6]!=oldBools[6]){ + sync(); + oldBools[6]=statusBools[6]; + } + } + + public static void isClimbing(boolean rft) { + statusBools[7] = rft; + if(statusBools[7]!=oldBools[7]){ + sync(); + oldBools[7]=statusBools[7]; + } + } + public void setElevatorPosition(short elevatorPos) { + elevatorPosition = elevatorPos; + if(elevatorPosition!=oldPosition){ + sync(); + oldPosition=elevatorPosition; + } + } +} \ No newline at end of file diff --git a/PowerUp/src/org/usfirst/frc/team548/robot/XBoxController.java b/PowerUp/src/org/usfirst/frc/team548/robot/XBoxController.java index ab78de0..7345e95 100644 --- a/PowerUp/src/org/usfirst/frc/team548/robot/XBoxController.java +++ b/PowerUp/src/org/usfirst/frc/team548/robot/XBoxController.java @@ -125,4 +125,11 @@ public double getTriggers() { return (getRawAxis(3)-getRawAxis(2)); } + public boolean isButtonClicked(){ + if(this.getLeftBumper()){ + return true; + }else{ + return false; + } + } } \ No newline at end of file