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