From e1c56ab463ba0d41b6a684d0a5e2d59bf80f7ca6 Mon Sep 17 00:00:00 2001 From: jessicabhalerao Date: Tue, 17 Jan 2017 20:41:49 -0800 Subject: [PATCH] finished Auton strat for peg close to boiler in any alliance --- ...AutonGearThenBaselinePegCloseToBoiler.java | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) create mode 100644 src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToBoiler.java diff --git a/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToBoiler.java b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToBoiler.java new file mode 100644 index 0000000..bdb9780 --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/commandgroups/AutonGearThenBaselinePegCloseToBoiler.java @@ -0,0 +1,30 @@ +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 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() { + + + 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 + + } +} -- 2.30.2