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