From fb75626b5f83301f4c51c6c9a29d5f948e10effd Mon Sep 17 00:00:00 2001 From: Kevin Zhang Date: Wed, 17 Feb 2016 15:08:40 -0800 Subject: [PATCH] Put drivetrain constants in Constants.java --- .../usfirst/frc/team3501/robot/Constants.java | 8 +- .../robot/commands/auton/AlignToScore.java | 149 ++++++++++++++++++ .../team3501/robot/subsystems/DriveTrain.java | 44 ++---- 3 files changed, 167 insertions(+), 34 deletions(-) create mode 100755 src/org/usfirst/frc/team3501/robot/commands/auton/AlignToScore.java diff --git a/src/org/usfirst/frc/team3501/robot/Constants.java b/src/org/usfirst/frc/team3501/robot/Constants.java index 4ad8a4bf..37f77e41 100644 --- a/src/org/usfirst/frc/team3501/robot/Constants.java +++ b/src/org/usfirst/frc/team3501/robot/Constants.java @@ -56,13 +56,13 @@ public class Constants { public final static int FORWARD_CHANNEL = 0; public final static int REVERSE_CHANNEL = 0; - private final static double WHEEL_DIAMETER = 6.0; // in inches - private final static double PULSES_PER_ROTATION = 256; // in pulses - private final static double OUTPUT_SPROCKET_DIAMETER = 2.0; // in inches - private final static double WHEEL_SPROCKET_DIAMETER = 3.5; // in inches public static final double INCHES_PER_PULSE = ((3.66 / 5.14) * 6 * Math.PI) / 256; + public static double kp = 0.013, ki = 0.000015, kd = -0.002; + public static double gp = 0.018, gi = 0.000015, gd = 0; + public static double encoderTolerance = 8.0, gyroTolerance = 5.0; + public static final int MANUAL_MODE = 1, ENCODER_MODE = 2, GYRO_MODE = 3; public static final int LEFT_FORWARD = 0, LEFT_REVERSE = 1, RIGHT_FORWARD = 2, RIGHT_REVERSE = 3; diff --git a/src/org/usfirst/frc/team3501/robot/commands/auton/AlignToScore.java b/src/org/usfirst/frc/team3501/robot/commands/auton/AlignToScore.java new file mode 100755 index 00000000..0f7a79b2 --- /dev/null +++ b/src/org/usfirst/frc/team3501/robot/commands/auton/AlignToScore.java @@ -0,0 +1,149 @@ +package org.usfirst.frc.team3501.robot.commands.auton; + +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; + +/** + * This command group will be used in autonomous. Based on what position the + * robot is in, the robot will align with the goal. In the Software 2015-2016 + * Google folder is a picture explaining each of the cases. + * + * dependency on sensors: lidars, encoders, gyro + * + * dependency on subsystems: drivetrain + * + * dependency on other commands: TurnForAngle(), DriveForDistance() + * + * pre-condition: robot is flush against a defense at the specified position in + * the opponent's courtyard + * + * post-condition: the robot is parallel to one of the three goals and the + * shooter is facing that goal + * + */ +public class AlignToScore extends CommandGroup { + private final static double CENTER_OF_MASS_TO_ROBOT_FRONT = 0; + private final static double DIST_CASTLE_WALL_TO_SIDE_GOAL = 0; + private final static double DIST_CASTLE_WALL_TO_FRONT_GOAL = 0; + + private final double DEFAULT_SPEED = 0.5; + private final double maxTimeout = 5; + + // in inches + // assuming that positive angle means turning right + // and negative angle means turning left + + // constants for position 1: low bar + private final double POS1_DIST1 = 109; + private final double POS1_TURN1 = 60; + private final double POS1_DIST2 = 0; + + // constants for position 2 + private final double POS2_DIST1 = 140; + private final double POS2_TURN1 = 60; + private final double POS2_DIST2 = 0; + + // constants for position 3 + private final double POS3_DIST1 = 0; + private final double POS3_TURN1 = 90; + private final double POS3_DIST2 = 35.5; + private final double POS3_TURN2 = -90; + private final double POS3_DIST3 = 0; + + // constants for position 4 + private final double POS4_DIST1 = 0; + private final double POS4_TURN1 = -90; + private final double POS4_DIST2 = 18.5; + private final double POS4_TURN2 = 90; + private final double POS4_DIST3 = 0; + + // constants for position 5 + private final double POS5_DIST1 = 0; + private final double POS5_TURN1 = -90; + private final double POS5_DIST2 = 72.5; + private final double POS5_TURN2 = 90; + private final double POS5_DIST3 = 0; + + public double horizontalDistToGoal; + + public AlignToScore(int position) { + + if (position == 1) { + + // position 1 is always the low bar + + addSequential(new DriveDistance(POS1_DIST1, DEFAULT_SPEED)); + addSequential(new TurnForAngle(POS1_TURN1, maxTimeout)); + addSequential(new DriveDistance(POS1_DIST2, DEFAULT_SPEED)); + horizontalDistToGoal = 0; + } else if (position == 2) { + + addSequential(new DriveDistance(POS2_DIST1, DEFAULT_SPEED)); + addSequential(new TurnForAngle(POS2_TURN1, maxTimeout)); + addSequential(new DriveDistance(POS2_DIST2, DEFAULT_SPEED)); + horizontalDistToGoal = 0; + + } else if (position == 3) { + + addSequential(new DriveDistance(POS3_DIST1, DEFAULT_SPEED)); + addSequential(new TurnForAngle(POS3_TURN1, maxTimeout)); + addSequential(new DriveDistance(POS3_DIST2, DEFAULT_SPEED)); + addSequential(new TurnForAngle(POS3_TURN2, maxTimeout)); + addSequential(new DriveDistance(POS3_DIST3, DEFAULT_SPEED)); + horizontalDistToGoal = 0; + + } else if (position == 4) { + + addSequential(new DriveDistance(POS4_DIST1, DEFAULT_SPEED)); + addSequential(new TurnForAngle(POS4_TURN1, maxTimeout)); + addSequential(new DriveDistance(POS4_DIST2, DEFAULT_SPEED)); + addSequential(new TurnForAngle(POS4_TURN2, maxTimeout)); + addSequential(new DriveDistance(POS4_DIST3, DEFAULT_SPEED)); + horizontalDistToGoal = 0; + + } else if (position == 5) { + + addSequential(new DriveDistance(POS5_DIST1, DEFAULT_SPEED)); + addSequential(new TurnForAngle(POS5_TURN1, maxTimeout)); + addSequential(new DriveDistance(POS5_DIST2, DEFAULT_SPEED)); + addSequential(new TurnForAngle(POS5_TURN2, maxTimeout)); + addSequential(new DriveDistance(POS5_DIST3, DEFAULT_SPEED)); + horizontalDistToGoal = 0; + + } + } + + // following commented out method is calculations for path of robot in auton + // after passing through defense using two lidars + /* + * public static double lidarCalculateAngleToTurn(int position, + * double horizontalDistToGoal) { + * double leftDist = Robot.driveTrain.getLeftLidarDistance(); + * double rightDist = Robot.driveTrain.getRightLidarDistance(); + * + * double errorAngle = Math.atan(Math.abs(leftDist - rightDist) / 2); + * double distToTower; + * // TODO: figure out if we do want to shoot into the side goal if we are + * // in position 1 or 2, or if we want to change that + * if (position == 1 || position == 2) { + * distToTower = Math + * .cos(CENTER_OF_MASS_TO_ROBOT_FRONT + (leftDist - rightDist) / 2) + * - DIST_CASTLE_WALL_TO_SIDE_GOAL; + * } + * + * // TODO: figure out if we do want to shoot into the font goal if we are + * // in position 3, 4, 5, or if we want to change that + * else { + * distToTower = Math + * .cos(CENTER_OF_MASS_TO_ROBOT_FRONT + (leftDist - rightDist) / 2) + * - DIST_CASTLE_WALL_TO_SIDE_GOAL; + * } + * + * double angleToTurn = Math.atan(distToTower / horizontalDistToGoal); + * + * return angleToTurn; + * } + */ +} diff --git a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java index 2b11491a..a4bba48b 100644 --- a/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java +++ b/src/org/usfirst/frc/team3501/robot/subsystems/DriveTrain.java @@ -16,18 +16,9 @@ import edu.wpi.first.wpilibj.RobotDrive; import edu.wpi.first.wpilibj.command.PIDSubsystem; public class DriveTrain extends PIDSubsystem { - private static double kp = 0.013, ki = 0.000015, kd = -0.002; - private static double gp = 0.018, gi = 0.000015, gd = 0; - private static double pidOutput = 0; - - // PID Controller tolerances for the error - private static double encoderTolerance = 8.0, gyroTolerance = 5.0; - // Current Drive Mode Default Drive Mode is Manual private int DRIVE_MODE = 1; - - // Different Drive Modes - private static final int MANUAL_MODE = 1, ENCODER_MODE = 2, GYRO_MODE = 3; + private static double pidOutput = 0; private Encoder leftEncoder, rightEncoder; @@ -39,23 +30,13 @@ public class DriveTrain extends PIDSubsystem { private GyroLib gyro; private DoubleSolenoid leftGearPiston, rightGearPiston; + // Drivetrain specific constants that relate to the inches per pulse value for // the encoders - private final static double WHEEL_DIAMETER = 6.0; // in inches - private final static double PULSES_PER_ROTATION = 256; // in pulses - private final static double OUTPUT_SPROCKET_DIAMETER = 2.0; // in inches - private final static double WHEEL_SPROCKET_DIAMETER = 3.5; // in inches - public final static double INCHES_PER_PULSE = (((Math.PI) - * OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION) - / WHEEL_SPROCKET_DIAMETER) * WHEEL_DIAMETER; - - // Drivetrain specific constants that relate to the PID controllers - private final static double Kp = 1.0, Ki = 0.0, - Kd = 0.0 * (OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION) - / (WHEEL_SPROCKET_DIAMETER) * WHEEL_DIAMETER; public DriveTrain() { - super(kp, ki, kd); + super(Constants.DriveTrain.kp, Constants.DriveTrain.ki, + Constants.DriveTrain.kd); frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT); frontRight = new CANTalon(Constants.DriveTrain.FRONT_RIGHT); @@ -66,7 +47,7 @@ public class DriveTrain extends PIDSubsystem { leftLidar = new Lidar(I2C.Port.kOnboard); rightLidar = new Lidar(I2C.Port.kOnboard); // TODO: find port for second - // lidar + // lidar leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A, Constants.DriveTrain.ENCODER_LEFT_B, false, EncodingType.k4X); rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A, @@ -192,9 +173,11 @@ public class DriveTrain extends PIDSubsystem { // Updates the PID constants based on which control mode is being used public void updatePID() { if (DRIVE_MODE == Constants.DriveTrain.ENCODER_MODE) - this.getPIDController().setPID(kp, ki, kd); + this.getPIDController().setPID(Constants.DriveTrain.kp, + Constants.DriveTrain.ki, Constants.DriveTrain.kd); else - this.getPIDController().setPID(gp, gd, gi); + this.getPIDController().setPID(Constants.DriveTrain.gp, + Constants.DriveTrain.gd, Constants.DriveTrain.gi); } public CANTalon getFrontLeft() { @@ -235,7 +218,7 @@ public class DriveTrain extends PIDSubsystem { if (Math.abs(output) > 0 && Math.abs(output) < 0.3) output = Math.signum(output) * 0.3; left = output; - right = output + drift * kp / 10; + right = output + drift * Constants.DriveTrain.kp / 10; } else if (DRIVE_MODE == Constants.DriveTrain.GYRO_MODE) { left = output; right = -output; @@ -305,7 +288,7 @@ public class DriveTrain extends PIDSubsystem { public void setEncoderPID() { DRIVE_MODE = Constants.DriveTrain.ENCODER_MODE; this.updatePID(); - this.setAbsoluteTolerance(encoderTolerance); + this.setAbsoluteTolerance(Constants.DriveTrain.encoderTolerance); this.setOutputRange(-1.0, 1.0); this.setInputRange(-200.0, 200.0); this.enable(); @@ -319,9 +302,10 @@ public class DriveTrain extends PIDSubsystem { private void setGyroPID() { DRIVE_MODE = Constants.DriveTrain.GYRO_MODE; this.updatePID(); - this.getPIDController().setPID(gp, gi, gd); + this.getPIDController().setPID(Constants.DriveTrain.gp, + Constants.DriveTrain.gi, Constants.DriveTrain.gd); - this.setAbsoluteTolerance(gyroTolerance); + this.setAbsoluteTolerance(Constants.DriveTrain.gyroTolerance); this.setOutputRange(-1.0, 1.0); this.setInputRange(-360.0, 360.0); this.enable(); -- 2.30.2