From 308d7f343658e09a92496e661a955272469240f5 Mon Sep 17 00:00:00 2001 From: Luke Staudacher Date: Sun, 11 Mar 2018 19:36:43 -0500 Subject: [PATCH] mid final changes --- src/org/usfirst/frc4388/robot/Robot.java | 8 ++++---- .../robot/commands/auton/CenterLeft.java | 10 ++++++---- .../robot/commands/auton/CenterRight.java | 1 + .../commands/auton/LeftStartRightScore.java | 11 +++++++++-- .../robot/commands/auton/LeftSwitchAuton.java | 8 +++++++- .../commands/auton/RightStartLeftScore.java | 10 +++++++++- .../robot/commands/auton/RightSwitchAuton.java | 9 ++++++--- .../robot/commands/auton/ScaleFrom1.java | 17 ++++++++++++----- .../robot/commands/auton/ScaleFrom3.java | 1 + .../frc4388/robot/subsystems/Climber.java | 2 +- 10 files changed, 56 insertions(+), 21 deletions(-) diff --git a/src/org/usfirst/frc4388/robot/Robot.java b/src/org/usfirst/frc4388/robot/Robot.java index ae18c00..2f6a904 100644 --- a/src/org/usfirst/frc4388/robot/Robot.java +++ b/src/org/usfirst/frc4388/robot/Robot.java @@ -127,7 +127,7 @@ public class Robot extends IterativeRobot RRautonTaskChooser.addObject("Left to Right Switch", new LeftStartRightScore()); RRautonTaskChooser.addObject("Center to Right Switch", new CenterRight()); - RRautonTaskChooser.addObject("Center to Right 2 cube", new CenterRight2Cube()); + //RRautonTaskChooser.addObject("Center to Right 2 cube", new CenterRight2Cube()); RRautonTaskChooser.addObject("Right to Right Switch", new RightSwitchAuton()); RRautonTaskChooser.addObject("Right to Right Scale", new ScaleFrom3()); @@ -148,7 +148,7 @@ public class Robot extends IterativeRobot RLautonTaskChooser.addObject("Left to Left Scale", new ScaleFrom1()); RLautonTaskChooser.addObject("Center to Right Switch", new CenterRight()); - RLautonTaskChooser.addObject("Center to Right 2 cube", new CenterRight2Cube()); + //RLautonTaskChooser.addObject("Center to Right 2 cube", new CenterRight2Cube()); RLautonTaskChooser.addObject("Right to Right Switch", new RightSwitchAuton()); @@ -169,7 +169,7 @@ public class Robot extends IterativeRobot LLautonTaskChooser.addObject("Center to Left Switch", new CenterLeft()); - LLautonTaskChooser.addObject("Center to Left Switch 2 cube", new CenterLeft2Cube()); + //LLautonTaskChooser.addObject("Center to Left Switch 2 cube", new CenterLeft2Cube()); LLautonTaskChooser.addObject("Right to Left Switch", new RightStartLeftScore()); @@ -185,7 +185,7 @@ public class Robot extends IterativeRobot LRautonTaskChooser.addObject("Left to Left Switch", new LeftSwitchAuton()); LRautonTaskChooser.addObject("Center to Left Switch", new CenterLeft()); - LRautonTaskChooser.addObject("Center to Left Switch 2 cube", new CenterLeft2Cube()); + //LRautonTaskChooser.addObject("Center to Left Switch 2 cube", new CenterLeft2Cube()); LRautonTaskChooser.addObject("Right to Left Switch", new RightStartLeftScore()); LRautonTaskChooser.addObject("Right To Right Scale", new ScaleFrom3()); diff --git a/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft.java b/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft.java index f9a94d6..19c3c8e 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft.java @@ -25,6 +25,7 @@ public class CenterLeft extends CommandGroup { public CenterLeft() { addSequential(new DriveGyroReset()); + addSequential(new WaitCommand(.2)); addSequential(new IntakePosition(true)); addSequential(new DriveSpeedShift(true)); @@ -33,19 +34,20 @@ public class CenterLeft extends CommandGroup { addSequential(new WaitCommand(.2)); addSequential(new DriveStraightBasic(80, 45, true, true, 0)); addSequential(new ElevatorBasic(30)); - addSequential(new DriveTurnBasic(true, 12, 150, MPSoftwareTurnType.TANK)); + addSequential(new DriveTurnBasic(true, 28, 150, MPSoftwareTurnType.TANK)); + addParallel(new TimeoutBecaseYea()); addSequential(new DriveStraightBasic(12, 45, true, true, 0)); //addParallel(new TimeoutBecaseYea()); - //addSequential(new WaitCommand(3)); - + addSequential(new WaitCommand(3)); +/* addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); //addSequential(new WaitCommand(.1)); addSequential(new IntakePosition(false)); addSequential(new WaitCommand(.2)); addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); - + */ diff --git a/src/org/usfirst/frc4388/robot/commands/auton/CenterRight.java b/src/org/usfirst/frc4388/robot/commands/auton/CenterRight.java index 5e86855..7363c7e 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/CenterRight.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/CenterRight.java @@ -36,6 +36,7 @@ public class CenterRight extends CommandGroup { addSequential(new DriveTurnBasic(true, -35, 150, MPSoftwareTurnType.TANK)); addParallel(new TimeoutBecaseYea()); addSequential(new DriveStraightBasic(25, 45, true, true, 0)); + //addParallel(new TimeoutBecaseYea()); addSequential(new WaitCommand(3)); diff --git a/src/org/usfirst/frc4388/robot/commands/auton/LeftStartRightScore.java b/src/org/usfirst/frc4388/robot/commands/auton/LeftStartRightScore.java index 94a81fb..252b898 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/LeftStartRightScore.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/LeftStartRightScore.java @@ -40,13 +40,20 @@ public class LeftStartRightScore extends CommandGroup { addSequential(new WaitCommand(.2)); addSequential(new DriveTurnBasic(true, 73, 150, MPSoftwareTurnType.TANK)); addSequential(new WaitCommand(.2)); + addParallel(new TimeoutBecaseYea()); addSequential(new DriveStraightBasic(10, 60, true, true, 0)); - addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); + addSequential(new WaitCommand(3)); + + + /*addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); //addSequential(new WaitCommand(.1)); addSequential(new IntakePosition(false)); addSequential(new DriveStraightBasic(5, 60, true, true, 0)); addSequential(new WaitCommand(.2)); - addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); + addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));*/ + + + addSequential(new DriveStraightBasic(-15, 60, true, true, 0)); addSequential(new DriveSpeedShift(false)); diff --git a/src/org/usfirst/frc4388/robot/commands/auton/LeftSwitchAuton.java b/src/org/usfirst/frc4388/robot/commands/auton/LeftSwitchAuton.java index 3e8d35c..7a38e25 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/LeftSwitchAuton.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/LeftSwitchAuton.java @@ -24,6 +24,7 @@ public class LeftSwitchAuton extends CommandGroup { public LeftSwitchAuton() { addSequential(new DriveGyroReset()); + addSequential(new WaitCommand(.2)); addSequential(new DriveSpeedShift(true)); addSequential(new IntakePosition(true)); @@ -32,7 +33,12 @@ public class LeftSwitchAuton extends CommandGroup { addSequential(new ElevatorBasic(30)); addSequential(new DriveTurnBasic(true, -87, 150, MPSoftwareTurnType.TANK)); addSequential(new ElevatorBasic(30)); + addParallel(new TimeoutBecaseYea()); addSequential(new DriveStraightBasic(20, 50, true, true, 0)); + addSequential(new WaitCommand(3)); + + + /* addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); //addSequential(new WaitCommand(.1)); addSequential(new IntakePosition(false)); @@ -40,7 +46,7 @@ public class LeftSwitchAuton extends CommandGroup { addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); addSequential(new DriveStraightBasic(-20, 50, true, true, 0)); addSequential(new DriveSpeedShift(false)); - +*/ //addSequential(new DriveStraightMP(50, 10, true, true, 0)); // 95 for 112" greenville } diff --git a/src/org/usfirst/frc4388/robot/commands/auton/RightStartLeftScore.java b/src/org/usfirst/frc4388/robot/commands/auton/RightStartLeftScore.java index 6759ddd..fa4b963 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/RightStartLeftScore.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/RightStartLeftScore.java @@ -39,14 +39,22 @@ public class RightStartLeftScore extends CommandGroup { addSequential(new ElevatorBasic(30)); addSequential(new WaitCommand(.2)); addSequential(new DriveTurnBasic(true, -73, 150, MPSoftwareTurnType.TANK)); - addSequential(new WaitCommand(.2)); + addParallel(new TimeoutBecaseYea()); addSequential(new DriveStraightBasic(10, 60, true, true, 0)); + addSequential(new WaitCommand(3)); + + + /* + addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); //addSequential(new WaitCommand(.1)); addSequential(new IntakePosition(false)); addSequential(new DriveStraightBasic(5, 60, true, true, 0)); addSequential(new WaitCommand(.2)); addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); + */ + + addSequential(new DriveStraightBasic(-15, 60, true, true, 0)); addSequential(new DriveSpeedShift(false)); diff --git a/src/org/usfirst/frc4388/robot/commands/auton/RightSwitchAuton.java b/src/org/usfirst/frc4388/robot/commands/auton/RightSwitchAuton.java index d85eea3..79af0a2 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/RightSwitchAuton.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/RightSwitchAuton.java @@ -33,12 +33,15 @@ public class RightSwitchAuton extends CommandGroup { addSequential(new WaitCommand(.2)); addSequential(new DriveTurnBasic(true, 87, 150, MPSoftwareTurnType.TANK)); addSequential(new ElevatorBasic(30)); - addSequential(new DriveStraightBasic(8, 50, true, true, 0)); - addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); + addParallel(new TimeoutBecaseYea()); + addSequential(new DriveStraightBasic(10, 50, true, true, 0)); + /*addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); //addSequential(new WaitCommand(.1)); addSequential(new IntakePosition(false)); addSequential(new WaitCommand(.5)); - addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); + + addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED));*/ + addSequential(new WaitCommand(3)); addSequential(new DriveStraightBasic(-20, 50, true, true, 0)); addSequential(new DriveSpeedShift(false)); diff --git a/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom1.java b/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom1.java index f4c03f4..a6eeb5a 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom1.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom1.java @@ -24,25 +24,32 @@ public class ScaleFrom1 extends CommandGroup { public ScaleFrom1() { addSequential(new DriveGyroReset()); + addSequential(new WaitCommand(.2)); addSequential(new DriveSpeedShift(true)); addSequential(new IntakePosition(true)); - addSequential(new DriveStraightBasic(-10, 45, true, true, 0)); + //addSequential(new DriveStraightBasic(-10, 50, true, true, 0)); - addSequential(new DriveStraightBasic(-270, 45, true, true, 0)); - addSequential(new ElevatorBasic(70)); + addSequential(new DriveStraightBasic(-280, 65, true, true, 0)); + addSequential(new ElevatorBasic(75)); addSequential(new DriveStraightBasic(-30, 20, true, true, 0)); - addSequential(new DriveTurnBasic(true, -90, 100, MPSoftwareTurnType.TANK)); + addSequential(new DriveTurnBasic(true, -80, 100, MPSoftwareTurnType.TANK)); //addSequential(new DriveStraightBasic(5, 20, true, true, 0)); addSequential(new WaitCommand(.5)); + addSequential(new DriveStraightBasic(15, 25, true, true, 0)); + addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); //addSequential(new WaitCommand(.1)); addSequential(new IntakePosition(false)); addSequential(new WaitCommand(.5)); addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); - addSequential(new DriveStraightBasic(-25, 20, true, true, 0)); + + + addSequential(new DriveStraightBasic(-20, 20, true, true, 0)); addSequential(new ElevatorBasic(10)); + addSequential(new DriveTurnBasic(true, 85, 100, MPSoftwareTurnType.TANK)); + addSequential(new DriveStraightBasic(70, 20, true, true, 0)); addSequential(new DriveSpeedShift(false)); diff --git a/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom3.java b/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom3.java index 8ae5417..4adb647 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom3.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom3.java @@ -24,6 +24,7 @@ public class ScaleFrom3 extends CommandGroup { public ScaleFrom3() { addSequential(new DriveGyroReset()); + addSequential(new WaitCommand(.2)); addSequential(new DriveSpeedShift(true)); addSequential(new IntakePosition(true)); diff --git a/src/org/usfirst/frc4388/robot/subsystems/Climber.java b/src/org/usfirst/frc4388/robot/subsystems/Climber.java index 54321f1..3cacf76 100644 --- a/src/org/usfirst/frc4388/robot/subsystems/Climber.java +++ b/src/org/usfirst/frc4388/robot/subsystems/Climber.java @@ -51,7 +51,7 @@ public class Climber extends Subsystem{ public void setClimbSpeed(boolean Climb) { if (Climb==true) { - Climber.set(0.95); + Climber.set(1.0); } if (Climb==false) { Climber.set(0);