From: Cindy Zhang Date: Mon, 15 Feb 2016 23:41:00 +0000 (-0800) Subject: update command names X-Git-Url: http://challenge-bot.com/repos/?p=3501%2Fstronghold-2016;a=commitdiff_plain;h=dfcc1b2194b4c82b295440af9b408b6b46f40e03 update command names --- diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/AlignToScore.java b/src/org/usfirst/frc/team3501/robot/commands/auton/AlignToScore.java index 31811723..918a9fca 100755 --- a/src/org/usfirst/frc/team3501/robot/commands/auton/AlignToScore.java +++ b/src/org/usfirst/frc/team3501/robot/commands/auton/AlignToScore.java @@ -1,6 +1,8 @@ package org.usfirst.frc.team3501.robot.commands.auton; import org.usfirst.frc.team3501.robot.Robot; +import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance; +import org.usfirst.frc.team3501.robot.commands.driving.TurnForAngle; import edu.wpi.first.wpilibj.command.CommandGroup; @@ -28,6 +30,7 @@ public class AlignToScore extends CommandGroup { private final static double DIST_CASTLE_WALL_TO_FRONT_GOAL = 0; private final double DEFAULT_SPEED = 0.5; + private final double maxTimeout = 5; // in inches // assuming that positive angle means turning right @@ -72,42 +75,42 @@ public class AlignToScore extends CommandGroup { // position 1 is always the low bar - addSequential(new DriveForDistance(POS1_DIST1, DEFAULT_SPEED)); - addSequential(new TurnForAngle(POS1_TURN1)); - addSequential(new DriveForDistance(POS1_DIST2, DEFAULT_SPEED)); + addSequential(new DriveDistance(POS1_DIST1, DEFAULT_SPEED)); + addSequential(new TurnForAngle(POS1_TURN1, maxTimeout)); + addSequential(new DriveDistance(POS1_DIST2, DEFAULT_SPEED)); horizontalDistToGoal = 0; } else if (position == 2) { - addSequential(new DriveForDistance(POS2_DIST1, DEFAULT_SPEED)); - addSequential(new TurnForAngle(POS2_TURN1)); - addSequential(new DriveForDistance(POS2_DIST2, DEFAULT_SPEED)); + addSequential(new DriveDistance(POS2_DIST1, DEFAULT_SPEED)); + addSequential(new TurnForAngle(POS2_TURN1, maxTimeout)); + addSequential(new DriveDistance(POS2_DIST2, DEFAULT_SPEED)); horizontalDistToGoal = 0; } else if (position == 3) { - addSequential(new DriveForDistance(POS3_DIST1, DEFAULT_SPEED)); - addSequential(new TurnForAngle(POS3_TURN1)); - addSequential(new DriveForDistance(POS3_DIST2, DEFAULT_SPEED)); - addSequential(new TurnForAngle(POS3_TURN2)); - addSequential(new DriveForDistance(POS3_DIST3, DEFAULT_SPEED)); + addSequential(new DriveDistance(POS3_DIST1, DEFAULT_SPEED)); + addSequential(new TurnForAngle(POS3_TURN1, maxTimeout)); + addSequential(new DriveDistance(POS3_DIST2, DEFAULT_SPEED)); + addSequential(new TurnForAngle(POS3_TURN2, maxTimeout)); + addSequential(new DriveDistance(POS3_DIST3, DEFAULT_SPEED)); horizontalDistToGoal = 0; } else if (position == 4) { - addSequential(new DriveForDistance(POS4_DIST1, DEFAULT_SPEED)); - addSequential(new TurnForAngle(POS4_TURN1)); - addSequential(new DriveForDistance(POS4_DIST2, DEFAULT_SPEED)); - addSequential(new TurnForAngle(POS4_TURN2)); - addSequential(new DriveForDistance(POS4_DIST3, DEFAULT_SPEED)); + addSequential(new DriveDistance(POS4_DIST1, DEFAULT_SPEED)); + addSequential(new TurnForAngle(POS4_TURN1, maxTimeout)); + addSequential(new DriveDistance(POS4_DIST2, DEFAULT_SPEED)); + addSequential(new TurnForAngle(POS4_TURN2, maxTimeout)); + addSequential(new DriveDistance(POS4_DIST3, DEFAULT_SPEED)); horizontalDistToGoal = 0; } else if (position == 5) { - addSequential(new DriveForDistance(POS5_DIST1, DEFAULT_SPEED)); - addSequential(new TurnForAngle(POS5_TURN1)); - addSequential(new DriveForDistance(POS5_DIST2, DEFAULT_SPEED)); - addSequential(new TurnForAngle(POS5_TURN2)); - addSequential(new DriveForDistance(POS5_DIST3, DEFAULT_SPEED)); + addSequential(new DriveDistance(POS5_DIST1, DEFAULT_SPEED)); + addSequential(new TurnForAngle(POS5_TURN1, maxTimeout)); + addSequential(new DriveDistance(POS5_DIST2, DEFAULT_SPEED)); + addSequential(new TurnForAngle(POS5_TURN2, maxTimeout)); + addSequential(new DriveDistance(POS5_DIST3, DEFAULT_SPEED)); horizontalDistToGoal = 0; } diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/PassLowBar.java b/src/org/usfirst/frc/team3501/robot/commands/auton/PassLowBar.java index c03ce143..cbdaeef6 100755 --- a/src/org/usfirst/frc/team3501/robot/commands/auton/PassLowBar.java +++ b/src/org/usfirst/frc/team3501/robot/commands/auton/PassLowBar.java @@ -1,5 +1,7 @@ package org.usfirst.frc.team3501.robot.commands.auton; +import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance; + import edu.wpi.first.wpilibj.command.CommandGroup; /*** @@ -8,7 +10,7 @@ import edu.wpi.first.wpilibj.command.CommandGroup; * dependency on sensors: encoders * dependency on subsystems: drivetrain * dependency on other commands: DriveForDist - * + * * pre-condition: robot is flush against the ramp of the outerworks in front of * the low bar * @@ -24,11 +26,12 @@ public class PassLowBar extends CommandGroup { private final double DEFAULT_SPEED = 0.5; public PassLowBar() { - // TODO: need to add sequential for retracting the arms and shooting hood once those commands are made - addSequential(new DriveForDistance(DISTANCE, DEFAULT_SPEED)); + // TODO: need to add sequential for retracting the arms and shooting hood + // once those commands are made + addSequential(new DriveDistance(DISTANCE, DEFAULT_SPEED)); } public PassLowBar(double speed) { - addSequential(new DriveForDistance(DISTANCE, speed)); + addSequential(new DriveDistance(DISTANCE, speed)); } } diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/PassRampart.java b/src/org/usfirst/frc/team3501/robot/commands/auton/PassRampart.java index e0dfb490..426063dc 100755 --- a/src/org/usfirst/frc/team3501/robot/commands/auton/PassRampart.java +++ b/src/org/usfirst/frc/team3501/robot/commands/auton/PassRampart.java @@ -1,5 +1,7 @@ package org.usfirst.frc.team3501.robot.commands.auton; +import org.usfirst.frc.team3501.robot.commands.driving.DriveForTime; + import edu.wpi.first.wpilibj.command.CommandGroup; /*** diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/PassRoughTerrain.java b/src/org/usfirst/frc/team3501/robot/commands/auton/PassRoughTerrain.java index 4fe09f4f..8851edf6 100755 --- a/src/org/usfirst/frc/team3501/robot/commands/auton/PassRoughTerrain.java +++ b/src/org/usfirst/frc/team3501/robot/commands/auton/PassRoughTerrain.java @@ -1,5 +1,7 @@ package org.usfirst.frc.team3501.robot.commands.auton; +import org.usfirst.frc.team3501.robot.commands.driving.DriveForTime; + import edu.wpi.first.wpilibj.command.CommandGroup; /***