| 1 | package org.usfirst.frc.team3501.robot.commands.auton; |
| 2 | |
| 3 | import org.usfirst.frc.team3501.robot.commands.driving.DriveDistance; |
| 4 | import org.usfirst.frc.team3501.robot.commands.driving.TurnForAngle; |
| 5 | |
| 6 | import edu.wpi.first.wpilibj.command.CommandGroup; |
| 7 | |
| 8 | /** |
| 9 | * This command group will be used in autonomous. Based on what position the |
| 10 | * robot is in, the robot will align with the goal. In the Software 2015-2016 |
| 11 | * Google folder is a picture explaining each of the cases. |
| 12 | * |
| 13 | * dependency on sensors: lidars, encoders, gyro |
| 14 | * |
| 15 | * dependency on subsystems: drivetrain |
| 16 | * |
| 17 | * dependency on other commands: TurnForAngle(), DriveForDistance() |
| 18 | * |
| 19 | * pre-condition: robot is flush against a defense at the specified position in |
| 20 | * the opponent's courtyard |
| 21 | * |
| 22 | * post-condition: the robot is parallel to one of the three goals and the |
| 23 | * shooter is facing that goal |
| 24 | * |
| 25 | */ |
| 26 | public class AlignToScore extends CommandGroup { |
| 27 | private final static double CENTER_OF_MASS_TO_ROBOT_FRONT = 0; |
| 28 | private final static double DIST_CASTLE_WALL_TO_SIDE_GOAL = 0; |
| 29 | private final static double DIST_CASTLE_WALL_TO_FRONT_GOAL = 0; |
| 30 | |
| 31 | private final double DEFAULT_SPEED = 0.5; |
| 32 | private final double maxTimeout = 5; |
| 33 | |
| 34 | // in inches |
| 35 | // assuming that positive angle means turning right |
| 36 | // and negative angle means turning left |
| 37 | |
| 38 | // constants for position 1: low bar |
| 39 | private final double POS1_DIST1 = 109; |
| 40 | private final double POS1_TURN1 = 60; |
| 41 | private final double POS1_DIST2 = 0; |
| 42 | |
| 43 | // constants for position 2 |
| 44 | private final double POS2_DIST1 = 140; |
| 45 | private final double POS2_TURN1 = 60; |
| 46 | private final double POS2_DIST2 = 0; |
| 47 | |
| 48 | // constants for position 3 |
| 49 | private final double POS3_DIST1 = 0; |
| 50 | private final double POS3_TURN1 = 90; |
| 51 | private final double POS3_DIST2 = 35.5; |
| 52 | private final double POS3_TURN2 = -90; |
| 53 | private final double POS3_DIST3 = 0; |
| 54 | |
| 55 | // constants for position 4 |
| 56 | private final double POS4_DIST1 = 0; |
| 57 | private final double POS4_TURN1 = -90; |
| 58 | private final double POS4_DIST2 = 18.5; |
| 59 | private final double POS4_TURN2 = 90; |
| 60 | private final double POS4_DIST3 = 0; |
| 61 | |
| 62 | // constants for position 5 |
| 63 | private final double POS5_DIST1 = 0; |
| 64 | private final double POS5_TURN1 = -90; |
| 65 | private final double POS5_DIST2 = 72.5; |
| 66 | private final double POS5_TURN2 = 90; |
| 67 | private final double POS5_DIST3 = 0; |
| 68 | |
| 69 | public double horizontalDistToGoal; |
| 70 | |
| 71 | public AlignToScore(int position) { |
| 72 | |
| 73 | if (position == 1) { |
| 74 | |
| 75 | // position 1 is always the low bar |
| 76 | |
| 77 | addSequential(new DriveDistance(POS1_DIST1, DEFAULT_SPEED)); |
| 78 | addSequential(new TurnForAngle(POS1_TURN1, maxTimeout)); |
| 79 | addSequential(new DriveDistance(POS1_DIST2, DEFAULT_SPEED)); |
| 80 | horizontalDistToGoal = 0; |
| 81 | } else if (position == 2) { |
| 82 | |
| 83 | addSequential(new DriveDistance(POS2_DIST1, DEFAULT_SPEED)); |
| 84 | addSequential(new TurnForAngle(POS2_TURN1, maxTimeout)); |
| 85 | addSequential(new DriveDistance(POS2_DIST2, DEFAULT_SPEED)); |
| 86 | horizontalDistToGoal = 0; |
| 87 | |
| 88 | } else if (position == 3) { |
| 89 | |
| 90 | addSequential(new DriveDistance(POS3_DIST1, DEFAULT_SPEED)); |
| 91 | addSequential(new TurnForAngle(POS3_TURN1, maxTimeout)); |
| 92 | addSequential(new DriveDistance(POS3_DIST2, DEFAULT_SPEED)); |
| 93 | addSequential(new TurnForAngle(POS3_TURN2, maxTimeout)); |
| 94 | addSequential(new DriveDistance(POS3_DIST3, DEFAULT_SPEED)); |
| 95 | horizontalDistToGoal = 0; |
| 96 | |
| 97 | } else if (position == 4) { |
| 98 | |
| 99 | addSequential(new DriveDistance(POS4_DIST1, DEFAULT_SPEED)); |
| 100 | addSequential(new TurnForAngle(POS4_TURN1, maxTimeout)); |
| 101 | addSequential(new DriveDistance(POS4_DIST2, DEFAULT_SPEED)); |
| 102 | addSequential(new TurnForAngle(POS4_TURN2, maxTimeout)); |
| 103 | addSequential(new DriveDistance(POS4_DIST3, DEFAULT_SPEED)); |
| 104 | horizontalDistToGoal = 0; |
| 105 | |
| 106 | } else if (position == 5) { |
| 107 | |
| 108 | addSequential(new DriveDistance(POS5_DIST1, DEFAULT_SPEED)); |
| 109 | addSequential(new TurnForAngle(POS5_TURN1, maxTimeout)); |
| 110 | addSequential(new DriveDistance(POS5_DIST2, DEFAULT_SPEED)); |
| 111 | addSequential(new TurnForAngle(POS5_TURN2, maxTimeout)); |
| 112 | addSequential(new DriveDistance(POS5_DIST3, DEFAULT_SPEED)); |
| 113 | horizontalDistToGoal = 0; |
| 114 | |
| 115 | } |
| 116 | } |
| 117 | |
| 118 | // following commented out method is calculations for path of robot in auton |
| 119 | // after passing through defense using two lidars |
| 120 | /* |
| 121 | * public static double lidarCalculateAngleToTurn(int position, |
| 122 | * double horizontalDistToGoal) { |
| 123 | * double leftDist = Robot.driveTrain.getLeftLidarDistance(); |
| 124 | * double rightDist = Robot.driveTrain.getRightLidarDistance(); |
| 125 | * |
| 126 | * double errorAngle = Math.atan(Math.abs(leftDist - rightDist) / 2); |
| 127 | * double distToTower; |
| 128 | * // TODO: figure out if we do want to shoot into the side goal if we are |
| 129 | * // in position 1 or 2, or if we want to change that |
| 130 | * if (position == 1 || position == 2) { |
| 131 | * distToTower = Math |
| 132 | * .cos(CENTER_OF_MASS_TO_ROBOT_FRONT + (leftDist - rightDist) / 2) |
| 133 | * - DIST_CASTLE_WALL_TO_SIDE_GOAL; |
| 134 | * } |
| 135 | * |
| 136 | * // TODO: figure out if we do want to shoot into the font goal if we are |
| 137 | * // in position 3, 4, 5, or if we want to change that |
| 138 | * else { |
| 139 | * distToTower = Math |
| 140 | * .cos(CENTER_OF_MASS_TO_ROBOT_FRONT + (leftDist - rightDist) / 2) |
| 141 | * - DIST_CASTLE_WALL_TO_SIDE_GOAL; |
| 142 | * } |
| 143 | * |
| 144 | * double angleToTurn = Math.atan(distToTower / horizontalDistToGoal); |
| 145 | * |
| 146 | * return angleToTurn; |
| 147 | * } |
| 148 | */ |
| 149 | } |