1 package org
.usfirst
.frc
.team3501
.robot
.commandgroups
;
3 import org
.usfirst
.frc
.team3501
.robot
.Constants
.Direction
;
4 import org
.usfirst
.frc
.team3501
.robot
.Robot
;
5 import org
.usfirst
.frc
.team3501
.robot
.commands
.driving
.DriveDistance
;
6 import org
.usfirst
.frc
.team3501
.robot
.commands
.driving
.TurnForAngle
;
8 import edu
.wpi
.first
.wpilibj
.command
.CommandGroup
;
11 * this is an auton strategy to cross the baseline and put a gear on a peg
12 * requires predetermined starting point anywhere except between 109.1767 inches
13 * and 205.7286 inches from the right side of the arena this program chooses
14 * which peg to go for based on the starting point
16 public class AutonGearThenBaselinePegCloseToBoiler
extends CommandGroup
20 private static final double LENGTH_OF_PEG
= 10.5;
21 private static final double PEG_EXTENDED
= 136.5664;
22 private static final int WIDTH_OF_ARENA
= 324;
23 private static final double MOTOR_VALUE_TURN
= 0.5;// this is probably not
25 private static final double MOTOR_VALUE_FORWARD
= 0.5;// random
26 double DISTANCE_FROM_BOILER_WALL
= 105.7126; // This is the starting point
27 // from the right side of the
30 public AutonGearThenBaselinePegCloseToBoiler() {
31 requires(Robot
.getDriveTrain());
32 if (DISTANCE_FROM_BOILER_WALL
> 0 && DISTANCE_FROM_BOILER_WALL
< 109.1767) // right
37 new DriveDistance(DISTANCE_FROM_BOILER_WALL
/ Math
.sqrt(3) + 50.2168,
38 MOTOR_VALUE_FORWARD
)); // drive straight into extended line of peg
39 addSequential(new TurnForAngle(60, Direction
.LEFT
, MOTOR_VALUE_TURN
));// position
45 addSequential(new DriveDistance(PEG_EXTENDED
46 - 2 * DISTANCE_FROM_BOILER_WALL
/ Math
.sqrt(3) - LENGTH_OF_PEG
,
47 MOTOR_VALUE_FORWARD
));// put gear on peg
48 // does not include drift distance
49 } else if (DISTANCE_FROM_BOILER_WALL
> 205.7286
50 && DISTANCE_FROM_BOILER_WALL
< 324) // mirror of right peg path {
52 DISTANCE_FROM_BOILER_WALL
= WIDTH_OF_ARENA
- DISTANCE_FROM_BOILER_WALL
; // Turning
53 // DISTANCE_FROM_BOILER_WALL
64 new DriveDistance(DISTANCE_FROM_BOILER_WALL
/ Math
.sqrt(3) + 50.2168,
65 MOTOR_VALUE_FORWARD
));
66 addSequential(new TurnForAngle(60, Direction
.RIGHT
, MOTOR_VALUE_TURN
));
67 addSequential(new DriveDistance(PEG_EXTENDED
68 - 2 * DISTANCE_FROM_BOILER_WALL
/ Math
.sqrt(3) - LENGTH_OF_PEG
,
69 MOTOR_VALUE_FORWARD
));
70 // does not include drift distance
73 "DO NOT START HERE!!! MOVE CLOSER TO WALL OR LOSE ALL AUTON POINTS!!");