diff --git a/build/jars/CTRLib.jar b/build/jars/CTRLib.jar deleted file mode 100644 index 238f8c4..0000000 Binary files a/build/jars/CTRLib.jar and /dev/null differ diff --git a/build/jars/Pathfinder-Java-1.8.jar b/build/jars/Pathfinder-Java-1.8.jar new file mode 100644 index 0000000..9054e2b Binary files /dev/null and b/build/jars/Pathfinder-Java-1.8.jar differ diff --git a/build/org/usfirst/frc/team1160/robot/OI.class b/build/org/usfirst/frc/team1160/robot/OI.class index 890c4fa..1384c91 100644 Binary files a/build/org/usfirst/frc/team1160/robot/OI.class and b/build/org/usfirst/frc/team1160/robot/OI.class differ diff --git a/build/org/usfirst/frc/team1160/robot/Robot.class b/build/org/usfirst/frc/team1160/robot/Robot.class index 4d38452..10963b0 100644 Binary files a/build/org/usfirst/frc/team1160/robot/Robot.class and b/build/org/usfirst/frc/team1160/robot/Robot.class differ diff --git a/build/org/usfirst/frc/team1160/robot/RobotMap.class b/build/org/usfirst/frc/team1160/robot/RobotMap.class index d17cd19..0dc4450 100644 Binary files a/build/org/usfirst/frc/team1160/robot/RobotMap.class and b/build/org/usfirst/frc/team1160/robot/RobotMap.class differ diff --git a/build/org/usfirst/frc/team1160/robot/TrajectoryWaypoints.class b/build/org/usfirst/frc/team1160/robot/TrajectoryWaypoints.class index 26ae586..ae6dfb7 100644 Binary files a/build/org/usfirst/frc/team1160/robot/TrajectoryWaypoints.class and b/build/org/usfirst/frc/team1160/robot/TrajectoryWaypoints.class differ diff --git a/build/org/usfirst/frc/team1160/robot/commands/auto/paths/Center_RightSwitch.class b/build/org/usfirst/frc/team1160/robot/commands/auto/paths/Center_RightSwitch.class index 986d9be..30edb38 100644 Binary files a/build/org/usfirst/frc/team1160/robot/commands/auto/paths/Center_RightSwitch.class and b/build/org/usfirst/frc/team1160/robot/commands/auto/paths/Center_RightSwitch.class differ diff --git a/build/org/usfirst/frc/team1160/robot/subsystems/DriveTrain.class b/build/org/usfirst/frc/team1160/robot/subsystems/DriveTrain.class index d62bbd5..328be47 100644 Binary files a/build/org/usfirst/frc/team1160/robot/subsystems/DriveTrain.class and b/build/org/usfirst/frc/team1160/robot/subsystems/DriveTrain.class differ diff --git a/src/org/usfirst/frc/team1160/robot/OI.java b/src/org/usfirst/frc/team1160/robot/OI.java index f4b303e..9c454c0 100644 --- a/src/org/usfirst/frc/team1160/robot/OI.java +++ b/src/org/usfirst/frc/team1160/robot/OI.java @@ -121,8 +121,8 @@ private void tieButtons() { highGear.whenPressed(new HighGear()); lowGear.whenPressed(new LowGear()); - intakeEat.whileHeld(new IntakeRotate(1)); - intakeSpit.whileHeld(new IntakeRotate(-1)); + intakeEat.whileHeld(new IntakeRotate(0.7)); + intakeSpit.whileHeld(new IntakeRotate(-0.7)); extendClimber.whenPressed(new LatchExtend()); retractClimber.whenPressed(new LatchRetract()); diff --git a/src/org/usfirst/frc/team1160/robot/Robot.java b/src/org/usfirst/frc/team1160/robot/Robot.java index f54b8c8..9c6d9db 100644 --- a/src/org/usfirst/frc/team1160/robot/Robot.java +++ b/src/org/usfirst/frc/team1160/robot/Robot.java @@ -90,7 +90,8 @@ public void robotInit() { * Center to Left Switch Backwards: 4 * Center to Right Switch: 5 */ - saveTrajectoriesAll(); + //saveTrajectoriesAll(); + autonomousCommand = new TurnAngle(90); } @@ -119,7 +120,9 @@ public void generateSegments(int choice) { //let's be even more honest, you shou //autonomousCommand = new Center_LeftSwitchBW(); break; case 5: //Center to Right Switch - segment_one = dt.generateTrajectorySetup(CENTER_RIGHT_SWITCH); + segment_one = dt.generateTrajectorySetup(CENTER_RIGHT_SWITCH_1); + segment_two = dt.generateTrajectorySetup(CENTER_RIGHT_SWITCH_2); + segment_three = dt.generateTrajectorySetup(CENTER_RIGHT_SWITCH_3); autonomousCommand = new Center_RightSwitch(); break; case 6: //Custom @@ -139,25 +142,29 @@ public void saveTrajectories(int choice) { //let's be honest, you should only ha switch (choice) { case 1: generateSegments(1); - file_one = new File("CENTER_LEFT_SWITCH_1.csv"); - Pathfinder.writeToCSV(file_one, segment_one); - file_one = new File("CENTER_LEFT_SWITCH_2.csv"); - Pathfinder.writeToCSV(file_one, segment_two); - file_one = new File("CENTER_LEFT_SWITCH_3.csv"); - Pathfinder.writeToCSV(file_one, segment_three); + file_one = new File("CENTER_LEFT_SWITCH_1.traj"); + Pathfinder.writeToFile(file_one, segment_one); + file_one = new File("CENTER_LEFT_SWITCH_2.traj"); + Pathfinder.writeToFile(file_one, segment_two); + file_one = new File("CENTER_LEFT_SWITCH_3.traj"); + Pathfinder.writeToFile(file_one, segment_three); break; case 2: case 3: generateSegments(2); - file_one = new File("X_X_SWITCH_1.csv"); - Pathfinder.writeToCSV(file_one, segment_one); - file_one = new File("X_X_SWITCH_2.csv"); - Pathfinder.writeToCSV(file_one, segment_two); + file_one = new File("X_X_SWITCH_1.traj"); + Pathfinder.writeToFile(file_one, segment_one); + file_one = new File("X_X_SWITCH_2.traj"); + Pathfinder.writeToFile(file_one, segment_two); break; case 5: generateSegments(5); - file_one = new File("CENTER_RIGHT_SWITCH.csv"); - Pathfinder.writeToCSV(file_one, segment_one); + file_one = new File("CENTER_RIGHT_SWITCH_1.traj"); + Pathfinder.writeToFile(file_one, segment_one); + file_one = new File("CENTER_RIGHT_SWITCH_2.traj"); + Pathfinder.writeToFile(file_one, segment_two); + file_one = new File("CENTER_RIGHT_SWITCH_3.traj"); + Pathfinder.writeToFile(file_one, segment_three); break; default: System.out.println("Hold this even bigger L"); @@ -171,20 +178,29 @@ public void saveTrajectoriesAll() { //Kobe: "Just save them all" File file_one; generateSegments(1); - file_one = new File("CENTER_LEFT_SWITCH_1.csv"); - Pathfinder.writeToCSV(file_one, segment_one); - file_one = new File("CENTER_LEFT_SWITCH_2.csv"); - Pathfinder.writeToCSV(file_one, segment_two); - file_one = new File("CENTER_LEFT_SWITCH_3.csv"); - Pathfinder.writeToCSV(file_one, segment_three); + //file_one = new File("CENTER_LEFT_SWITCH_1.traj"); + file_one = new File("CLS1.traj"); + System.out.println("Attempted to save center left switch 1"); + Pathfinder.writeToFile(file_one, segment_one); + System.out.println("Successfully saved center left switch 1"); + file_one = new File("CENTER_LEFT_SWITCH_2.traj"); + Pathfinder.writeToFile(file_one, segment_two); + file_one = new File("CENTER_LEFT_SWITCH_3.traj"); + Pathfinder.writeToFile(file_one, segment_three); generateSegments(2); - file_one = new File("X_X_SWITCH_1.csv"); - Pathfinder.writeToCSV(file_one, segment_one); - file_one = new File("X_X_SWITCH_2.csv"); - Pathfinder.writeToCSV(file_one, segment_two); + file_one = new File("X_X_SWITCH_1.traj"); + Pathfinder.writeToFile(file_one, segment_one); + file_one = new File("X_X_SWITCH_2.traj"); + Pathfinder.writeToFile(file_one, segment_two); generateSegments(5); - file_one = new File("CENTER_RIGHT_SWITCH.csv"); - Pathfinder.writeToCSV(file_one, segment_one); + file_one = new File("CENTER_RIGHT_SWITCH_1.traj"); + Pathfinder.writeToFile(file_one, segment_one); + file_one = new File("CENTER_RIGHT_SWITCH_2.traj"); + Pathfinder.writeToFile(file_one, segment_two); + file_one = new File("CENTER_RIGHT_SWITCH_3.traj"); + Pathfinder.writeToFile(file_one, segment_three); + + System.out.println("All paths saved to csv"); } public void loadTrajectories(int choice) { //now this is the good stuff, this is what you wanna run 25/7 @@ -192,31 +208,35 @@ public void loadTrajectories(int choice) { //now this is the good stuff, this is switch (choice) { case 1: - file_one = new File("CENTER_LEFT_SWITCH_1.csv"); - segment_one = Pathfinder.readFromCSV(file_one); - file_one = new File("CENTER_LEFT_SWITCH_2.csv"); - segment_two = Pathfinder.readFromCSV(file_one); - file_one = new File("CENTER_LEFT_SWITCH_3.csv"); - segment_three = Pathfinder.readFromCSV(file_one); + file_one = new File("CENTER_LEFT_SWITCH_1.traj"); + segment_one = Pathfinder.readFromFile(file_one); + file_one = new File("CENTER_LEFT_SWITCH_2.traj"); + segment_two = Pathfinder.readFromFile(file_one); + file_one = new File("CENTER_LEFT_SWITCH_3.traj"); + segment_three = Pathfinder.readFromFile(file_one); autonomousCommand = new Center_LeftSwitch(); break; case 2: - file_one = new File("X_X_SWITCH_1.csv"); - segment_one = Pathfinder.readFromCSV(file_one); - file_one = new File("X_X_SWITCH_2.csv"); - segment_two = Pathfinder.readFromCSV(file_one); + file_one = new File("X_X_SWITCH_1.traj"); + segment_one = Pathfinder.readFromFile(file_one); + file_one = new File("X_X_SWITCH_2.traj"); + segment_two = Pathfinder.readFromFile(file_one); autonomousCommand = new Left_LeftSwitch(); break; case 3: - file_one = new File("X_X_SWITCH_1.csv"); - segment_one = Pathfinder.readFromCSV(file_one); - file_one = new File("X_X_SWITCH_2.csv"); - segment_two = Pathfinder.readFromCSV(file_one); + file_one = new File("X_X_SWITCH_1.traj"); + segment_one = Pathfinder.readFromFile(file_one); + file_one = new File("X_X_SWITCH_2.traj"); + segment_two = Pathfinder.readFromFile(file_one); autonomousCommand = new Right_RightSwitch(); break; case 5: - file_one = new File("CENTER_RIGHT_SWITCH.csv"); - segment_one = Pathfinder.readFromCSV(file_one); + file_one = new File("CENTER_RIGHT_SWITCH_1.traj"); + segment_one = Pathfinder.readFromFile(file_one); + file_one = new File("CENTER_RIGHT_SWITCH_2.traj"); + segment_two = Pathfinder.readFromFile(file_one); + file_one = new File("CENTER_RIGHT_SWITCH_3.traj"); + segment_three = Pathfinder.readFromFile(file_one); autonomousCommand = new Center_RightSwitch(); break; default: diff --git a/src/org/usfirst/frc/team1160/robot/TrajectoryWaypoints.java b/src/org/usfirst/frc/team1160/robot/TrajectoryWaypoints.java index 570d1ae..8a20862 100644 --- a/src/org/usfirst/frc/team1160/robot/TrajectoryWaypoints.java +++ b/src/org/usfirst/frc/team1160/robot/TrajectoryWaypoints.java @@ -23,11 +23,11 @@ public interface TrajectoryWaypoints { //From the center to the left switch public static final Waypoint[] CENTER_LEFT_SWITCH_1 = - new Waypoint[] {new Waypoint(0,0,0),new Waypoint(3,0,0)}; + new Waypoint[] {new Waypoint(0,0,0),new Waypoint(2,0,0)}; public static final Waypoint[] CENTER_LEFT_SWITCH_2 = - new Waypoint[] {new Waypoint(0,0,0),new Waypoint(40*Math.pow(2, 0.5)/12,0,0)}; + new Waypoint[] {new Waypoint(0,0,0),new Waypoint(5.0*Math.pow(2, 0.5),0,0)}; public static final Waypoint[] CENTER_LEFT_SWITCH_3 = - new Waypoint[] {new Waypoint(0,0,0),new Waypoint(64/12,0,0)}; + new Waypoint[] {new Waypoint(0,0,0),new Waypoint(2.0,0,0)}; //From the center to the left switch BACKWARDS public static final Waypoint[] CENTER_LEFT_SWITCH_1_BACKWARDS = @@ -38,14 +38,23 @@ public interface TrajectoryWaypoints { new Waypoint[] {new Waypoint(0,0,0),new Waypoint(-64/12,0,0)}; //From the center to the right switch + + /* THIS IS FROM PRE-THURSDAY LV CODE public static final Waypoint[] CENTER_RIGHT_SWITCH = new Waypoint[] {new Waypoint(0,0,0),new Waypoint(140/12,0,0)}; + */ + public static final Waypoint[] CENTER_RIGHT_SWITCH_1 = + new Waypoint[] {new Waypoint(0,0,0),new Waypoint(2,0,0)}; + public static final Waypoint[] CENTER_RIGHT_SWITCH_2 = + new Waypoint[] {new Waypoint(0,0,0),new Waypoint(4.5*Math.pow(2, 0.5),0,0)}; + public static final Waypoint[] CENTER_RIGHT_SWITCH_3 = + new Waypoint[] {new Waypoint(0,0,0),new Waypoint(2,0,0)}; //From the X side to the X switch where X is either left or right public static final Waypoint[] X_X_SWITCH_1 = - new Waypoint[] {new Waypoint(0,0,0),new Waypoint(168.0/12.0,0,0)}; + new Waypoint[] {new Waypoint(0,0,0),new Waypoint(12.4,0,0)}; public static final Waypoint[] X_X_SWITCH_2 = - new Waypoint[] {new Waypoint(0,0,0),new Waypoint(42.0/12.0,0,0)}; + new Waypoint[] {new Waypoint(0,0,0),new Waypoint(1.5,0,0)}; //Straight line from start to switch public static final Waypoint[] START_SWITCH = diff --git a/src/org/usfirst/frc/team1160/robot/commands/auto/paths/Center_RightSwitch.java b/src/org/usfirst/frc/team1160/robot/commands/auto/paths/Center_RightSwitch.java index 6d33976..3dae85d 100644 --- a/src/org/usfirst/frc/team1160/robot/commands/auto/paths/Center_RightSwitch.java +++ b/src/org/usfirst/frc/team1160/robot/commands/auto/paths/Center_RightSwitch.java @@ -5,6 +5,7 @@ import org.usfirst.frc.team1160.robot.commands.auto.drive.FollowTrajectory; import org.usfirst.frc.team1160.robot.commands.auto.drive.TurnAngle; import org.usfirst.frc.team1160.robot.commands.auto.intake.AutoBoxSpit; +import org.usfirst.frc.team1160.robot.commands.auto.lift.BangBangMove; import org.usfirst.frc.team1160.robot.commands.intake.IntakeExtend; import org.usfirst.frc.team1160.robot.commands.intake.Toggle; @@ -19,6 +20,13 @@ public Center_RightSwitch() { addSequential(new IntakeExtend()); addSequential(new ResetEncoderYaw()); addSequential(new FollowTrajectory(Robot.segment_one)); + addSequential(new TurnAngle(45)); + addSequential(new ResetEncoderYaw()); + addSequential(new FollowTrajectory(Robot.segment_two)); + addSequential(new TurnAngle(-45)); + addSequential(new BangBangMove()); + addSequential(new ResetEncoderYaw()); + addSequential(new FollowTrajectory(Robot.segment_three)); addSequential(new AutoBoxSpit()); //and then now we manipulate the lift