From a37c420e300213dbfe615a8c62180578027184ec Mon Sep 17 00:00:00 2001 From: jessicabhalerao Date: Thu, 19 Jan 2017 21:04:22 -0800 Subject: [PATCH] FINALLY Finished Auton Command Group --- src/org/usfirst/frc/team3501/robot/Robot.java | 1 + ...AutonGearThenBaselinePegCloseToBoiler.java | 74 +++++++++++++++---- ...arThenBaselinePegCloseToRetrievalZone.java | 33 --------- 3 files changed, 60 insertions(+), 48 deletions(-) delete mode 100644 src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToRetrievalZone.java diff --git a/src/org/usfirst/frc/team3501/robot/Robot.java b/src/org/usfirst/frc/team3501/robot/Robot.java index 5fbfe40..58f5d0b 100644 --- a/src/org/usfirst/frc/team3501/robot/Robot.java +++ b/src/org/usfirst/frc/team3501/robot/Robot.java @@ -42,6 +42,7 @@ public class Robot extends IterativeRobot { @Override public void autonomousInit() { Scheduler.getInstance().add(new TimeDrive(1.5, 0.4)); + } @Override diff --git a/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToBoiler.java b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToBoiler.java index 5466e92..b674b35 100644 --- a/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToBoiler.java +++ b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToBoiler.java @@ -8,25 +8,69 @@ import org.usfirst.frc.team3501.robot.commands.driving.TurnForAngle; import edu.wpi.first.wpilibj.command.CommandGroup; /** - * + * this is an auton strategy to cross the baseline and put a gear on a peg + * requires predetermined starting point anywhere except between 109.1767 inches + * and 205.7286 inches from the right side of the arena this program chooses + * which peg to go for based on the starting point */ -public class AutonGearThenBaselinePegCloseToBoiler extends CommandGroup { +public class AutonGearThenBaselinePegCloseToBoiler extends CommandGroup - private static final int MOTOR_VALUE_TURN = 60; - private static final int MOTOR_VALUE_FORWARD = 60; - private static final double DISTANCE_FROM_BOILER_WALL = 111.5676; +{ - public AutonGearThenBaselinePegCloseToBoiler() { + private static final double LENGTH_OF_PEG = 10.5; + private static final double PEG_EXTENDED = 136.5664; + private static final int WIDTH_OF_ARENA = 324; + private static final double MOTOR_VALUE_TURN = 0.5;// this is probably not + // correct + private static final double MOTOR_VALUE_FORWARD = 0.5;// random + double DISTANCE_FROM_BOILER_WALL = 105.7126; // This is the starting point + // from the right side of the + // arena in inches + public AutonGearThenBaselinePegCloseToBoiler() { requires(Robot.getDriveTrain()); - addSequential( - new DriveDistance(DISTANCE_FROM_BOILER_WALL / Math.sqrt(3) + 52.4614, - MOTOR_VALUE_FORWARD)); - addSequential(new TurnForAngle(60, Direction.LEFT, MOTOR_VALUE_TURN)); - addSequential(new DriveDistance( - 143.3272 - 2 * DISTANCE_FROM_BOILER_WALL / Math.sqrt(3) - 10.5, - MOTOR_VALUE_FORWARD)); - // does not include drift distance - + if (DISTANCE_FROM_BOILER_WALL > 0 && DISTANCE_FROM_BOILER_WALL < 109.1767) // right + // peg + // path + { + addSequential( + new DriveDistance(DISTANCE_FROM_BOILER_WALL / Math.sqrt(3) + 50.2168, + MOTOR_VALUE_FORWARD)); // drive straight into extended line of peg + addSequential(new TurnForAngle(60, Direction.LEFT, MOTOR_VALUE_TURN));// position + // robot + // for + // optimal + // gear + // placement + addSequential(new DriveDistance(PEG_EXTENDED + - 2 * DISTANCE_FROM_BOILER_WALL / Math.sqrt(3) - LENGTH_OF_PEG, + MOTOR_VALUE_FORWARD));// put gear on peg + // does not include drift distance + } else if (DISTANCE_FROM_BOILER_WALL > 205.7286 + && DISTANCE_FROM_BOILER_WALL < 324) // mirror of right peg path { + { + DISTANCE_FROM_BOILER_WALL = WIDTH_OF_ARENA - DISTANCE_FROM_BOILER_WALL; // Turning + // DISTANCE_FROM_BOILER_WALL + // into + // starting + // point + // from + // left + // side + // by + // reversing + // it + addSequential( + new DriveDistance(DISTANCE_FROM_BOILER_WALL / Math.sqrt(3) + 50.2168, + MOTOR_VALUE_FORWARD)); + addSequential(new TurnForAngle(60, Direction.RIGHT, MOTOR_VALUE_TURN)); + addSequential(new DriveDistance(PEG_EXTENDED + - 2 * DISTANCE_FROM_BOILER_WALL / Math.sqrt(3) - LENGTH_OF_PEG, + MOTOR_VALUE_FORWARD)); + // does not include drift distance + } else { + System.out.println( + "DO NOT START HERE!!! MOVE CLOSER TO WALL OR LOSE ALL AUTON POINTS!!"); + } } } diff --git a/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToRetrievalZone.java b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToRetrievalZone.java deleted file mode 100644 index edb7d95..0000000 --- a/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToRetrievalZone.java +++ /dev/null @@ -1,33 +0,0 @@ -package org.usfirst.frc.team3501.robot.commandgroups; - -import org.usfirst.frc.team3501.robot.Constants.Direction; -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; - -/** - * - */ -public class AutonGearThenBaselinePegCloseToRetrievalZone extends CommandGroup { - - private static final double DISTANCE_FROM_RETRIEVAL_ZONE_WALL = 111.5676; - private static final int MOTOR_VALUE_TURN = 60; - private static final int MOTOR_VALUE_FORWARD = 60; - - public AutonGearThenBaselinePegCloseToRetrievalZone() { - - double x = DISTANCE_FROM_RETRIEVAL_ZONE_WALL; - requires(Robot.getDriveTrain()); - addSequential(new DriveDistance( - DISTANCE_FROM_RETRIEVAL_ZONE_WALL / Math.sqrt(3) + 52.4614, - MOTOR_VALUE_FORWARD)); - addSequential(new TurnForAngle(60, Direction.RIGHT, MOTOR_VALUE_TURN)); - addSequential(new DriveDistance( - 143.3272 - 2 * DISTANCE_FROM_RETRIEVAL_ZONE_WALL / Math.sqrt(3) - 10.5, - MOTOR_VALUE_FORWARD)); - // does not include drift distance - - } -} -- 2.30.2