challenge-bot.com Git - 3501/stronghold-2016/blob - src/org/usfirst/frc/team3501/robot/commands/auton/AlignToScore.java

   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 }