diff --git a/src/org/usfirst/frc4388/robot/Constants.java b/src/org/usfirst/frc4388/robot/Constants.java index 5208db8..bbc4054 100644 --- a/src/org/usfirst/frc4388/robot/Constants.java +++ b/src/org/usfirst/frc4388/robot/Constants.java @@ -23,7 +23,7 @@ public class Constants { public static double kDriveStraightBasicMinSpeedInchesPerSec = 5.0; public static double kDriveStraightBasicYawErrorDivisor = 20.0; // steer parameter will be -yawError divided by this public static double kDriveStraightBasicMaxSteerMagnitude = 0.8; // keep absolute value of steer parameter below this - public static double kDriveTurnBasicTankMotorOutput = 0.5; + public static double kDriveTurnBasicTankMotorOutput = 0.4; public static double kDriveTurnBasicSingleMotorOutput = 0.15; public static double kElevatorWheelDiameterInches = 1; // Encoders @@ -35,7 +35,7 @@ public class Constants { public static double kElevatorInchesOfTravelPerRev = 3.75; // public static double kElevatorEncoderTicksPerInch = (double)kElevatorEncoderTickesPerRev / kElevatorInchesOfTravelPerRev; public static double kElevatorEncoderTicksPerInch = 126.36; - public static double kElevatorBasicPercentOutputUp = -.8; + public static double kElevatorBasicPercentOutputUp = -0.85; public static double kElevatorBasicPercentOutputDown =.7; // CONTROL LOOP GAINS diff --git a/src/org/usfirst/frc4388/robot/OI.java b/src/org/usfirst/frc4388/robot/OI.java index e3a4232..92c20bb 100644 --- a/src/org/usfirst/frc4388/robot/OI.java +++ b/src/org/usfirst/frc4388/robot/OI.java @@ -192,7 +192,7 @@ public class OI //SmartDashboard.putData("Move Stop", new ElevatorSetSpeed(ElevatorAuton.RAISE_ELEVATOR_SPEED)); - SmartDashboard.putData("move elevator", new ElevatorBasic(20)); + //SmartDashboard.putData("move elevator", new ElevatorBasic(20)); ///SmartDashboard.putData("Move Down", new ElevatorSetSpeed(ElevatorAuton.LOWER_ELEVATOR_SPEED)); ///SmartDashboard.putData("Move UP", new ElevatorSetSpeed(ElevatorAuton.STOP_ELEVATOR_SPEED)); @@ -260,7 +260,7 @@ public class OI //Button turnRelativeMP = new InternalButton(); //turnRelativeMP.whenPressed(new DriveRelativeTurnMP(10, Drive.MP_AUTON_MAX_TURN_RATE_DEG_PER_SEC, MPSoftwareTurnType.LEFT_SIDE_ONLY )); //SmartDashboard.putData("Turn Relative 10", turnRelativeMP); - + /* Button turnAbsoluteMP = new InternalButton(); turnAbsoluteMP.whenPressed(new DriveAbsoluteTurnMP(45, Drive.MP_AUTON_MAX_TURN_RATE_DEG_PER_SEC, MPSoftwareTurnType.LEFT_SIDE_ONLY)); SmartDashboard.putData("Turn Absolute 45", turnAbsoluteMP); @@ -268,13 +268,13 @@ public class OI Button turnRelativePID = new InternalButton(); turnRelativePID.whenPressed(new DriveRelativeTurnPID(45, MPSoftwareTurnType.TANK)); SmartDashboard.putData("Turn Relative 45", turnRelativePID); - +*/ // Button gyroReset = new InternalButton(); // gyroReset.whenPressed(new DriveGyroReset()); // SmartDashboard.putData("Gyro Reset", gyroReset); SmartDashboard.putData("Reset Gyro", new DriveGyroReset()); //SmartDashboard.putData("reset Encoders", new drive.resetEncoders()); - + SmartDashboard.putData("clampy boi", new IntakePosition(true)); // Camera //Button cameraUpdateDashboard = new InternalButton(); diff --git a/src/org/usfirst/frc4388/robot/Robot.java b/src/org/usfirst/frc4388/robot/Robot.java index 2f6a904..9efc772 100644 --- a/src/org/usfirst/frc4388/robot/Robot.java +++ b/src/org/usfirst/frc4388/robot/Robot.java @@ -123,14 +123,13 @@ public class Robot extends IterativeRobot RRautonTaskChooser.addDefault("Choose RR Program", new CrossTheBaseLine()); - - RRautonTaskChooser.addObject("Left to Right Switch", new LeftStartRightScore()); + RRautonTaskChooser.addObject("Right too 2 Cube Scale", new Cube2Right()); + RRautonTaskChooser.addObject("Center too right 2 Cube", new CenterRight2Cube()); + 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("Right to Right Switch", new RightSwitchAuton()); - RRautonTaskChooser.addObject("Right to Right Scale", new ScaleFrom3()); + RRautonTaskChooser.addObject("Right to Right tall boi", new ScaleFrom3()); SmartDashboard.putData("RRAuton Task", RRautonTaskChooser); @@ -145,12 +144,13 @@ public class Robot extends IterativeRobot RLautonTaskChooser.addObject("Left to Right Switch", new LeftStartRightScore()); - RLautonTaskChooser.addObject("Left to Left Scale", new ScaleFrom1()); - + RLautonTaskChooser.addObject("Left to Left tall boi", new ScaleFrom1()); + RLautonTaskChooser.addObject("Center too right 2 Cube", new CenterRight2Cube()); RLautonTaskChooser.addObject("Center to Right Switch", new CenterRight()); - //RLautonTaskChooser.addObject("Center to Right 2 cube", new CenterRight2Cube()); - RLautonTaskChooser.addObject("Right to Right Switch", new RightSwitchAuton()); + RLautonTaskChooser.addObject("Left to left 2 Cube Scale", new Cube2Left()); + + SmartDashboard.putData("RLAuton Task", RLautonTaskChooser); @@ -164,15 +164,12 @@ public class Robot extends IterativeRobot LLautonTaskChooser.addDefault("Choose LL Program", new CrossTheBaseLine()); - LLautonTaskChooser.addObject("Left to Left Scale", new ScaleFrom1()); + LLautonTaskChooser.addObject("Left to Left tall boi", new ScaleFrom1()); LLautonTaskChooser.addObject("Left to Left Switch", new LeftSwitchAuton()); - - + LLautonTaskChooser.addObject("Center too 2 left cube", new CenterLeft2Cube()); LLautonTaskChooser.addObject("Center to Left Switch", new CenterLeft()); - //LLautonTaskChooser.addObject("Center to Left Switch 2 cube", new CenterLeft2Cube()); - - LLautonTaskChooser.addObject("Right to Left Switch", new RightStartLeftScore()); + LLautonTaskChooser.addObject("Left to left 2 Cube Scale", new Cube2Left ()); SmartDashboard.putData("LLAuton Task", LLautonTaskChooser); @@ -181,14 +178,15 @@ public class Robot extends IterativeRobot LRautonTaskChooser = new SendableChooser(); LRautonTaskChooser.addDefault("Choose LR Program", new CrossTheBaseLine()); + LRautonTaskChooser.addObject("Left to Left Switch", new LeftSwitchAuton()); - + LRautonTaskChooser.addObject("Right to 2 Cube Scale", new Cube2Right()); LRautonTaskChooser.addObject("Center to Left Switch", new CenterLeft()); - //LRautonTaskChooser.addObject("Center to Left Switch 2 cube", new CenterLeft2Cube()); - + LRautonTaskChooser.addObject("Center too left 2 cube", new CenterLeft2Cube()); LRautonTaskChooser.addObject("Right to Left Switch", new RightStartLeftScore()); - LRautonTaskChooser.addObject("Right To Right Scale", new ScaleFrom3()); + LRautonTaskChooser.addObject("Right To Right tall boi", new ScaleFrom3()); + SmartDashboard.putData("LRAuton Task", LRautonTaskChooser); diff --git a/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft.java b/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft.java index 19c3c8e..638faf3 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft.java @@ -37,7 +37,7 @@ public class CenterLeft extends CommandGroup { 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)); diff --git a/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft2Cube.java b/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft2Cube.java index 1857e60..a4a4c63 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft2Cube.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft2Cube.java @@ -33,30 +33,26 @@ public class CenterLeft2Cube extends CommandGroup { addSequential(new WaitCommand(.2)); addSequential(new DriveStraightBasic(80, 45, true, true, 0)); addSequential(new ElevatorBasic(30)); - addSequential(new DriveTurnBasic(true, 17, 150, MPSoftwareTurnType.TANK)); - addSequential(new DriveStraightBasic(10, 45, true, true, 0)); - 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)); + addSequential(new DriveTurnBasic(true, 28, 150, MPSoftwareTurnType.TANK)); + addParallel(new TimeoutBecaseYea()); + addSequential(new DriveStraightBasic(12, 45, true, true, 0)); + addSequential(new WaitCommand(1)); addSequential(new DriveStraightBasic(-20, 45, true, true, 0)); - addSequential(new ElevatorBasic(10)); - addSequential(new WaitCommand(.2)); - addSequential(new ElevatorBasic(2)); - addSequential(new DriveTurnBasic(true, -90, 150, MPSoftwareTurnType.TANK)); + addSequential(new ElevatorBasic(3)); + addSequential(new DriveTurnBasic(true, 75, 150, MPSoftwareTurnType.TANK)); addSequential(new IntakeSetSpeed(Carriage.CUBE_INTAKE_SPEED)); - addSequential(new DriveStraightBasic(20, 45, true, true, 0)); + addSequential(new DriveStraightBasic(45, 45, true, true, 0)); + addSequential(new WaitCommand(2)); addSequential(new IntakePosition(true)); - addSequential(new DriveStraightBasic(-20, 45, true, true, 0)); - addSequential(new ElevatorBasic(25)); - addSequential(new DriveTurnBasic(true, 90, 150, MPSoftwareTurnType.TANK)); - addSequential(new DriveStraightBasic(10, 45, true, true, 0)); - addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); - //addSequential(new WaitCommand(.1)); - addSequential(new IntakePosition(false)); - addSequential(new WaitCommand(.2)); + addSequential(new DriveStraightBasic(-40, 45, true, true, 0)); addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); + addSequential(new ElevatorBasic(25)); + addSequential(new DriveTurnBasic(true, -75, 150, MPSoftwareTurnType.TANK)); + addParallel(new TimeoutBecaseYea()); + addSequential(new DriveStraightBasic(20, 45, true, true, 0)); + + addSequential(new WaitCommand(1)); + addSequential(new DriveStraightBasic(-20, 45, true, true, 0)); addSequential(new DriveSpeedShift(false)); diff --git a/src/org/usfirst/frc4388/robot/commands/auton/CenterLeftAbs.java b/src/org/usfirst/frc4388/robot/commands/auton/CenterLeftAbs.java deleted file mode 100644 index c4e3981..0000000 --- a/src/org/usfirst/frc4388/robot/commands/auton/CenterLeftAbs.java +++ /dev/null @@ -1,46 +0,0 @@ -package org.usfirst.frc4388.robot.commands.auton; - - -import org.usfirst.frc4388.robot.commands.DriveGyroReset; -import org.usfirst.frc4388.robot.commands.DriveSpeedShift; -import org.usfirst.frc4388.robot.commands.DriveStraightBasic; -import org.usfirst.frc4388.robot.commands.DriveStraightBasicAbs; -import org.usfirst.frc4388.robot.commands.DriveTurnBasic; -import org.usfirst.frc4388.robot.commands.ElevatorBasic; - -import org.usfirst.frc4388.robot.commands.IntakePosition; -import org.usfirst.frc4388.robot.commands.IntakeSetSpeed; -import org.usfirst.frc4388.robot.subsystems.Carriage; - -import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; - -import edu.wpi.first.wpilibj.command.CommandGroup; -import edu.wpi.first.wpilibj.command.WaitCommand; - -/** - * - */ -public class CenterLeftAbs extends CommandGroup { - - public CenterLeftAbs() - { - addSequential(new DriveGyroReset()); - addSequential(new IntakePosition(true)); - addSequential(new DriveSpeedShift(true)); - - addSequential(new DriveStraightBasicAbs(-15, 50, true, true, 0)); - addSequential(new DriveTurnBasic(true, 130, 300, MPSoftwareTurnType.TANK)); - addSequential(new DriveStraightBasicAbs(53, 50, true, true, 130)); - addSequential(new ElevatorBasic(20)); - addSequential(new DriveTurnBasic(true, 164.5, 300, MPSoftwareTurnType.TANK)); - addSequential(new DriveStraightBasicAbs(19, 50, true, true, 164.5)); - 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)); - addSequential(new DriveStraightBasicAbs(-20, 50, true, true, 164.5)); - addSequential(new ElevatorBasic(10)); - addSequential(new DriveSpeedShift(false)); - } -} diff --git a/src/org/usfirst/frc4388/robot/commands/auton/CenterRight2Cube.java b/src/org/usfirst/frc4388/robot/commands/auton/CenterRight2Cube.java index 6de2081..b08b620 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/CenterRight2Cube.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/CenterRight2Cube.java @@ -18,40 +18,35 @@ public class CenterRight2Cube extends CommandGroup { public CenterRight2Cube() { addSequential(new DriveGyroReset()); + addSequential(new WaitCommand(.2)); addSequential(new IntakePosition(true)); addSequential(new DriveSpeedShift(true)); - addSequential(new DriveStraightBasic(-15, 30, true, true, 0)); + addSequential(new DriveStraightBasic(-15, 60, true, true, 0)); addSequential(new DriveTurnBasic(true, -115, 150, MPSoftwareTurnType.TANK)); addSequential(new WaitCommand(.2)); - addSequential(new DriveStraightBasic(80, 45, true, true, 0)); + addSequential(new DriveStraightBasic(65, 60, true, true, 0)); addSequential(new ElevatorBasic(25)); - addSequential(new DriveTurnBasic(true, -35, 150, MPSoftwareTurnType.TANK)); - addSequential(new DriveStraightBasic(15, 45, true, true, 0)); - 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)); - addSequential(new DriveStraightBasic(-25, 45, true, true, 0)); - addSequential(new ElevatorBasic(10)); - addSequential(new WaitCommand(.2)); - addSequential(new ElevatorBasic(2)); - addSequential(new DriveTurnBasic(true, 90, 150, MPSoftwareTurnType.TANK)); - addSequential(new IntakeSetSpeed(Carriage.CUBE_INTAKE_SPEED)); - addSequential(new DriveStraightBasic(20, 45, true, true, 0)); - addSequential(new IntakePosition(true)); - addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); + addSequential(new DriveTurnBasic(true, -38, 150, MPSoftwareTurnType.TANK)); + addParallel(new TimeoutBecaseYea()); + addSequential(new DriveStraightBasic(25, 45, true, true, 0)); + addSequential(new WaitCommand(1)); addSequential(new DriveStraightBasic(-20, 45, true, true, 0)); - - addSequential(new ElevatorBasic(25)); - addSequential(new DriveTurnBasic(true, -90, 150, MPSoftwareTurnType.TANK)); - addSequential(new DriveStraightBasic(10, 45, true, true, 0)); - addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); - //addSequential(new WaitCommand(.1)); - addSequential(new IntakePosition(false)); - addSequential(new WaitCommand(.2)); + addSequential(new ElevatorBasic(3)); + addSequential(new DriveTurnBasic(true, -75, 150, MPSoftwareTurnType.TANK)); + addSequential(new IntakeSetSpeed(Carriage.CUBE_INTAKE_SPEED)); + addSequential(new DriveStraightBasic(45, 45, true, true, 0)); + addSequential(new WaitCommand(2)); + addSequential(new IntakePosition(true)); + addSequential(new DriveStraightBasic(-40, 60, true, true, 0)); addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); + addSequential(new ElevatorBasic(25)); + addSequential(new DriveTurnBasic(true, 75, 150, MPSoftwareTurnType.TANK)); + addParallel(new TimeoutBecaseYea()); + addSequential(new DriveStraightBasic(20, 45, true, true, 0)); + + addSequential(new WaitCommand(1)); + addSequential(new DriveStraightBasic(-20, 45, true, true, 0)); addSequential(new DriveSpeedShift(false)); diff --git a/src/org/usfirst/frc4388/robot/commands/auton/CenterRightAbs.java b/src/org/usfirst/frc4388/robot/commands/auton/CenterRightAbs.java deleted file mode 100644 index 538a7db..0000000 --- a/src/org/usfirst/frc4388/robot/commands/auton/CenterRightAbs.java +++ /dev/null @@ -1,45 +0,0 @@ -package org.usfirst.frc4388.robot.commands.auton; - - -import org.usfirst.frc4388.robot.commands.DriveGyroReset; -import org.usfirst.frc4388.robot.commands.DriveSpeedShift; -import org.usfirst.frc4388.robot.commands.DriveStraightBasic; -import org.usfirst.frc4388.robot.commands.DriveStraightBasicAbs; -import org.usfirst.frc4388.robot.commands.DriveTurnBasic; -import org.usfirst.frc4388.robot.commands.ElevatorBasic; - -import org.usfirst.frc4388.robot.commands.IntakePosition; -import org.usfirst.frc4388.robot.commands.IntakeSetSpeed; -import org.usfirst.frc4388.robot.subsystems.Carriage; - -import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; - -import edu.wpi.first.wpilibj.command.CommandGroup; -import edu.wpi.first.wpilibj.command.WaitCommand; - -/** - * - */ -public class CenterRightAbs extends CommandGroup { - - public CenterRightAbs() - { - addSequential(new DriveGyroReset()); - addSequential(new IntakePosition(true)); - addSequential(new DriveSpeedShift(true)); - - addSequential(new DriveStraightBasicAbs(-15, 50, true, true, 0)); - addSequential(new DriveTurnBasic(true, -130, 300, MPSoftwareTurnType.TANK)); - addSequential(new DriveStraightBasicAbs(53, 50, true, true, -130)); - addSequential(new ElevatorBasic(20)); - addSequential(new DriveTurnBasic(true, -164.5, 300, MPSoftwareTurnType.TANK)); - addSequential(new DriveStraightBasicAbs(19, 50, true, true, -164.5)); - 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)); - addSequential(new DriveStraightBasic(-20, 50, true, true, -164.5)); - addSequential(new DriveSpeedShift(false)); - } -} diff --git a/src/org/usfirst/frc4388/robot/commands/auton/Cube2Left.java b/src/org/usfirst/frc4388/robot/commands/auton/Cube2Left.java new file mode 100644 index 0000000..ff69654 --- /dev/null +++ b/src/org/usfirst/frc4388/robot/commands/auton/Cube2Left.java @@ -0,0 +1,75 @@ +package org.usfirst.frc4388.robot.commands.auton; + + +import org.usfirst.frc4388.robot.commands.DriveGyroReset; +import org.usfirst.frc4388.robot.commands.DriveSpeedShift; +import org.usfirst.frc4388.robot.commands.DriveStraightBasic; + +import org.usfirst.frc4388.robot.commands.DriveTurnBasic; +import org.usfirst.frc4388.robot.commands.ElevatorBasic; + +import org.usfirst.frc4388.robot.commands.IntakePosition; +import org.usfirst.frc4388.robot.commands.IntakeSetSpeed; +import org.usfirst.frc4388.robot.subsystems.Carriage; + +import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; + +import edu.wpi.first.wpilibj.command.CommandGroup; +import edu.wpi.first.wpilibj.command.WaitCommand; + +/** + * + */ +public class Cube2Left extends CommandGroup { + + public Cube2Left() { + addSequential(new DriveGyroReset()); + addSequential(new WaitCommand(.2)); + addSequential(new DriveSpeedShift(true)); + addSequential(new IntakePosition(true)); + + addSequential(new DriveStraightBasic(-300, 70, true, true, 0)); + addSequential(new DriveStraightBasic(1, 30, true, true, 0)); + addSequential(new ElevatorBasic(70)); + addSequential(new DriveTurnBasic(true, -90, 100, MPSoftwareTurnType.TANK)); + addSequential(new WaitCommand(.5)); + 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(-10, 30, true, true, 0)); + addParallel(new ElevatorBasic(4)); + + ////////////////////////////////////////////////////////////////////////// + addSequential(new DriveTurnBasic(true, 69, 100, MPSoftwareTurnType.TANK)); + ////////////////////////////////////////////////////////////////////////// + + addSequential(new IntakeSetSpeed(Carriage.CUBE_INTAKE_FAST_SPEED)); + + ///////////////////////////////////////////////////////////// + addSequential(new DriveStraightBasic(10, 30, true, true, 0)); + addSequential(new DriveStraightBasic(80, 60, true, true, 0)); + addSequential(new DriveStraightBasic(33, 20, true, true, 0)); + ///////////////////////////////////////////////////////////// + addSequential(new WaitCommand(.5)); + addSequential(new IntakePosition(true)); + //addSequential(new WaitCommand(.3)); + addSequential(new WaitCommand(.2)); + addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); + addParallel(new TimeoutBecaseYea()); + addSequential(new ElevatorBasic(28)); + ///////////////////////////////////////////////////////////// + addSequential(new DriveStraightBasic(6, 50, true, true, 0)); + ///////////////////////////////////////////////////////////// + addSequential(new WaitCommand(1.5)); + addSequential(new DriveStraightBasic(-20, 40, true, true, 0)); + addSequential(new ElevatorBasic(5)); + + + addSequential(new DriveSpeedShift(false)); + + + } +} \ No newline at end of file diff --git a/src/org/usfirst/frc4388/robot/commands/auton/Cube2Right.java b/src/org/usfirst/frc4388/robot/commands/auton/Cube2Right.java index 6fe0160..cfdb81b 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/Cube2Right.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/Cube2Right.java @@ -24,40 +24,52 @@ public class Cube2Right extends CommandGroup { public Cube2Right() { addSequential(new DriveGyroReset()); + addSequential(new WaitCommand(.2)); addSequential(new DriveSpeedShift(true)); addSequential(new IntakePosition(true)); - addSequential(new DriveStraightBasic(-140, 50, true, true, 0)); - addSequential(new ElevatorBasic(30)); - 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 DriveStraightBasic(-10, 35, true, true, 0)); + + addSequential(new DriveStraightBasic(-300, 70, true, true, 0)); + addSequential(new DriveStraightBasic(1, 30, true, true, 0)); + addSequential(new ElevatorBasic(70)); + addSequential(new DriveTurnBasic(true, 90, 100, MPSoftwareTurnType.TANK)); + addSequential(new WaitCommand(.5)); addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); - //addSequential(new WaitCommand(.1)); - addSequential(new IntakePosition(false)); - addSequential(new WaitCommand(.4)); - addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); - addSequential(new DriveStraightBasic(-10, 50, true, true, 0)); - addSequential(new DriveTurnBasic(true, 40, 150, MPSoftwareTurnType.TANK)); - addSequential(new DriveStraightBasic(-30, 50, true, true, 0)); - addSequential(new DriveTurnBasic(true, -10, 150, MPSoftwareTurnType.TANK)); - addSequential(new IntakeSetSpeed(Carriage.CUBE_INTAKE_SPEED)); - addSequential(new DriveStraightBasic(30, 50, true, true, 0)); - addSequential(new IntakePosition(true)); - addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); - addSequential(new DriveStraightBasic(-30, 50, true, true, 0)); - addSequential(new DriveTurnBasic(true, 30, 150, MPSoftwareTurnType.TANK)); - addSequential(new ElevatorBasic(70)); - addSequential(new DriveStraightBasic(-20, 20, true, true, 0)); - addSequential(new DriveTurnBasic(true, 87, 150, MPSoftwareTurnType.TANK)); - addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); - //addSequential(new WaitCommand(.1)); + addSequential(new WaitCommand(.1)); addSequential(new IntakePosition(false)); addSequential(new WaitCommand(.5)); addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); + addSequential(new DriveStraightBasic(-10, 30, true, true, 0)); + addParallel(new ElevatorBasic(4)); + + ////////////////////////////////////////////////////////////////////////// + addSequential(new DriveTurnBasic(true, -69, 100, MPSoftwareTurnType.TANK)); + ////////////////////////////////////////////////////////////////////////// + + addSequential(new IntakeSetSpeed(Carriage.CUBE_INTAKE_FAST_SPEED)); + + ///////////////////////////////////////////////////////////// + addSequential(new DriveStraightBasic(10, 30, true, true, 0)); + addSequential(new DriveStraightBasic(80, 60, true, true, 0)); + addSequential(new DriveStraightBasic(33, 20, true, true, 0)); + ///////////////////////////////////////////////////////////// + addSequential(new WaitCommand(.5)); + addSequential(new IntakePosition(true)); + //addSequential(new WaitCommand(.3)); + addSequential(new WaitCommand(.2)); + addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); + addParallel(new TimeoutBecaseYea()); + addSequential(new ElevatorBasic(28)); + ///////////////////////////////////////////////////////////// + addSequential(new DriveStraightBasic(6, 50, true, true, 0)); + ///////////////////////////////////////////////////////////// + addSequential(new WaitCommand(1.5)); + addSequential(new DriveStraightBasic(-20, 40, true, true, 0)); + addSequential(new ElevatorBasic(5)); + addSequential(new DriveSpeedShift(false)); diff --git a/src/org/usfirst/frc4388/robot/commands/auton/LeftStartRightScore.java b/src/org/usfirst/frc4388/robot/commands/auton/LeftStartRightScore.java index 252b898..4804d8a 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/LeftStartRightScore.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/LeftStartRightScore.java @@ -30,7 +30,7 @@ public class LeftStartRightScore extends CommandGroup { addSequential(new IntakePosition(true)); - addSequential(new DriveStraightBasic(-215, 40, true, true, 0)); + addSequential(new DriveStraightBasic(-215, 75, true, true, 0)); addSequential(new WaitCommand(.2)); addSequential(new ElevatorBasic(5)); addSequential(new DriveTurnBasic(true, -73, 150, MPSoftwareTurnType.TANK)); diff --git a/src/org/usfirst/frc4388/robot/commands/auton/RightStartLeftScore.java b/src/org/usfirst/frc4388/robot/commands/auton/RightStartLeftScore.java index fa4b963..86a31af 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/RightStartLeftScore.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/RightStartLeftScore.java @@ -30,7 +30,7 @@ public class RightStartLeftScore extends CommandGroup { addSequential(new IntakePosition(true)); - addSequential(new DriveStraightBasic(-215, 40, true, true, 0)); + addSequential(new DriveStraightBasic(-215, 60, true, true, 0)); addSequential(new WaitCommand(.2)); addSequential(new ElevatorBasic(5)); addSequential(new DriveTurnBasic(true, 73, 150, MPSoftwareTurnType.TANK)); diff --git a/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom3.java b/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom3.java index 4adb647..efc4879 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom3.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom3.java @@ -29,7 +29,7 @@ public class ScaleFrom3 extends CommandGroup { addSequential(new IntakePosition(true)); - addSequential(new DriveStraightBasic(-10, 45, true, true, 0)); + addSequential(new DriveStraightBasic(-10, 20, true, true, 0)); addSequential(new DriveStraightBasic(-270, 45, true, true, 0)); addSequential(new ElevatorBasic(70)); diff --git a/src/org/usfirst/frc4388/robot/commands/auton/TimeoutBecaseYea.java b/src/org/usfirst/frc4388/robot/commands/auton/TimeoutBecaseYea.java index ab260b5..0cf2d00 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/TimeoutBecaseYea.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/TimeoutBecaseYea.java @@ -24,7 +24,7 @@ public class TimeoutBecaseYea extends CommandGroup { public TimeoutBecaseYea() { - addSequential(new WaitCommand(2)); + addSequential(new WaitCommand(1.5)); addSequential(new IntakeSetSpeed(Carriage.CUBE_EJECT_SPEED)); //addSequential(new WaitCommand(.1)); addSequential(new IntakePosition(false)); diff --git a/src/org/usfirst/frc4388/robot/subsystems/Carriage.java b/src/org/usfirst/frc4388/robot/subsystems/Carriage.java index bde3755..2744aec 100644 --- a/src/org/usfirst/frc4388/robot/subsystems/Carriage.java +++ b/src/org/usfirst/frc4388/robot/subsystems/Carriage.java @@ -35,7 +35,8 @@ public class Carriage extends Subsystem { private WPI_TalonSRX carriageRight; public static enum CarriageControlMode { JOYSTICK, MP_STRAIGHT, HOLD, MANUAL}; public static final double CUBE_INTAKE_SPEED = 0.40; - public static final double CUBE_EJECT_SPEED = -0.8; + public static final double CUBE_INTAKE_FAST_SPEED = 0.50; + public static final double CUBE_EJECT_SPEED = -0.75; public static final double CUBE_STOP_SPEED = 0; /////^^^^^^^^^ replace this line with the modes we need