diff --git a/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/Robot.java b/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/Robot.java index d71ebc3..c2f3e99 100755 --- a/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/Robot.java +++ b/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/Robot.java @@ -12,11 +12,10 @@ package org.usfirst.frc4905.Galaktika; import org.usfirst.frc4905.Galaktika.commands.AutonomousCommand; +import org.usfirst.frc4905.Galaktika.groupcommands.AutoCombinedLeftRight; import org.usfirst.frc4905.Galaktika.groupcommands.AutoCrossTheLine; import org.usfirst.frc4905.Galaktika.groupcommands.AutoMiddleLoadSwitch; import org.usfirst.frc4905.Galaktika.groupcommands.AutoMiddleRightLoadSwitch; -import org.usfirst.frc4905.Galaktika.groupcommands.AutoPlayoffs; -import org.usfirst.frc4905.Galaktika.groupcommands.AutoQuals; import org.usfirst.frc4905.Galaktika.subsystems.DriveTrain; import org.usfirst.frc4905.Galaktika.subsystems.Elevator; import org.usfirst.frc4905.Galaktika.subsystems.Intake; @@ -129,27 +128,34 @@ public void robotInit() { chooser.addDefault("Do Nothing", new Pair<>(new AutonomousCommand(), '*')); // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=AUTONOMOUS - chooser.addObject("Qualifier Match Starting from Left", new Pair<>(new AutoQuals(false), 'L')); - chooser.addObject("Qualifier Match Starting from Right", new Pair<>(new AutoQuals(false), 'R')); + Command command; + command = new AutoCombinedLeftRight(false, AutoCombinedLeftRight.MatchType.QUALIFIERS); + chooser.addObject("Qualifier Match Starting from Left", new Pair<>(command, 'L')); + command = new AutoCombinedLeftRight(false, AutoCombinedLeftRight.MatchType.QUALIFIERS); + chooser.addObject("Qualifier Match Starting from Right", new Pair<>(command, 'R')); chooser.addObject("Load Switch from Middle", new Pair<>(new AutoMiddleLoadSwitch(false), 'M')); chooser.addObject("Load Switch from Middle Right", new Pair<>(new AutoMiddleRightLoadSwitch(false), 'M')); chooser.addObject("Cross The Line from Left", new Pair<>(new AutoCrossTheLine(false), 'L')); chooser.addObject("Cross The Line from Middle", new Pair<>(new AutoCrossTheLine(false), 'M')); chooser.addObject("Cross The Line from Right", new Pair<>(new AutoCrossTheLine(false), 'R')); - chooser.addObject("Playoff Match Starting from Left", new Pair<>(new AutoPlayoffs(false), 'L')); - chooser.addObject("Playoff Match Starting from Middle", new Pair<>(new AutoPlayoffs(false), 'M')); - chooser.addObject("Playoff Match Starting from Right", new Pair<>(new AutoPlayoffs(false), 'R')); - - chooser.addObject("Delay and Qualifier Match Starting from Left", new Pair<>(new AutoQuals(true), 'L')); - chooser.addObject("Delay and Qualifier Match Starting from Right", new Pair<>(new AutoQuals(true), 'R')); + command = new AutoCombinedLeftRight(false, AutoCombinedLeftRight.MatchType.PLAYOFFS); + chooser.addObject("Playoff Match Starting from Left", new Pair<>(command, 'L')); + command = new AutoCombinedLeftRight(false, AutoCombinedLeftRight.MatchType.PLAYOFFS); + chooser.addObject("Playoff Match Starting from Right", new Pair<>(command, 'R')); + + command = new AutoCombinedLeftRight(true, AutoCombinedLeftRight.MatchType.QUALIFIERS); + chooser.addObject("Delay and Qualifier Match Starting from Left", new Pair<>(command, 'L')); + command = new AutoCombinedLeftRight(true, AutoCombinedLeftRight.MatchType.QUALIFIERS); + chooser.addObject("Delay and Qualifier Match Starting from Right", new Pair<>(command, 'R')); chooser.addObject("Delay and Load Switch from Middle", new Pair<>(new AutoMiddleLoadSwitch(true), 'M')); chooser.addObject("Delay and Load Switch from Middle Right", new Pair<>(new AutoMiddleRightLoadSwitch(true), 'M')); chooser.addObject("Delay and Cross The Line from Left", new Pair<>(new AutoCrossTheLine(true), 'L')); chooser.addObject("Delay and Cross The Line from Middle", new Pair<>(new AutoCrossTheLine(true), 'M')); chooser.addObject("Delay and Cross The Line from Right", new Pair<>(new AutoCrossTheLine(true), 'R')); - chooser.addObject("Delay and Playoff Match Starting from Left", new Pair<>(new AutoPlayoffs(true), 'L')); - chooser.addObject("Delay and Playoff Match Starting from Middle", new Pair<>(new AutoPlayoffs(true), 'M')); - chooser.addObject("Delay and Playoff Match Starting from Right", new Pair<>(new AutoPlayoffs(true), 'R')); + command = new AutoCombinedLeftRight(true, AutoCombinedLeftRight.MatchType.PLAYOFFS); + chooser.addObject("Delay and Playoff Match Starting from Left", new Pair<>(command, 'L')); + command = new AutoCombinedLeftRight(true, AutoCombinedLeftRight.MatchType.PLAYOFFS); + chooser.addObject("Delay and Playoff Match Starting from Right", new Pair<>(command, 'R')); SmartDashboard.putData("Auto mode", chooser); SmartDashboard.putNumber("Autonomous Delay", delaySeconds); @@ -183,7 +189,7 @@ public void robotInit() { */ @Override public void disabledInit(){ - + Trace.getInstance().flushTraceFiles(); Robot.ramps.lockRampsIn(); @@ -192,7 +198,7 @@ public void disabledInit(){ @Override public void disabledPeriodic() { - + Scheduler.getInstance().run(); } diff --git a/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/commands/RunIntakeIn.java b/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/commands/RunIntakeIn.java index c9d0aed..caa4d3b 100644 --- a/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/commands/RunIntakeIn.java +++ b/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/commands/RunIntakeIn.java @@ -22,7 +22,7 @@ public class RunIntakeIn extends Command { Joystick intakeController; private static final double kDeadzone = 0; - public static final double kIntakeSpeed = 0.75; + public static final double kIntakeSpeed = 1; // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_DECLARATIONS // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=VARIABLE_DECLARATIONS diff --git a/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/commands/SetShouldJawsBeOpenStateCommand.java b/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/commands/SetShouldJawsBeOpenStateCommand.java index e5e7bfa..cf1075b 100644 --- a/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/commands/SetShouldJawsBeOpenStateCommand.java +++ b/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/commands/SetShouldJawsBeOpenStateCommand.java @@ -8,9 +8,9 @@ * */ public class SetShouldJawsBeOpenStateCommand extends Command { - + private boolean m_state; - + public SetShouldJawsBeOpenStateCommand(boolean state) { // Use requires() here to declare subsystem dependencies requires(Robot.jaws); @@ -23,7 +23,7 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run protected void execute() { - Robot.jaws.setShouldJawsBeOpenBoolean(m_state); + Robot.jaws.setShouldJawsBeOpenBoolean(m_state); } // Make this return true when this Command no longer needs to run execute() diff --git a/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/commands/ShootCubeInAuto.java b/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/commands/ShootCubeInAuto.java new file mode 100644 index 0000000..5912326 --- /dev/null +++ b/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/commands/ShootCubeInAuto.java @@ -0,0 +1,37 @@ +package org.usfirst.frc4905.Galaktika.commands; + +import org.usfirst.frc4905.Galaktika.Robot; + +import edu.wpi.first.wpilibj.command.TimedCommand; + +/** + * + */ +public class ShootCubeInAuto extends TimedCommand { + + public ShootCubeInAuto(double timeout) { + super(timeout); + // Use requires() here to declare subsystem dependencies + requires(Robot.intake); + } + + // Called just before this Command runs the first time + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + Robot.intake.ejectIntake(1); + } + + // Called once after timeout + protected void end() { + Robot.intake.stopIntake(); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + end(); + } +} diff --git a/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoCombinedLeftRight.java b/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoCombinedLeftRight.java new file mode 100644 index 0000000..fffb3bd --- /dev/null +++ b/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoCombinedLeftRight.java @@ -0,0 +1,191 @@ +package org.usfirst.frc4905.Galaktika.groupcommands; + +import org.usfirst.frc4905.Galaktika.Robot; +import org.usfirst.frc4905.Galaktika.groupcommands.AutoCommand.MatchType; + +import edu.wpi.first.wpilibj.command.CommandGroup; + +public class AutoCombinedLeftRight extends AutoCommand { + private final boolean m_useDelay; + protected final MatchType m_matchType; + public AutoCombinedLeftRight(boolean useDelay, MatchType matchType) { + // Add Commands here: + // e.g. addSequential(new Command1()); + // addSequential(new Command2()); + // these will run in order. + + // To run multiple commands at the same time, + // use addParallel() + // e.g. addParallel(new Command1()); + // addSequential(new Command2()); + // Command1 and Command2 will run in parallel. + + // A command group will require all of the subsystems that each member + // would require. + // e.g. if Command1 requires chassis, and Command2 requires arm, + // a CommandGroup containing them would require both the chassis and the + // arm. + debug("top of AutoQuals constructor"); + m_useDelay = useDelay; + m_matchType = matchType; + debug("bottom of AutoQuals constructor"); + } + + protected void prepareToStart() { + debug("top of prepareToStart"); + if (m_useDelay) { + delay(Robot.getAutonomousDelay()); + } + addAutoCombinedCommands(m_matchType); + debug("bottom of prepareToStart"); + } + + private void loadNearSwitchPlate(char robotPos) { + debug("top of AutoQuals loadNearSwitchPlate"); + closeJaws(false); + parallelJawsOpenClose(); + lowerIntake(); + parallelRetractExtendArms(); + + moveElevatorToSwitchHeight(); + + driveForward(FORWARD_DISTANCE_TO_SWITCH); + if (robotPos == 'R') { + turnLeft(); + } else { + turnRight(); + } + driveForwardToWall(LATERAL_DISTANCE_TO_SWITCH); + openJaws(); + parallelJawsOpenClose(); + debug("bottom of AutoQuals loadNearSwitchPlate"); + } + + private void loadNearScalePlate(char robotPos) { + debug("top of AutoQuals loadNearScalePlate"); + + closeJaws(false); + parallelJawsOpenClose(); + lowerIntake(); + parallelRetractExtendArms(); + + + driveForward(240);//empirical measurement subject to change + + if (robotPos == 'R') { + turnDeltaAngle(-18.4); + } else { + turnDeltaAngle(18.4); + } + moveElevatorToScaleHeight(); + delay(2); + driveForward(45); + + shootCube(2); + + debug("bottom of AutoQuals loadNearScalePlate"); + } + + protected void addAutoCombinedCommands(MatchType matchType) { + char robotPos = Robot.getInitialRobotLocation(); + char switchPlatePos = Robot.getSwitchPlatePosition(); + char scalePlatePos = Robot.getScalePlatePosition(); + if (matchType == MatchType.QUALIFIERS) { + if (switchPlatePos == robotPos) { + loadNearSwitchPlate(robotPos); + } else if (scalePlatePos == robotPos){ + loadNearScalePlate(robotPos); + } else { + crossAutoLine(robotPos); + } + } else { + closeJaws(false); + parallelJawsOpenClose(); + lowerIntake(); + parallelRetractExtendArms(); + + if (robotPos == 'L') { + if (scalePlatePos == 'L') { + loadNearScalePlate('L'); + + } else { + if (switchPlatePos == 'L') { + + moveElevatorToSwitchHeight(); + driveForward(FORWARD_DISTANCE_TO_SWITCH); + turnRight(); + driveForwardToWall(LATERAL_DISTANCE_TO_SWITCH); + openJaws(); + parallelJawsOpenClose(); + + } else { + driveForward(FORWARD_DISTANCE_TO_AUTO_LINE); + } + } + } else if (robotPos == 'R') { + if (scalePlatePos == 'R') { + loadNearScalePlate('R'); + } else { + if (switchPlatePos == 'R') { + moveElevatorToSwitchHeight(); + driveForward(FORWARD_DISTANCE_TO_SWITCH); + turnLeft(); + driveForwardToWall(LATERAL_DISTANCE_TO_SWITCH); + openJaws(); + parallelJawsOpenClose(); + + } else { + //lost the switch and scale, just go forwards + driveForward(FORWARD_DISTANCE_TO_AUTO_LINE); + } + } + } + } + } + + protected void pickupFirstCubeFromScale(double deltaAngle) { + turnDeltaAngle(deltaAngle); + moveElevatorToGroundHeight(); + driveForward(53); + turnToCompassHeading(180); + driveForwardToWall(40); + closeJaws(true); + } + + protected void pickupFirstCubeFromLeftSwitchPlate() { + driveBackward(CLEARANCE_TO_TURN); + turnLeft(); + driveForward(80); + turnRight(); + driveForward(40 + CLEARANCE_TO_TURN); + turnRight(); + moveElevatorToGroundHeight(); + driveForward(33); + closeJaws(true); + } + + protected void pickupFarCubeFromLeftSwitchPlate() { + driveBackward(CLEARANCE_TO_TURN); + turnLeft(); + driveForward(80); + turnRight(); + driveForward(LATERAL_DISTANCE_TO_SCALE_PLATES + CLEARANCE_TO_TURN + 40); + turnRight(); + moveElevatorToGroundHeight(); + driveForward(33); + closeJaws(true); + } + + protected void pickupFirstCubeFromRightSwitchPlate() { + driveBackward(CLEARANCE_TO_TURN); + turnRight(); + driveForward(80); + turnLeft(); + driveForward(40 + CLEARANCE_TO_TURN); + turnLeft(); + moveElevatorToGroundHeight(); + driveForward(33); + closeJaws(true); + } + +} diff --git a/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoCommand.java b/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoCommand.java index 8a03495..2975989 100755 --- a/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoCommand.java +++ b/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoCommand.java @@ -6,7 +6,6 @@ import org.usfirst.frc4905.Galaktika.commands.AutoTimedArmsClose; import org.usfirst.frc4905.Galaktika.commands.AutonomousCommand; import org.usfirst.frc4905.Galaktika.commands.Delay; - import org.usfirst.frc4905.Galaktika.commands.ExtendIntakeInAuto; import org.usfirst.frc4905.Galaktika.commands.GyroPIDTurnDeltaAngle; import org.usfirst.frc4905.Galaktika.commands.JawsOpenClose; @@ -16,6 +15,7 @@ import org.usfirst.frc4905.Galaktika.commands.RetractExtendArms; import org.usfirst.frc4905.Galaktika.commands.SetIntakeShouldBeUpCommand; import org.usfirst.frc4905.Galaktika.commands.SetShouldJawsBeOpenStateCommand; +import org.usfirst.frc4905.Galaktika.commands.ShootCubeInAuto; import org.usfirst.frc4905.Galaktika.commands.TurnToCompassHeading; import org.usfirst.frc4905.Galaktika.commands.ResetElevatorEncoder; import org.usfirst.frc4905.Galaktika.groupcommands.AutoCommand.MoveToWall; @@ -29,214 +29,215 @@ * */ public abstract class AutoCommand extends CommandGroup { - private boolean m_preparedToStart = false; - @Deprecated // ("Use the new Ultrasonic command") - public class MoveToWall extends Command { - - @Override - protected boolean isFinished() { - // TODO Auto-generated method stub - return false; - } - } - - public AutoCommand() { - // Add Commands here: - // e.g. addSequential(new Command1()); - // addSequential(new Command2()); - // these will run in order. - - // To run multiple commands at the same time, - // use addParallel() - // e.g. addParallel(new Command1()); - // addSequential(new Command2()); - // Command1 and Command2 will run in parallel. - - // A command group will require all of the subsystems that each member - // would require. - // e.g. if Command1 requires chassis, and Command2 requires arm, - // a CommandGroup containing them would require both the chassis and the - // arm. - } - - protected static final double FORWARD_DISTANCE_TO_SWITCH = 148.04; - protected static final double LATERAL_DISTANCE_TO_SWITCH = 28.72; - //TODO: Get the following number from CAD - protected static final double FORWARD_DISTANCE_TO_SWITCH_PLATES = 115; - protected static final double FORWARD_DISTANCE_TO_SCALE = 304.25; - protected static final double LATERAL_DISTANCE_TO_SCALE = 15.08; - protected static final double FORWARD_DISTANCE_TO_MIDDLE = 212; - protected static final double LATERAL_DISTANCE_TO_SCALE_PLATES = 200; - protected static final double FORWARD_DISTANCE_BETWEEN_SWITCH_AND_SCALE = 228.16; - protected static final double LATERAL_DISTANCE_BETWEEN_PATHS = 236.6; - protected static final double FORWARD_DISTANCE_TO_AUTO_LINE = 122; - protected static final double LATERAL_DISTANCE_TO_LEFT_SWITCH_PLATE = 41.15; - protected static final double LATERAL_DISTANCE_TO_RIGHT_SWITCH_PLATE = 36.85; - protected static final double LATERAL_DISTANCE_TO_FIRST_CUBE = 50.75; - protected static final double LATERAL_DISTANCE_TO_EXCHANGE_L = 90; - protected static final double LATERAL_DISTANCE_TO_EXCHANGE_R = 154; - protected static final double LATERAL_DISTANCE_TO_EXCHANGE_M = 31.13; + private boolean m_preparedToStart = false; + @Deprecated // ("Use the new Ultrasonic command") + public class MoveToWall extends Command { + + @Override + protected boolean isFinished() { + // TODO Auto-generated method stub + return false; + } + } + + public enum MatchType { + QUALIFIERS, + PLAYOFFS + } + + public AutoCommand() { + // Add Commands here: + // e.g. addSequential(new Command1()); + // addSequential(new Command2()); + // these will run in order. + + // To run multiple commands at the same time, + // use addParallel() + // e.g. addParallel(new Command1()); + // addSequential(new Command2()); + // Command1 and Command2 will run in parallel. + + // A command group will require all of the subsystems that each member + // would require. + // e.g. if Command1 requires chassis, and Command2 requires arm, + // a CommandGroup containing them would require both the chassis and the + // arm. + } + + protected static final double FORWARD_DISTANCE_TO_SWITCH = 120; + protected static final double LATERAL_DISTANCE_TO_SWITCH = 28.72; + //TODO: Get the following number from CAD + protected static final double FORWARD_DISTANCE_TO_SWITCH_PLATES = 100; + protected static final double FORWARD_DISTANCE_TO_SCALE = 304.25; + protected static final double LATERAL_DISTANCE_TO_SCALE = 15.08; + protected static final double FORWARD_DISTANCE_TO_MIDDLE = 212; + protected static final double LATERAL_DISTANCE_TO_SCALE_PLATES = 199.99; + protected static final double FORWARD_DISTANCE_BETWEEN_SWITCH_AND_SCALE = 228.16; + protected static final double LATERAL_DISTANCE_BETWEEN_PATHS = 236.6; + protected static final double FORWARD_DISTANCE_TO_AUTO_LINE = 122; + protected static final double LATERAL_DISTANCE_TO_LEFT_SWITCH_PLATE = 41.15; + protected static final double LATERAL_DISTANCE_TO_RIGHT_SWITCH_PLATE = 36.85; + protected static final double LATERAL_DISTANCE_TO_FIRST_CUBE = 50.75; + protected static final double LATERAL_DISTANCE_TO_EXCHANGE_L = 90; + protected static final double LATERAL_DISTANCE_TO_EXCHANGE_R = 154; + protected static final double LATERAL_DISTANCE_TO_EXCHANGE_M = 31.13; private static final double BUMPER_WIDTH = 1.5; protected static final double CLEARANCE_TO_TURN = 25; - protected void driveForward(double forwardDistanceInches) { - double distanceScaleFactor = Robot.getAutonomousDistanceScaleFactor(); - debug("top of driveForward, Forward Distance = " + - forwardDistanceInches + - "Scale Factor = " + distanceScaleFactor + - "Actual Distance = " + distanceScaleFactor * forwardDistanceInches); + protected void driveForward(double forwardDistanceInches) { + double distanceScaleFactor = Robot.getAutonomousDistanceScaleFactor(); + debug("top of driveForward, Forward Distance = " + + forwardDistanceInches + + "Scale Factor = " + distanceScaleFactor + + "Actual Distance = " + distanceScaleFactor * forwardDistanceInches); addSequential(new MoveUsingEncoderPID(forwardDistanceInches * distanceScaleFactor)); - } - - protected void delay(double delaySeconds) { - addSequential(new Delay(delaySeconds)); - - } - - protected void debug(String information) { - char location = Robot.safelyGetInitialRobotLocation(); - //System.out.println("In AutoCommand.java (" + getClass().getSimpleName() + ")! "); - //System.out.flush(); - // System.out.println("In AutoCommand.java Field Setup: Robot = " + - // location + "! " + - // information); - // System.out.flush(); - } - - public void start() { - debug("top of start"); - if(!m_preparedToStart) { - prepareToStart(); - m_preparedToStart = true; - } - super.start(); - debug("bottom of start"); - } - - protected void prepareToStart() { - debug("top of AutoCommand prepareToStart"); - debug("bottom of AutoCommand prepareToStart"); - } + } - protected void turnRight() { - addSequential(new GyroPIDTurnDeltaAngle(90)); + protected void delay(double delaySeconds) { + addSequential(new Delay(delaySeconds)); - } + } - protected void turnLeft() { - addSequential(new GyroPIDTurnDeltaAngle(-90)); + protected void debug(String information) { + char location = Robot.safelyGetInitialRobotLocation(); + //System.out.println("In AutoCommand.java (" + getClass().getSimpleName() + ")! "); + //System.out.flush(); + // System.out.println("In AutoCommand.java Field Setup: Robot = " + + // location + "! " + + // information); + // System.out.flush(); + } - } + public void start() { + debug("top of start"); + if(!m_preparedToStart) { + prepareToStart(); + m_preparedToStart = true; + } + super.start(); + debug("bottom of start"); + } - protected void turnAround() { - addSequential(new GyroPIDTurnDeltaAngle(180)); - } + protected void prepareToStart() { + debug("top of AutoCommand prepareToStart"); + debug("bottom of AutoCommand prepareToStart"); + } - protected void driveForwardToWall(double estimatedDistance) { - double distanceScaleFactor = Robot.getAutonomousDistanceScaleFactor(); - if (DriveTrain.ULTRASONIC_RANGE_IN_INCHES < estimatedDistance) { - addSequential(new MoveUsingEncoderPID(estimatedDistance*distanceScaleFactor - DriveTrain.ULTRASONIC_RANGE_IN_INCHES)); - } - if (DriveTrain.ULTRASONIC_RANGE_IN_INCHES > 0) { - addSequential(new MoveUsingFrontUltrasonic(BUMPER_WIDTH)); - } - } + protected void turnRight() { + addSequential(new GyroPIDTurnDeltaAngle(90)); - protected void moveElevatorToSwitchHeight() { - addParallel(new MoveElevator(MoveElevator.SWITCH_HEIGHT)); - } + } - protected void moveElevatorToSwitchHeightSequential() { - addSequential(new MoveElevator(MoveElevator.SWITCH_HEIGHT)); - } + protected void turnLeft() { + addSequential(new GyroPIDTurnDeltaAngle(-90)); - protected void moveElevatorToScaleHeight() { - addParallel(new MoveElevator(MoveElevator.HIGH_SCALE_HEIGHT)); - } - - protected void moveElevatorToLowScaleHeight() { - addParallel(new MoveElevator(MoveElevator.LOW_SCALE_HEIGHT)); - } + } - protected void moveElevatorToExchangeHeight() { - addParallel(new MoveElevator(MoveElevator.EXCHANGE_HEIGHT)); - } + protected void turnAround() { + addSequential(new GyroPIDTurnDeltaAngle(180)); + } - protected void moveElevatorToGroundHeight(){ - addParallel(new MoveElevator(MoveElevator.GROUND_LEVEL)); - } + protected void driveForwardToWall(double estimatedDistance) { + double distanceScaleFactor = Robot.getAutonomousDistanceScaleFactor(); + if (DriveTrain.ULTRASONIC_RANGE_IN_INCHES < estimatedDistance) { + addSequential(new MoveUsingEncoderPID(estimatedDistance*distanceScaleFactor - DriveTrain.ULTRASONIC_RANGE_IN_INCHES)); + } + if (DriveTrain.ULTRASONIC_RANGE_IN_INCHES > 0) { + addSequential(new MoveUsingFrontUltrasonic(BUMPER_WIDTH)); + } + } - protected void resetElevatorInAuto() { - addSequential(new ResetElevatorEncoder()); - } + protected void moveElevatorToSwitchHeight() { + addParallel(new MoveElevator(MoveElevator.SWITCH_HEIGHT)); + } + protected void moveElevatorToSwitchHeightSequential() { + addSequential(new MoveElevator(MoveElevator.SWITCH_HEIGHT)); + } - protected void driveBackward(double backwardDistanceInches) { - driveForward(- backwardDistanceInches); - } + protected void moveElevatorToScaleHeight() { + addParallel(new MoveElevator(MoveElevator.HIGH_SCALE_HEIGHT)); + } + protected void moveElevatorToLowScaleHeight() { + addParallel(new MoveElevator(MoveElevator.LOW_SCALE_HEIGHT)); + } - protected void closeArmsInAuto(double timeout) { - addParallel(new AutoTimedArmsClose(timeout)); - } + protected void moveElevatorToExchangeHeight() { + addParallel(new MoveElevator(MoveElevator.EXCHANGE_HEIGHT)); + } - protected void extendIntakeAuto() { - addParallel(new ExtendIntakeInAuto()); - } + protected void moveElevatorToGroundHeight(){ + addParallel(new MoveElevator(MoveElevator.GROUND_LEVEL)); + } - protected void resetEncoderInElevator() { + protected void resetElevatorInAuto() { addSequential(new ResetElevatorEncoder()); - } + } - protected void turnToCompassHeading(double compassHeading) { - addSequential(new TurnToCompassHeading(compassHeading)); - } + protected void parallelJawsOpenClose(){ + addParallel(new JawsOpenClose()); + } - protected void setJawsShouldBeOpenState(boolean state){ - addSequential(new SetShouldJawsBeOpenStateCommand(state)); - } + protected void parallelRetractExtendArms(){ + addParallel(new RetractExtendArms()); + } - protected void parallelJawsOpenClose(){ - addParallel(new JawsOpenClose()); - } + protected void driveBackward(double backwardDistanceInches) { + driveForward(- backwardDistanceInches); + } - protected void parallelRetractExtendArms(){ - addParallel(new RetractExtendArms()); - } - protected void setRetractorShouldBeUp(boolean state){ - addSequential(new SetIntakeShouldBeUpCommand(state)); - } + protected void closeArmsInAuto(double timeout) { + addParallel(new AutoTimedArmsClose(timeout)); + } - protected void turnDeltaAngle(double angle){ - addSequential(new GyroPIDTurnDeltaAngle(angle)); + protected void extendIntakeAuto() { + addParallel(new ExtendIntakeInAuto()); + } - } + protected void resetEncoderInElevator() { + addSequential(new ResetElevatorEncoder()); + } - protected void doubleScaleCube(){ - addSequential(new AutoDoubleScale()); - } + protected void turnToCompassHeading(double compassHeading) { + addSequential(new TurnToCompassHeading(compassHeading)); + } - protected void sameSideDoubleSwitchCube(){ - addSequential(new AutoDoubleSwitch()); - } + protected void openJaws(){ + addSequential(new SetShouldJawsBeOpenStateCommand(true)); + // ENABLE IF BACK-UP IS TOO SOON AFTER OPEN + // delay(0.5); + } - protected void autoQuals(boolean m_useDelay){ - AutoQuals autoQuals = new AutoQuals(m_useDelay); + protected void closeJaws(boolean waitForClose){ + addSequential(new SetShouldJawsBeOpenStateCommand(false)); + if (waitForClose) { + delay(0.5); + } + } - autoQuals.prepareToStart(); - } + protected void raiseIntake(){ + addSequential(new SetIntakeShouldBeUpCommand(true)); + } - protected void autoDoubleScale(boolean m_useDelay){ - AutoDoubleScale autoDoubleScale = new AutoDoubleScale(m_useDelay); - autoDoubleScale.prepareToStart(); - } + protected void lowerIntake(){ + addSequential(new SetIntakeShouldBeUpCommand(false)); + } + protected void turnDeltaAngle(double angle){ + addSequential(new GyroPIDTurnDeltaAngle(angle)); - protected void autoDoubleSwitch(boolean m_useDelay){ - AutoDoubleSwitch autoDoubleSwitch = new AutoDoubleSwitch(m_useDelay); + } - autoDoubleSwitch.prepareToStart(); - } + public void crossAutoLine(char robotPos) { + debug("top of crossAutoLine"); + driveForward(FORWARD_DISTANCE_TO_AUTO_LINE); + debug("bottom of crossAutoLine"); + } + + public void shootCube(double timeout){ + addSequential(new ShootCubeInAuto(timeout)); + } } diff --git a/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoCrossTheLine.java b/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoCrossTheLine.java old mode 100644 new mode 100755 index 7ffc682..699b3cb --- a/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoCrossTheLine.java +++ b/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoCrossTheLine.java @@ -24,12 +24,8 @@ protected void prepareToStart() { if (m_useDelay) { delay(Robot.getAutonomousDelay()); } - setRetractorShouldBeUp(true); - parallelJawsOpenClose(); - parallelRetractExtendArms(); - setJawsShouldBeOpenState(false); + lowerIntake(); - if (robotPos == 'M') { moveElevatorToSwitchHeight(); driveForward(FORWARD_DISTANCE_TO_AUTO_LINE / 2.0); diff --git a/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoDoubleScale.java b/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoDoubleScale.java index 7e4bc9a..0429254 100644 --- a/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoDoubleScale.java +++ b/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoDoubleScale.java @@ -4,80 +4,68 @@ import org.usfirst.frc4905.Galaktika.commands.JawsOpenClose; import org.usfirst.frc4905.Galaktika.commands.RetractExtendArms; import org.usfirst.frc4905.Galaktika.commands.SetIntakeShouldBeUpCommand; +import org.usfirst.frc4905.Galaktika.groupcommands.AutoCommand.MatchType; -public class AutoDoubleScale extends AutoCommand { +public class AutoDoubleScale extends AutoCombinedLeftRight { - boolean m_useDelay; - - public AutoDoubleScale(boolean useDelay) { - if (useDelay) { - m_useDelay = useDelay; - } - } - - public AutoDoubleScale() { - this(false); + public AutoDoubleScale(boolean useDelay, MatchType matchType) { + super(useDelay, matchType); } protected void prepareToStart() { - char robotPos = Robot.getInitialRobotLocation(); - char scalePlatePos = Robot.getScalePlatePosition(); - if (m_useDelay) { - delay(Robot.getAutonomousDelay()); - } - autoQuals(false); - //Only for when robotPos is 'L' or 'R' - if (robotPos == 'L' && scalePlatePos == 'L') { - turnDeltaAngle(17); - driveBackward(53); - moveElevatorToGroundHeight(); - turnAround(); - driveForward(53); - setJawsShouldBeOpenState(false); - delay(0.5);//make sure jaws close, could be changed - moveElevatorToScaleHeight(); - driveBackward(53); - turnAround(); - driveForward(53); - setJawsShouldBeOpenState(true); - System.out.println("Done :D"); - } else if (robotPos == 'R' && scalePlatePos == 'R') { - moveElevatorToScaleHeight(); - turnDeltaAngle(-17); - driveBackward(53); - moveElevatorToGroundHeight(); - turnAround(); - driveForward(53); - setJawsShouldBeOpenState(false); - delay(0.5);//make sure jaws close, could be changed - moveElevatorToScaleHeight(); - driveBackward(53); - turnAround(); - driveForward(53); - setJawsShouldBeOpenState(true); - System.out.println("Done :D"); - } else if (robotPos == 'L' && scalePlatePos == 'R') { - driveForward(FORWARD_DISTANCE_BETWEEN_SWITCH_AND_SCALE); - turnRight(); - driveForward(LATERAL_DISTANCE_TO_SCALE_PLATES); - turnLeft(); - moveElevatorToScaleHeight(); - driveForward(71.18); - setJawsShouldBeOpenState(true); - driveBackward(52); - moveElevatorToGroundHeight(); - turnAround(); - driveForwardToWall(52); - setJawsShouldBeOpenState(false); - delay(0.5); - driveBackward(52); - turnAround(); - // Could be moved v - moveElevatorToScaleHeight(); - driveForward(52); - setJawsShouldBeOpenState(true); - System.out.println("Done :D"); - } else { + super.prepareToStart(); + addAutoDoubleScaleCommands(m_matchType); + } + + protected void addAutoDoubleScaleCommands(MatchType matchType) { + addAutoCombinedCommands(matchType); + char robotPos = Robot.getInitialRobotLocation(); + char scalePlatePos = Robot.getScalePlatePosition(); + char switchPlatePos = Robot.getSwitchPlatePosition(); + + //Only for when robotPos is 'L' or 'R' + if (robotPos == 'L' && scalePlatePos == 'L') { + //Dummy numbers + double deltaAngle = 17; + pickupFirstCubeFromScale(deltaAngle); + moveElevatorToScaleHeight(); + turnToCompassHeading(0); + driveForward(50); + openJaws(); + System.out.println("Done left near side Scale :D"); + } else if (robotPos == 'R' && scalePlatePos == 'R') { + double deltaAngle = -17; + pickupFirstCubeFromScale(deltaAngle); + moveElevatorToScaleHeight(); + turnToCompassHeading(0); + driveForward(50); + openJaws(); + System.out.println("Done right near side Scale :D"); + } else if (robotPos == 'L' && scalePlatePos == 'R') { + if (switchPlatePos == 'L') { + pickupFarCubeFromLeftSwitchPlate(); + } else { + driveForward(FORWARD_DISTANCE_BETWEEN_SWITCH_AND_SCALE - FORWARD_DISTANCE_TO_AUTO_LINE); + turnRight(); + driveForward(LATERAL_DISTANCE_TO_SCALE_PLATES); + turnLeft(); + moveElevatorToScaleHeight(); + driveForward(71.18); + openJaws(); + driveBackward(52); + moveElevatorToGroundHeight(); + turnAround(); + driveForwardToWall(52); + closeJaws(true); + } + driveBackward(52); + turnAround(); + // Could be moved v + moveElevatorToScaleHeight(); + driveForward(52); + openJaws(); + System.out.println("Done left far side Scale :D"); + } else { // RobotPos = R and ScalePos = L driveForward(FORWARD_DISTANCE_BETWEEN_SWITCH_AND_SCALE); turnLeft(); @@ -85,20 +73,21 @@ protected void prepareToStart() { turnRight(); moveElevatorToScaleHeight(); driveForward(71.18); - setJawsShouldBeOpenState(true); + openJaws(); driveBackward(52); moveElevatorToGroundHeight(); turnAround(); driveForwardToWall(52); - setJawsShouldBeOpenState(false); - delay(0.5); + closeJaws(true); driveBackward(52); turnAround(); // Could be moved v moveElevatorToScaleHeight(); driveForward(52); - setJawsShouldBeOpenState(true); - System.out.println("Done :D"); - } - } + openJaws(); + System.out.println("Done right far side Scale :D"); + } + } + + } diff --git a/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoDoubleSwitch.java b/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoDoubleSwitch.java index 5b31983..38e38ff 100644 --- a/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoDoubleSwitch.java +++ b/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoDoubleSwitch.java @@ -4,58 +4,65 @@ import org.usfirst.frc4905.Galaktika.commands.JawsOpenClose; import org.usfirst.frc4905.Galaktika.commands.RetractExtendArms; import org.usfirst.frc4905.Galaktika.commands.SetIntakeShouldBeUpCommand; +import org.usfirst.frc4905.Galaktika.groupcommands.AutoCommand.MatchType; -public class AutoDoubleSwitch extends AutoCommand { +public class AutoDoubleSwitch extends AutoCombinedLeftRight { - boolean m_useDelay; - - public AutoDoubleSwitch(boolean useDelay) { - if (useDelay) { - m_useDelay = useDelay; - } + public AutoDoubleSwitch(boolean useDelay, MatchType matchType) { + super(useDelay, matchType); } - public AutoDoubleSwitch() { - this(false); + protected void prepareToStart() { + super.prepareToStart(); + addAutoDoubleSwitchCommands(m_matchType); + } + + protected void addAutoDoubleSwitchCommands(MatchType matchType) { + char robotPos = Robot.getInitialRobotLocation(); + char switchPlatePos = Robot.getSwitchPlatePosition(); + char scalePlatePos = Robot.getScalePlatePosition(); + addAutoCombinedCommands(matchType); + //Only for when robotPos is 'L' or 'R' + if (robotPos == 'L') { + if (switchPlatePos == 'L' && + (matchType == MatchType.QUALIFIERS || scalePlatePos == 'R')) { + + if (matchType == MatchType.PLAYOFFS && scalePlatePos == 'L') { + pickupFirstCubeFromScale(16.99); + } else { + pickupFirstCubeFromLeftSwitchPlate(); + } + dropCubeOntoSwitch(); + } else if (switchPlatePos == 'R') { + if (scalePlatePos == 'L') { + pickupFirstCubeFromScale(16.99); + } + } + + System.out.println("Done left side :D"); + } else if (robotPos == 'R') { + if ( switchPlatePos == 'R' && + (matchType == MatchType.QUALIFIERS || scalePlatePos == 'L')) { + + if (matchType == MatchType.PLAYOFFS && scalePlatePos == 'R') { + pickupFirstCubeFromScale(-16.99); + } else { + pickupFirstCubeFromRightSwitchPlate(); + } + dropCubeOntoSwitch(); + } else if (switchPlatePos == 'L') { + if (scalePlatePos == 'R') { + pickupFirstCubeFromScale(-16.99); + } + } + System.out.println("Done right side :D"); + } } - protected void prepareToStart() { - char robotPos = Robot.getInitialRobotLocation(); - char switchPlatePos = Robot.getSwitchPlatePosition(); - if (m_useDelay) { - delay(Robot.getAutonomousDelay()); - } - autoQuals(false); - //Only for when robotPos is 'L' or 'R' - if (robotPos == 'L' && switchPlatePos == 'L') { - driveBackward(CLEARANCE_TO_TURN); - turnLeft(); - driveForward(80); - turnRight(); - driveForward(40); - turnRight(); - moveElevatorToGroundHeight(); - driveForward(33); - setJawsShouldBeOpenState(false); - moveElevatorToSwitchHeightSequential(); - driveForwardToWall(13); - setJawsShouldBeOpenState(true); - System.out.println("Done :D"); - } else if (robotPos == 'R' && switchPlatePos == 'R') { - driveBackward(CLEARANCE_TO_TURN); - turnRight(); - driveForward(80); - turnLeft(); - driveForward(40); - turnLeft(); - moveElevatorToGroundHeight(); - driveForward(33); - setJawsShouldBeOpenState(false); - moveElevatorToSwitchHeightSequential(); - driveForwardToWall(13); - setJawsShouldBeOpenState(true); - System.out.println("Done :D"); - } - } + protected void dropCubeOntoSwitch() { + moveElevatorToSwitchHeightSequential(); + driveForwardToWall(13); + openJaws(); + } } diff --git a/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoMiddleLoadSwitch.java b/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoMiddleLoadSwitch.java index 569d66e..47de90c 100755 --- a/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoMiddleLoadSwitch.java +++ b/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoMiddleLoadSwitch.java @@ -17,19 +17,15 @@ public AutoMiddleLoadSwitch(boolean useDelay) { } protected void prepareToStart() { - char platePos = Robot.getSwitchPlatePosition(); if (m_useDelay) { delay(Robot.getAutonomousDelay()); } - parallelJawsOpenClose(); - parallelRetractExtendArms(); - setJawsShouldBeOpenState(false); - setRetractorShouldBeUp(false); - + closeJaws(false); + lowerIntake(); moveElevatorToSwitchHeight(); - driveForward(FORWARD_DISTANCE_TO_AUTO_LINE/2); + char platePos = Robot.getSwitchPlatePosition(); if (platePos == 'R') { turnRight(); driveForward(LATERAL_DISTANCE_TO_RIGHT_SWITCH_PLATE); @@ -42,27 +38,7 @@ protected void prepareToStart() { driveForwardToWall(FORWARD_DISTANCE_TO_SWITCH_PLATES - (FORWARD_DISTANCE_TO_AUTO_LINE / 2.0)); } - setJawsShouldBeOpenState(true); - - /* - if (platePos == 'R') { - - driveBackward(FORWARD_DISTANCE_TO_SWITCH_PLATES - (FORWARD_DISTANCE_TO_AUTO_LINE / 2.0)); - moveElevatorToGroundHeight(); - turnLeft(); - driveForward(LATERAL_DISTANCE_TO_RIGHT_SWITCH_PLATE); - turnRight(); - - } else { - driveBackward(FORWARD_DISTANCE_TO_SWITCH_PLATES - (FORWARD_DISTANCE_TO_AUTO_LINE / 2.0)); - moveElevatorToGroundHeight(); - turnRight(); - driveForward(LATERAL_DISTANCE_TO_RIGHT_SWITCH_PLATE); - turnLeft(); - - } - -*/ + openJaws(); } diff --git a/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoMiddleRightLoadSwitch.java b/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoMiddleRightLoadSwitch.java index ddab59b..77a5653 100755 --- a/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoMiddleRightLoadSwitch.java +++ b/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoMiddleRightLoadSwitch.java @@ -24,13 +24,13 @@ protected void prepareToStart() { if (m_useDelay) { delay(Robot.getAutonomousDelay()); } - setRetractorShouldBeUp(true); - setJawsShouldBeOpenState(false); + raiseIntake(); + closeJaws(false); parallelJawsOpenClose(); parallelRetractExtendArms(); if (platePos == 'L') { - /*System.out.println("We're on the left!"); + System.out.println("Plate on the left!"); driveForward(FORWARD_DISTANCE_TO_AUTO_LINE / 3); delay(1); turnLeft(); @@ -40,29 +40,30 @@ protected void prepareToStart() { turnRight(); delay(1); moveElevatorToSwitchHeight(); - driveForwardToWall(FORWARD_DISTANCE_TO_SWITCH_PLATES - (FORWARD_DISTANCE_TO_AUTO_LINE / 3 * 2.0)); + driveForwardToWall(FORWARD_DISTANCE_TO_SWITCH_PLATES - ((FORWARD_DISTANCE_TO_AUTO_LINE / 3) * 2.0)); - setRetractorShouldBeUp(false); + lowerIntake(); parallelRetractExtendArms(); delay(1); - setJawsShouldBeOpenState(true); - parallelJawsOpenClose();*/ - driveForwardToWall(FORWARD_DISTANCE_TO_SWITCH_PLATES); - //EMERGENCY CODE FIX FOR A POTENTIAL EASY SWITCH AUTO. WE WILL TRY AND GET IT IF THE PLATE IS ON THE RIGHT, - // OTHERWISE WE WILL JUST DRIVEFORWARD + openJaws(); + parallelJawsOpenClose(); + } else { moveElevatorToSwitchHeight(); driveForwardToWall(FORWARD_DISTANCE_TO_SWITCH_PLATES); - setRetractorShouldBeUp(false); + lowerIntake(); parallelRetractExtendArms(); delay(1); - setJawsShouldBeOpenState(true); + openJaws(); parallelJawsOpenClose(); } + delay(2); + driveBackward(30); + /* if (platePos == 'R') { @@ -80,11 +81,8 @@ protected void prepareToStart() { turnLeft(); } - - */ - - + */ } -} +} \ No newline at end of file diff --git a/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoPlayoffs.java b/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoPlayoffs.java deleted file mode 100755 index cd1f06f..0000000 --- a/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoPlayoffs.java +++ /dev/null @@ -1,120 +0,0 @@ -package org.usfirst.frc4905.Galaktika.groupcommands; - -import org.usfirst.frc4905.Galaktika.Robot; -import org.usfirst.frc4905.Galaktika.commands.SetIntakeShouldBeUpCommand; -import org.usfirst.frc4905.Galaktika.commands.SetShouldJawsBeOpenStateCommand; - -import edu.wpi.first.wpilibj.command.CommandGroup; - -public class AutoPlayoffs extends AutoCommand { - boolean m_useDelay; - - public AutoPlayoffs(boolean useDelay) { - // Add Commands here: - // e.g. addSequential(new Command1()); - // addSequential(new Command2()); - // these will run in order. - - // To run multiple commands at the same time, - // use addParallel() - // e.g. addParallel(new Command1()); - // addSequential(new Command2()); - // Command1 and Command2 will run in parallel. - - // A command group will require all of the subsystems that each member - // would require. - // e.g. if Command1 requires chassis, and Command2 requires arm, - // a CommandGroup containing them would require both the chassis and the - // arm. - if (useDelay) { - m_useDelay = useDelay; - } - } - - protected void prepareToStart() { - char robotPos = Robot.getInitialRobotLocation(); - char switchPlatePos = Robot.getSwitchPlatePosition(); - char scalePlatePos = Robot.getScalePlatePosition(); - if (m_useDelay) { - delay(Robot.getAutonomousDelay()); - } - parallelJawsOpenClose(); - parallelRetractExtendArms(); - setJawsShouldBeOpenState(false); - setRetractorShouldBeUp(false); - - - - - if (robotPos == 'L') { - if (scalePlatePos == 'L') { - - - moveElevatorToScaleHeight(); - driveForward(FORWARD_DISTANCE_TO_AUTO_LINE); - turnDeltaAngle(-4.8); - - driveForward(177.6);//assumed distance from pythagorean theorem to approach plate - setJawsShouldBeOpenState(true); - - } else { - if (switchPlatePos == 'L') { - - moveElevatorToSwitchHeight(); - driveForward(FORWARD_DISTANCE_TO_SWITCH); - turnRight(); - driveForwardToWall(LATERAL_DISTANCE_TO_SWITCH); - setJawsShouldBeOpenState(true); - - } else { - driveForward(FORWARD_DISTANCE_TO_AUTO_LINE); - } - } - } else if (robotPos == 'R') { - if (scalePlatePos == 'R') { - moveElevatorToScaleHeight(); - driveForward(FORWARD_DISTANCE_TO_AUTO_LINE); - turnDeltaAngle(4.8); - - driveForward(177.6);//assumed distance from pythagorean theorem to approach plate - setJawsShouldBeOpenState(true); - } else { - if (switchPlatePos == 'R') { - - moveElevatorToSwitchHeight(); - driveForward(FORWARD_DISTANCE_TO_SWITCH); - turnLeft(); - driveForwardToWall(LATERAL_DISTANCE_TO_SWITCH); - setJawsShouldBeOpenState(true); - - } else { - //lost the switch and scale, just go forwards - driveForward(FORWARD_DISTANCE_TO_AUTO_LINE); - } - } - } else { - //we are in the middle - - moveElevatorToSwitchHeight(); - driveForward(FORWARD_DISTANCE_TO_AUTO_LINE/2); - - if (switchPlatePos == 'R') { - turnRight(); - driveForward(LATERAL_DISTANCE_TO_RIGHT_SWITCH_PLATE); - turnLeft(); - driveForwardToWall(FORWARD_DISTANCE_TO_SWITCH_PLATES - (FORWARD_DISTANCE_TO_AUTO_LINE / 2.0)); - - - } else { - turnLeft(); - driveForward(LATERAL_DISTANCE_TO_LEFT_SWITCH_PLATE); - turnRight(); - driveForwardToWall(FORWARD_DISTANCE_TO_SWITCH_PLATES - (FORWARD_DISTANCE_TO_AUTO_LINE / 2.0)); - } - - setJawsShouldBeOpenState(true); - } - } - - -} diff --git a/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoQuals.java b/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoQuals.java deleted file mode 100755 index 20ff9cd..0000000 --- a/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoQuals.java +++ /dev/null @@ -1,132 +0,0 @@ -package org.usfirst.frc4905.Galaktika.groupcommands; - -import org.usfirst.frc4905.Galaktika.Robot; - -import edu.wpi.first.wpilibj.command.CommandGroup; - -public class AutoQuals extends AutoCommand { - boolean m_useDelay; - public AutoQuals(boolean useDelay) { - // Add Commands here: - // e.g. addSequential(new Command1()); - // addSequential(new Command2()); - // these will run in order. - - // To run multiple commands at the same time, - // use addParallel() - // e.g. addParallel(new Command1()); - // addSequential(new Command2()); - // Command1 and Command2 will run in parallel. - - // A command group will require all of the subsystems that each member - // would require. - // e.g. if Command1 requires chassis, and Command2 requires arm, - // a CommandGroup containing them would require both the chassis and the - // arm. - debug("top of AutoQuals constructor"); - if (useDelay) { - m_useDelay = useDelay; - } - debug("bottom of AutoQuals constructor"); - } - - public void crossAutoLine(char robotPos) { - debug("top of crossAutoLine"); - driveForward(FORWARD_DISTANCE_TO_AUTO_LINE); - debug("bottom of crossAutoLine"); - } - - private void returnToLoadExchange(char robotPos) { - turnAround(); - driveForward(FORWARD_DISTANCE_TO_AUTO_LINE / 2.0); - if (robotPos == 'R' || robotPos == 'M') { - turnRight(); - driveForward(LATERAL_DISTANCE_TO_EXCHANGE_R); - turnLeft(); - } else { - turnLeft(); - driveForward(LATERAL_DISTANCE_TO_EXCHANGE_L); - turnRight(); - } - moveElevatorToExchangeHeight(); - driveForward(FORWARD_DISTANCE_TO_AUTO_LINE / 2.0); - } - - private void loadNearSwitchPlate(char robotPos) { - debug("top of AutoQuals loadNearSwitchPlate"); - parallelJawsOpenClose(); - parallelRetractExtendArms(); - setJawsShouldBeOpenState(false); - setRetractorShouldBeUp(false); - - moveElevatorToSwitchHeight(); - - driveForward(FORWARD_DISTANCE_TO_SWITCH); - if (robotPos == 'R') { - turnLeft(); - } else { - turnRight(); - } - driveForwardToWall(LATERAL_DISTANCE_TO_SWITCH); - setJawsShouldBeOpenState(true); - - debug("bottom of AutoQuals loadNearSwitchPlate"); - } - - private void loadNearScalePlate(char robotPos) { - debug("top of AutoQuals loadNearScalePlate"); - - parallelJawsOpenClose(); - parallelRetractExtendArms(); - setJawsShouldBeOpenState(false); - setRetractorShouldBeUp(false); - - moveElevatorToScaleHeight(); - driveForward(FORWARD_DISTANCE_TO_AUTO_LINE); - - if (robotPos == 'R') { - turnDeltaAngle(-4.8); - } else { - turnDeltaAngle(4.8); - } - - driveForward(177.6); - - setJawsShouldBeOpenState(true); - - debug("bottom of AutoQuals loadNearScalePlate"); - } - - protected void prepareToStart() { - debug("top of prepareToStart"); - char robotPos = Robot.getInitialRobotLocation(); - char switchPlatePos = Robot.getSwitchPlatePosition(); - char scalePlatePos = Robot.getScalePlatePosition(); - if (m_useDelay) { - delay(Robot.getAutonomousDelay()); - } - - if (switchPlatePos == robotPos) { - - loadNearSwitchPlate(robotPos); - } else if (scalePlatePos == robotPos){ - loadNearScalePlate(robotPos); - } else { - crossAutoLine(robotPos); - - } - debug("bottom of prepareToStart"); - } - - private void returnToLoadExchange() { - - turnLeft(); - turnLeft(); - driveForward(FORWARD_DISTANCE_TO_AUTO_LINE / 2.0); - turnRight(); - driveForward(LATERAL_DISTANCE_TO_EXCHANGE_M); - turnLeft(); - driveForward(FORWARD_DISTANCE_TO_AUTO_LINE / 2.0); - moveElevatorToExchangeHeight(); - } -} diff --git a/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoScaleThenSwitch.java b/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoScaleThenSwitch.java index fb5378e..9f7a0e2 100644 --- a/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoScaleThenSwitch.java +++ b/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoScaleThenSwitch.java @@ -25,10 +25,8 @@ protected void prepareToStart() { if (m_useDelay) { delay(Robot.getAutonomousDelay()); } - parallelJawsOpenClose(); - parallelRetractExtendArms(); - setJawsShouldBeOpenState(false); - setRetractorShouldBeUp(false); + closeJaws(false); + lowerIntake(); //Only for when robotPos is 'L' or 'R' if (robotPos == 'L' && scalePlatePos == 'L' && switchPlatePos == 'L') { @@ -36,32 +34,30 @@ protected void prepareToStart() { driveForward(FORWARD_DISTANCE_TO_AUTO_LINE); turnDeltaAngle(-4.8); driveForward(177.6);//assumed distance from pythagorean theorem to approach plate - setJawsShouldBeOpenState(true); + openJaws(); turnDeltaAngle(17); driveBackward(53); moveElevatorToGroundHeight(); turnAround(); driveForward(53); - setJawsShouldBeOpenState(false); - delay(0.5);//make sure jaws close, could be changed + closeJaws(true); moveElevatorToSwitchHeightSequential(); - setJawsShouldBeOpenState(true); + openJaws(); System.out.println("Done :D"); } else if (robotPos == 'R' && scalePlatePos == 'R' && switchPlatePos == 'R') { moveElevatorToScaleHeight(); driveForward(FORWARD_DISTANCE_TO_AUTO_LINE); turnDeltaAngle(4.8); driveForward(177.6);//assumed distance from pythagorean theorem to approach plate - setJawsShouldBeOpenState(true); + openJaws(); turnDeltaAngle(-17); driveBackward(53); moveElevatorToGroundHeight(); turnAround(); driveForward(53); - setJawsShouldBeOpenState(false); - delay(0.5);//make sure jaws close, could be changed + closeJaws(true); moveElevatorToSwitchHeightSequential(); - setJawsShouldBeOpenState(true); + openJaws(); } } } diff --git a/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoTripleScaleScaleSwitch.java b/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoTripleFromScale.java similarity index 66% rename from eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoTripleScaleScaleSwitch.java rename to eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoTripleFromScale.java index 06279c8..9b23d53 100644 --- a/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoTripleScaleScaleSwitch.java +++ b/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoTripleFromScale.java @@ -5,46 +5,34 @@ import org.usfirst.frc4905.Galaktika.commands.RetractExtendArms; import org.usfirst.frc4905.Galaktika.commands.SetIntakeShouldBeUpCommand; -public class AutoTripleScaleScaleSwitch extends AutoCommand { - - boolean m_useDelay; - - public AutoTripleScaleScaleSwitch(boolean useDelay) { - if (useDelay) { - m_useDelay = useDelay; - } - } - - public AutoTripleScaleScaleSwitch() { - this(false); +public class AutoTripleFromScale extends AutoDoubleScale { + public AutoTripleFromScale(boolean useDelay, MatchType matchType) { + super(useDelay, matchType); } protected void prepareToStart() { + super.prepareToStart(); char robotPos = Robot.getInitialRobotLocation(); char scalePlatePos = Robot.getScalePlatePosition(); - if (m_useDelay) { - delay(Robot.getAutonomousDelay()); - } - autoDoubleScale(false); //Only for when robotPos is 'L' or 'R' if (robotPos == 'L' && scalePlatePos == 'L') { driveBackward(53); moveElevatorToGroundHeight(); turnDeltaAngle(175); driveForward(53); - setJawsShouldBeOpenState(false); + closeJaws(true); moveElevatorToSwitchHeightSequential(); - setJawsShouldBeOpenState(true); + openJaws(); System.out.println("Done :D"); } else if (robotPos == 'R' && scalePlatePos == 'R') { driveBackward(53); moveElevatorToGroundHeight(); turnDeltaAngle(-175); driveForward(53); - setJawsShouldBeOpenState(false); + closeJaws(true); moveElevatorToSwitchHeightSequential(); - setJawsShouldBeOpenState(true); + openJaws(); System.out.println("Done :D"); } } diff --git a/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoTripleSwitch.java b/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoTripleFromSwitch.java similarity index 60% rename from eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoTripleSwitch.java rename to eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoTripleFromSwitch.java index c4f2964..b854957 100644 --- a/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoTripleSwitch.java +++ b/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/groupcommands/AutoTripleFromSwitch.java @@ -5,52 +5,36 @@ import org.usfirst.frc4905.Galaktika.commands.RetractExtendArms; import org.usfirst.frc4905.Galaktika.commands.SetIntakeShouldBeUpCommand; -public class AutoTripleSwitch extends AutoCommand { - - boolean m_useDelay; - - public AutoTripleSwitch(boolean useDelay) { - if (useDelay) { - m_useDelay = useDelay; - } - } - - public AutoTripleSwitch() { - this(false); +public class AutoTripleFromSwitch extends AutoDoubleSwitch { + public AutoTripleFromSwitch(boolean useDelay, MatchType matchType) { + super(useDelay, matchType); } protected void prepareToStart() { + super.prepareToStart(); // TRIPLE SWITCH = NOT LIKELY TO HAVE TIME char robotPos = Robot.getInitialRobotLocation(); char switchPlatePos = Robot.getSwitchPlatePosition(); - if (m_useDelay) { - delay(Robot.getAutonomousDelay()); - } - autoDoubleSwitch(false); //Only for when robotPos is 'L' or 'R' if (robotPos == 'L' && switchPlatePos == 'L') { - setJawsShouldBeOpenState(true); driveBackward(CLEARANCE_TO_TURN); turnLeft(); driveForward(15.4); turnRight(); driveForward(13); - setJawsShouldBeOpenState(false); - moveElevatorToSwitchHeightSequential(); - setJawsShouldBeOpenState(true); + closeJaws(true); + dropCubeOntoSwitch(); System.out.println("Done :D"); } else if (robotPos == 'R' && switchPlatePos == 'R') { - setJawsShouldBeOpenState(true); driveBackward(CLEARANCE_TO_TURN); turnRight(); driveForward(15.4); turnLeft(); driveForward(13); - setJawsShouldBeOpenState(false); - moveElevatorToSwitchHeightSequential(); - setJawsShouldBeOpenState(true); + closeJaws(true); + dropCubeOntoSwitch(); System.out.println("Done :D"); } } diff --git a/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/subsystems/DriveTrain.java b/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/subsystems/DriveTrain.java index df4632f..81bd527 100755 --- a/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/subsystems/DriveTrain.java +++ b/eclipse/Galaktika/src/org/usfirst/frc4905/Galaktika/subsystems/DriveTrain.java @@ -69,7 +69,7 @@ public class DriveTrain extends Subsystem { public static double getMaxVelocity() { return kEncoderMaxVelocity; - + } public static double getMaxAcceleration() { @@ -326,7 +326,7 @@ static public double calculateOutput(double output, double previousOutput, output = previousOutput + deltaOutput; } return output; - + } private class EncoderPIDOut implements PIDOutput { private double m_previousOutput = 0; @@ -337,7 +337,7 @@ public EncoderPIDOut(double maxAllowableDelta) { } @Override public void pidWrite(double output) { - + output = -output; /* if(output != 0.0) { @@ -379,9 +379,9 @@ public void enableEncoderPID(double setpoint) { m_encoderPID.setSetpoint(setpoint + currentEncoderPosition); System.out.println("Current position: " + currentEncoderPosition); if (setpoint + currentEncoderPosition > currentEncoderPosition) { - System.out.println("Moving backwards, distance = " + Math.abs(setpoint)); - } else { System.out.println("Moving forwards, distance = " + Math.abs(setpoint)); + } else { + System.out.println("Moving backwards, distance = " + Math.abs(setpoint)); } m_encoderPID.enable(); } @@ -427,14 +427,14 @@ public GyroPIDOut(double maxAllowableDelta) { @Override public void pidWrite(double output) { - double kMinOutput = 0.125; +/* double kMinOutput = 0.125; if((output != 0.0) && (Math.abs(output) < kMinOutput)) { if(output < 0.0) { output = -kMinOutput; } else { output = kMinOutput; } - } + } */ move(0,output,false); m_previousOutput = output; } @@ -445,7 +445,7 @@ public void initGyroPIDDeltaAngle() { double gyroPIDI = 0.00; double gyroPIDD = 0.0; double gyroPIDF = 0.0; - double gyroPIDOutputRange = 0.5; + double gyroPIDOutputRange = 0.4; double gyroPIDAbsTolerance = 5; double maxAllowableDelta = 0.2; GyroPIDIn gyroPIDIn = new GyroPIDIn(); @@ -682,4 +682,6 @@ public void resetRobotState() { m_savedAngle = RobotMap.navX.getRobotAngle(); } + + }