From 74ed82fc406e2222956ffb401ab73750841585c4 Mon Sep 17 00:00:00 2001 From: lukesta182 <33330639+lukesta182@users.noreply.github.com> Date: Fri, 9 Mar 2018 00:01:29 -0600 Subject: [PATCH] Auto Fixes --- src/org/usfirst/frc4388/robot/Robot.java | 25 ++------ .../robot/commands/AutonomousCommand.java | 59 ------------------- .../robot/commands/auton/CenterLeft.java | 14 +++-- .../robot/commands/auton/CenterRight.java | 14 +++-- .../commands/auton/CrossTheBaseLine.java | 13 +--- .../commands/auton/LeftStartRightScore.java | 11 ++-- .../robot/commands/auton/LeftSwitchAuton.java | 47 +++++++++++++++ .../commands/auton/RightStartLeftScore.java | 11 ++-- .../commands/auton/RightSwitchAuton.java | 12 ++-- .../robot/commands/auton/ScaleFrom1.java | 47 +++++++++++++++ .../robot/commands/auton/ScaleFrom3.java | 14 +++-- .../frc4388/robot/subsystems/Elevator.java | 2 +- 12 files changed, 152 insertions(+), 117 deletions(-) delete mode 100644 src/org/usfirst/frc4388/robot/commands/AutonomousCommand.java create mode 100644 src/org/usfirst/frc4388/robot/commands/auton/LeftSwitchAuton.java create mode 100644 src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom1.java diff --git a/src/org/usfirst/frc4388/robot/Robot.java b/src/org/usfirst/frc4388/robot/Robot.java index 0204b23..bea5105 100644 --- a/src/org/usfirst/frc4388/robot/Robot.java +++ b/src/org/usfirst/frc4388/robot/Robot.java @@ -126,13 +126,11 @@ public class Robot extends IterativeRobot RRautonTaskChooser.addObject("Left to Right Switch", new LeftStartRightScore()); - RRautonTaskChooser.addObject("Center to Left Switch", new CenterLeft()); RRautonTaskChooser.addObject("Center to Right Switch", new CenterRight()); RRautonTaskChooser.addObject("Right to Right Switch", new RightSwitchAuton()); - RRautonTaskChooser.addObject("Right to Left Switch", new RightStartLeftScore()); - RRautonTaskChooser.addObject("Scale3", new ScaleFrom3()); + RRautonTaskChooser.addObject("Right to Right Scale", new ScaleFrom3()); SmartDashboard.putData("RRAuton Task", RRautonTaskChooser); @@ -147,15 +145,11 @@ public class Robot extends IterativeRobot RLautonTaskChooser.addObject("Left to Right Switch", new LeftStartRightScore()); - + RLautonTaskChooser.addObject("Left to Left Scale", new ScaleFrom1()); - RLautonTaskChooser.addObject("Center to Left Switch", new CenterLeft()); RLautonTaskChooser.addObject("Center to Right Switch", new CenterRight()); - RLautonTaskChooser.addObject("Right to Right Switch", new RightSwitchAuton()); - RLautonTaskChooser.addObject("Right to Left Switch", new RightStartLeftScore()); - SmartDashboard.putData("RLAuton Task", RLautonTaskChooser); @@ -169,36 +163,29 @@ public class Robot extends IterativeRobot LLautonTaskChooser.addDefault("Choose LL Program", new CrossTheBaseLine()); - LLautonTaskChooser.addObject("Left to Right Switch", new LeftStartRightScore()); + LLautonTaskChooser.addObject("Left to Left Scale", new ScaleFrom1()); + LLautonTaskChooser.addObject("Left to Left Switch", new LeftSwitchAuton()); LLautonTaskChooser.addObject("Center to Left Switch", new CenterLeft()); - LLautonTaskChooser.addObject("Center to Right Switch", new CenterRight()); - LLautonTaskChooser.addObject("Right to Right Switch", new RightSwitchAuton()); LLautonTaskChooser.addObject("Right to Left Switch", new RightStartLeftScore()); SmartDashboard.putData("LLAuton Task", LLautonTaskChooser); - - - LRautonTaskChooser = new SendableChooser(); LRautonTaskChooser.addDefault("Choose LR Program", new CrossTheBaseLine()); - LRautonTaskChooser.addObject("Left to Right Switch", new LeftStartRightScore()); - + LRautonTaskChooser.addObject("Left to Left Switch", new LeftSwitchAuton()); LRautonTaskChooser.addObject("Center to Left Switch", new CenterLeft()); - LRautonTaskChooser.addObject("Center to Right Switch", new CenterRight()); - LRautonTaskChooser.addObject("Right to Right Switch", new RightSwitchAuton()); LRautonTaskChooser.addObject("Right to Left Switch", new RightStartLeftScore()); - + LRautonTaskChooser.addObject("Right To Right Scale", new ScaleFrom3()); SmartDashboard.putData("LRAuton Task", LRautonTaskChooser); diff --git a/src/org/usfirst/frc4388/robot/commands/AutonomousCommand.java b/src/org/usfirst/frc4388/robot/commands/AutonomousCommand.java deleted file mode 100644 index e02ed3c..0000000 --- a/src/org/usfirst/frc4388/robot/commands/AutonomousCommand.java +++ /dev/null @@ -1,59 +0,0 @@ -// RobotBuilder Version: 2.0 -// -// This file was generated by RobotBuilder. It contains sections of -// code that are automatically generated and assigned by robotbuilder. -// These sections will be updated in the future when you export to -// Java from RobotBuilder. Do not put any code or make any change in -// the blocks indicating autogenerated code or it will be lost on an -// update. Deleting the comments indicating the section will prevent -// it from being updated in the future. - - -package org.usfirst.frc4388.robot.commands; -import org.usfirst.frc4388.robot.Robot; - -import edu.wpi.first.wpilibj.command.Command; - -/** - * - */ -public class AutonomousCommand extends Command { - - // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_DECLARATIONS - - // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_DECLARATIONS - - // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR - public AutonomousCommand() { - - // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=CONSTRUCTOR - // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_SETTING - - // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_SETTING - // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES - - // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=REQUIRES - } - - // Called just before this Command runs the first time - protected void initialize() { - } - - // Called repeatedly when this Command is scheduled to run - protected void execute() { - } - - // Make this return true when this Command no longer needs to run execute() - protected boolean isFinished() { - return false; - } - - // Called once after isFinished returns true - protected void end() { - } - - // Called when another command which requires one or more of the same - // subsystems is scheduled to run - protected void interrupted() { - } -} diff --git a/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft.java b/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft.java index 5d8bc4b..ed95744 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/CenterLeft.java @@ -1,17 +1,17 @@ package org.usfirst.frc4388.robot.commands.auton; -import org.usfirst.frc4388.robot.commands.DriveAbsoluteTurnMP; + 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.DriveStraightMP; + import org.usfirst.frc4388.robot.commands.DriveTurnBasic; import org.usfirst.frc4388.robot.commands.ElevatorBasic; -import org.usfirst.frc4388.robot.commands.ElevatorSetSpeed; + 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.robot.subsystems.Drive; + import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; import edu.wpi.first.wpilibj.command.CommandGroup; @@ -25,8 +25,9 @@ public class CenterLeft extends CommandGroup { public CenterLeft() { addSequential(new DriveGyroReset()); - addSequential(new IntakePosition(true)); + addSequential(new DriveSpeedShift(true)); + addSequential(new DriveStraightBasic(-15, 60, true, true, 0)); addSequential(new DriveTurnBasic(true, 130, 300, MPSoftwareTurnType.TANK)); addSequential(new DriveStraightBasic(53, 60, true, true, 0)); @@ -38,5 +39,8 @@ public class CenterLeft extends CommandGroup { addSequential(new IntakePosition(false)); addSequential(new WaitCommand(.2)); addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); + addSequential(new DriveStraightBasic(-20, 60, true, true, 0)); + addSequential(new ElevatorBasic(10)); + addSequential(new DriveSpeedShift(false)); } } diff --git a/src/org/usfirst/frc4388/robot/commands/auton/CenterRight.java b/src/org/usfirst/frc4388/robot/commands/auton/CenterRight.java index a31e2c9..06a9a50 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/CenterRight.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/CenterRight.java @@ -1,17 +1,17 @@ package org.usfirst.frc4388.robot.commands.auton; -import org.usfirst.frc4388.robot.commands.DriveAbsoluteTurnMP; + 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.DriveStraightMP; + import org.usfirst.frc4388.robot.commands.DriveTurnBasic; import org.usfirst.frc4388.robot.commands.ElevatorBasic; -import org.usfirst.frc4388.robot.commands.ElevatorSetSpeed; + 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.robot.subsystems.Drive; + import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; import edu.wpi.first.wpilibj.command.CommandGroup; @@ -25,8 +25,10 @@ public class CenterRight extends CommandGroup { public CenterRight() { addSequential(new DriveGyroReset()); - addSequential(new IntakePosition(true)); + addSequential(new DriveSpeedShift(true)); + + addSequential(new DriveStraightBasic(-15, 60, true, true, 0)); addSequential(new DriveTurnBasic(true, -130, 300, MPSoftwareTurnType.TANK)); addSequential(new DriveStraightBasic(53, 60, true, true, 0)); @@ -38,5 +40,7 @@ public class CenterRight extends CommandGroup { addSequential(new IntakePosition(false)); addSequential(new WaitCommand(.2)); addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); + addSequential(new DriveStraightBasic(-20, 60, true, true, 0)); + addSequential(new DriveSpeedShift(false)); } } diff --git a/src/org/usfirst/frc4388/robot/commands/auton/CrossTheBaseLine.java b/src/org/usfirst/frc4388/robot/commands/auton/CrossTheBaseLine.java index 275f37a..44919b5 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/CrossTheBaseLine.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/CrossTheBaseLine.java @@ -1,20 +1,13 @@ package org.usfirst.frc4388.robot.commands.auton; -import org.usfirst.frc4388.robot.commands.DriveAbsoluteTurnMP; + 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.DriveStraightMP; -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.robot.subsystems.Drive; -import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; import edu.wpi.first.wpilibj.command.CommandGroup; -import edu.wpi.first.wpilibj.command.WaitCommand; /** * @@ -25,9 +18,9 @@ public class CrossTheBaseLine extends CommandGroup { addSequential(new DriveGyroReset()); addSequential(new DriveSpeedShift(true)); - addSequential(new DriveSpeedShift(true)); addSequential(new IntakePosition(true)); addSequential(new DriveStraightBasic(-90, 60, true, true, 0)); + 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 62713df..da28b00 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/LeftStartRightScore.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/LeftStartRightScore.java @@ -1,16 +1,17 @@ package org.usfirst.frc4388.robot.commands.auton; -import org.usfirst.frc4388.robot.commands.DriveAbsoluteTurnMP; + 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.DriveStraightMP; + 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.robot.subsystems.Drive; + import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; import edu.wpi.first.wpilibj.command.CommandGroup; @@ -25,8 +26,9 @@ public class LeftStartRightScore extends CommandGroup { { addSequential(new DriveGyroReset()); addSequential(new DriveSpeedShift(true)); - addSequential(new DriveSpeedShift(true)); addSequential(new IntakePosition(true)); + + addSequential(new DriveStraightBasic(215, 60, true, true, 0)); addSequential(new ElevatorBasic(5)); addSequential(new DriveTurnBasic(true, -87, 300, MPSoftwareTurnType.TANK)); @@ -41,6 +43,7 @@ public class LeftStartRightScore extends CommandGroup { addSequential(new WaitCommand(.2)); addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); addSequential(new DriveStraightBasic(-10, 60, true, true, 0)); + addSequential(new DriveSpeedShift(false)); //addSequential(new DriveStraightMP(60, 10, true, true, 0)); // 95 for 112" greenville } } diff --git a/src/org/usfirst/frc4388/robot/commands/auton/LeftSwitchAuton.java b/src/org/usfirst/frc4388/robot/commands/auton/LeftSwitchAuton.java new file mode 100644 index 0000000..1cbe289 --- /dev/null +++ b/src/org/usfirst/frc4388/robot/commands/auton/LeftSwitchAuton.java @@ -0,0 +1,47 @@ +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 LeftSwitchAuton extends CommandGroup { + + public LeftSwitchAuton() { + addSequential(new DriveGyroReset()); + addSequential(new DriveSpeedShift(true)); + addSequential(new IntakePosition(true)); + + + addSequential(new DriveStraightBasic(-130, 60, true, true, 0)); + addSequential(new ElevatorBasic(30)); + addSequential(new DriveTurnBasic(true, -87, 300, MPSoftwareTurnType.TANK)); + addSequential(new ElevatorBasic(30)); + addSequential(new DriveStraightBasic(20, 60, 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(-20, 60, true, true, 0)); + addSequential(new DriveSpeedShift(false)); + + //addSequential(new DriveStraightMP(60, 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 e53b19d..afa0e69 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/RightStartLeftScore.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/RightStartLeftScore.java @@ -1,16 +1,17 @@ package org.usfirst.frc4388.robot.commands.auton; -import org.usfirst.frc4388.robot.commands.DriveAbsoluteTurnMP; + 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.DriveStraightMP; + 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.robot.subsystems.Drive; + import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; import edu.wpi.first.wpilibj.command.CommandGroup; @@ -25,8 +26,9 @@ public class RightStartLeftScore extends CommandGroup { { addSequential(new DriveGyroReset()); addSequential(new DriveSpeedShift(true)); - addSequential(new DriveSpeedShift(true)); addSequential(new IntakePosition(true)); + + addSequential(new DriveStraightBasic(-215, 60, true, true, 0)); addSequential(new ElevatorBasic(5)); addSequential(new DriveTurnBasic(true, 87, 300, MPSoftwareTurnType.TANK)); @@ -41,6 +43,7 @@ public class RightStartLeftScore extends CommandGroup { addSequential(new WaitCommand(.2)); addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); addSequential(new DriveStraightBasic(-10, 60, true, true, 0)); + addSequential(new DriveSpeedShift(false)); //addSequential(new DriveStraightMP(60, 10, true, true, 0)); // 95 for 112" greenville } } diff --git a/src/org/usfirst/frc4388/robot/commands/auton/RightSwitchAuton.java b/src/org/usfirst/frc4388/robot/commands/auton/RightSwitchAuton.java index 928d60d..b5b4a49 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/RightSwitchAuton.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/RightSwitchAuton.java @@ -1,17 +1,17 @@ package org.usfirst.frc4388.robot.commands.auton; -import org.usfirst.frc4388.robot.commands.DriveAbsoluteTurnMP; + 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.DriveStraightMP; + import org.usfirst.frc4388.robot.commands.DriveTurnBasic; import org.usfirst.frc4388.robot.commands.ElevatorBasic; -import org.usfirst.frc4388.robot.commands.ElevatorSetSpeed; + 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.robot.subsystems.Drive; + import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; import edu.wpi.first.wpilibj.command.CommandGroup; @@ -26,6 +26,8 @@ public class RightSwitchAuton extends CommandGroup { addSequential(new DriveGyroReset()); addSequential(new DriveSpeedShift(true)); addSequential(new IntakePosition(true)); + + addSequential(new DriveStraightBasic(-130, 60, true, true, 0)); addSequential(new ElevatorBasic(30)); addSequential(new DriveTurnBasic(true, 87, 300, MPSoftwareTurnType.TANK)); @@ -36,6 +38,8 @@ public class RightSwitchAuton extends CommandGroup { addSequential(new IntakePosition(false)); addSequential(new WaitCommand(.5)); addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); + addSequential(new DriveStraightBasic(-20, 60, true, true, 0)); + addSequential(new DriveSpeedShift(false)); //addSequential(new DriveStraightMP(60, 10, true, true, 0)); // 95 for 112" greenville diff --git a/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom1.java b/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom1.java new file mode 100644 index 0000000..bfd7da8 --- /dev/null +++ b/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom1.java @@ -0,0 +1,47 @@ +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 ScaleFrom1 extends CommandGroup { + + public ScaleFrom1() { + addSequential(new DriveGyroReset()); + addSequential(new DriveSpeedShift(true)); + addSequential(new IntakePosition(true)); + + + addSequential(new DriveStraightBasic(-290, 60, true, true, 0)); + addSequential(new ElevatorBasic(70)); + addSequential(new DriveTurnBasic(true, -90, 100, MPSoftwareTurnType.TANK)); + //addSequential(new DriveStraightBasic(5, 20, 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(-15, 20, true, true, 0)); + addSequential(new ElevatorBasic(10)); + addSequential(new DriveSpeedShift(false)); + + //addSequential(new DriveStraightMP(60, 10, true, true, 0)); // 95 for 112" greenville + + } +} \ No newline at end of file diff --git a/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom3.java b/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom3.java index 840080a..25dfd3e 100644 --- a/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom3.java +++ b/src/org/usfirst/frc4388/robot/commands/auton/ScaleFrom3.java @@ -1,17 +1,17 @@ package org.usfirst.frc4388.robot.commands.auton; -import org.usfirst.frc4388.robot.commands.DriveAbsoluteTurnMP; + 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.DriveStraightMP; + import org.usfirst.frc4388.robot.commands.DriveTurnBasic; import org.usfirst.frc4388.robot.commands.ElevatorBasic; -import org.usfirst.frc4388.robot.commands.ElevatorSetSpeed; + 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.robot.subsystems.Drive; + import org.usfirst.frc4388.utility.MPSoftwarePIDController.MPSoftwareTurnType; import edu.wpi.first.wpilibj.command.CommandGroup; @@ -26,7 +26,8 @@ public class ScaleFrom3 extends CommandGroup { addSequential(new DriveGyroReset()); addSequential(new DriveSpeedShift(true)); addSequential(new IntakePosition(true)); - addSequential(new ElevatorBasic(10)); + + addSequential(new DriveStraightBasic(-290, 60, true, true, 0)); addSequential(new ElevatorBasic(70)); addSequential(new DriveTurnBasic(true, 90, 100, MPSoftwareTurnType.TANK)); @@ -37,7 +38,8 @@ public class ScaleFrom3 extends CommandGroup { addSequential(new WaitCommand(.5)); addSequential(new IntakeSetSpeed(Carriage.CUBE_STOP_SPEED)); addSequential(new DriveStraightBasic(-15, 20, true, true, 0)); - addSequential(new ElevatorBasic(5)); + addSequential(new ElevatorBasic(10)); + addSequential(new DriveSpeedShift(false)); //addSequential(new DriveStraightMP(60, 10, true, true, 0)); // 95 for 112" greenville diff --git a/src/org/usfirst/frc4388/robot/subsystems/Elevator.java b/src/org/usfirst/frc4388/robot/subsystems/Elevator.java index 2deb2b9..49045cf 100644 --- a/src/org/usfirst/frc4388/robot/subsystems/Elevator.java +++ b/src/org/usfirst/frc4388/robot/subsystems/Elevator.java @@ -194,7 +194,7 @@ public class Elevator extends Subsystem implements ControlLoopable public void moveElevatorXbox() { double moveElevatorInput; - double elevatorSafeZone = -30; + double elevatorSafeZone = 15; double elevatorPos = getEncoderElevatorPosition();