diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index af98ba7..9bcef90 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,6 +18,9 @@ import frc.robot.sensors.AnalogSelector; import frc.robot.subsystems.*; import frc.robot.commands.ArcadeDriveWithJoystick; +import frc.robot.commands.DriveWithDistance; +import frc.robot.commands.DriveWithTime; + import java.util.logging.Logger; @@ -34,8 +37,8 @@ public class RobotContainer { * The container for the robot. Contains subsystems, OI devices, and commands. */ - // RobotContainer is a singleton class - private static RobotContainer currentInstance; + // RobotContainer is a singleton class + private static RobotContainer currentInstance; // The robot's subsystems and commands are defined here... private Joystick driverStick; @@ -46,17 +49,15 @@ public class RobotContainer { private Command intakeCommand; private Command reverseFeeder; private Command moveToOuterPort; - private Command reverseArmManually; + private Command reverseArmManually; + private Command positionPowercell; private Command rampShooterCommand; private Command incrementFeeder; private Command moveArm; private Command sensitiveDriving; private Logger logger = Logger.getLogger("RobotContainer"); - private final double AUTO_DRIVE_TIME = 1.0; - private final double AUTO_LEFT_MOTOR_SPEED = 0.2; - private final double AUTO_RIGHT_MOTOR_SPEED = 0.2; - private Command runFeeder; + private Command runFeeder; /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -125,11 +126,13 @@ private void configureButtonBindings() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - int selectorOne = 1; + //Robot has only one selector + int selectorOne = 0; if (analogSelectorOne != null){ selectorOne = analogSelectorOne.getIndex(); } + logger.info("Selectors: " + selectorOne); if (selectorOne == 0) { @@ -137,17 +140,24 @@ public Command getAutonomousCommand() { return null; } else if (selectorOne == 1) { return new SpinUpShooterWithTime(Config.RPM.get(), 7).alongWith(new RunFeederCommandWithTime(-0.7, 7)).andThen(new DriveWithTime(0.5, 0.5, 0.5)); - // return new DriveWithTime(AUTO_DRIVE_TIME, AUTO_LEFT_MOTOR_SPEED, AUTO_RIGHT_MOTOR_SPEED); } else if(selectorOne == 2) { return new DriveWithTime(0.5, 0.5, 0.5); + } else if (selectorOne == 3) { + /* + * When the selector is set to one, the robot will run for x seconds at y left motor speed and z right motor speed + */ + return new DriveWithTime(Config.AUTO_DRIVE_TIME, Config.AUTO_LEFT_MOTOR_SPEED, Config.AUTO_RIGHT_MOTOR_SPEED); + } else if(selectorOne == 4){ + + //List distance, then the drive unit (in option of meters, cm, inches or feet), and then the right and left speed (if not specified, it is 0.5) + //Robot drives one meter forward + return new DriveWithDistance(Config.AUTO_DISTANCE, Config.DEFAULT_UNIT, Config.AUTO_RIGHT_MOTOR_SPEED, Config.AUTO_LEFT_MOTOR_SPEED); } - - - - // Also return null if this ever gets to here because safety return null; } + + public void joystickRumble(double leftValue, double rightValue) { //Joystick rumble (driver feedback). leftValue/rightValue sets vibration force. diff --git a/src/main/java/frc/robot/commands/DriveWithDistance.java b/src/main/java/frc/robot/commands/DriveWithDistance.java new file mode 100644 index 0000000..604b444 --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveWithDistance.java @@ -0,0 +1,196 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc.robot.commands; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.DriveBase; +import frc.robot.subsystems.DriveBaseHolder; +import frc.robot.subsystems.DriveBase.DistanceType; + +public class DriveWithDistance extends CommandBase { + + //Initializing private version of drivebase object + private final DriveBase driveBase; + + //The speed that the robot must move below to move backwards + private final static double ZERO_SPEED = 0; + + private final static double DEFAULT_RIGHT_SPEED = 0.5; + private final static double DEFAULT_LEFT_SPEED = 0.5; + + // initialization of the variables + public double desiredLeftDistance = 0; + public double desiredRightDistance = 0; + public double desiredRightEncoderTicks, desiredLeftEncoderTicks; + public double leftSpeed = 0; + public double rightSpeed = 0; + public DistanceType distanceType; + public double currentConversion; + public boolean commandFinished = false; + + /** + * Constructor to be used when the input specifications do not provide the right and left speed (assumed to be 0.5) + * + * @param distance The distance to drive + * @param distanceType The distance unit type to drive in + */ + + public DriveWithDistance(double distance, DistanceType distanceType) { + this(distance, distanceType, DEFAULT_RIGHT_SPEED, DEFAULT_LEFT_SPEED); + } + + /** + * Constructor to be used when the input gives the right and left speed + * + * @param distance The distance to drive + * @param distanceType The distance unit type to drive in + * @param rightSpeed The right motor speed + * @param leftSpeed The left motor speed + */ + public DriveWithDistance(double distance, DistanceType distanceType, double rightSpeed, double leftSpeed) { + this(distance, distance, distanceType, rightSpeed, leftSpeed); + } + + + /** + * Constructor to be used when the input gives the right and left distance + * + * @param rightDistance The distance to drive for right motor + * @param leftDistance The distance to drive for left motor + * @param distanceType The distance unit type to drive in + * @param rightSpeed The right motor speed + * @param leftSpeed The left motor speed + */ + public DriveWithDistance(double rightDistance, double leftDistance, DistanceType distanceType, double rightSpeed, double leftSpeed) { + //Initating drivebase + this.driveBase = DriveBaseHolder.getInstance(); + + //Setting current distance unit + this.distanceType = distanceType; + + addRequirements(driveBase); + + assignDistanceType(distanceType); + + //Setting the right speed and left speed according to input specifications + this.rightSpeed = rightSpeed; + this.leftSpeed = leftSpeed; + + //Determing the amount of desired encoder ticks to drive by getting the distnace and multiplying by the chosen distance unit + this.desiredRightEncoderTicks = rightDistance * this.currentConversion; + this.desiredLeftEncoderTicks = leftDistance * this.currentConversion; + + } + + /** + * Assigns the current conversion from encoder ticks to distance type + * + * @param distanceType The distance unit that the robot is using for driving + */ + private void assignDistanceType(DistanceType distanceType) { + //Setting the current conversion type according to the chosen distance unit + switch (distanceType) { + case FEET: + this.currentConversion = DriveBase.DistanceUnit.FEET.encoderTicks; + + break; + case METERS: + this.currentConversion = DriveBase.DistanceUnit.METERS.encoderTicks; + + break; + case CENTIMETERS: + this.currentConversion = DriveBase.DistanceUnit.CENTIMETERS.encoderTicks; + + break; + case INCHES: + this.currentConversion = DriveBase.DistanceUnit.INCHES.encoderTicks; + + break; + + //Default is meters + default: + + this.currentConversion = DriveBase.DistanceUnit.METERS.encoderTicks; + break; + + } + } + + /** + * Called one when the command is initially scheduled and determines the desired distance to travel + * + */ + @Override + public void initialize() { + + //Add the desired amount of encoder ticks to the current distance to get the amount of encoder ticks that the robot needs to drive + desiredRightDistance = driveBase.getRightDistance() + desiredRightEncoderTicks; + desiredLeftDistance = driveBase.getLeftDistance() + desiredLeftEncoderTicks; + + addRequirements(DriveBaseHolder.getInstance()); + } + + /** + * Called every time the scheduler runs while the command is scheduled. (run every 20ms) and determines the current distance + * + */ + @Override + public void execute() { + + //Get the current distance of the right encoder and store value in variable + double currentRightDistance = DriveBaseHolder.getInstance().getRightDistance(); + double currentLeftDistance = DriveBaseHolder.getInstance().getLeftDistance(); + + //Run motors according to output of the speeds + driveBase.tankDrive(leftSpeed, rightSpeed, false); + + //Dashboard + SmartDashboard.putNumber("Left Speed", leftSpeed); + SmartDashboard.putNumber("Right Speed", rightSpeed); + SmartDashboard.putNumber("Left Error", desiredLeftDistance - currentLeftDistance); + SmartDashboard.putNumber("Right Error", desiredRightDistance - currentRightDistance); + + //Code for figuring out when the robot is finished driving and handling user input validation + + // Update to the current right/left distance that the robot has driven + currentRightDistance = DriveBaseHolder.getInstance().getRightDistance(); + currentLeftDistance = DriveBaseHolder.getInstance().getLeftDistance(); + + // If the robot has reached or surpassed the desired distance, then stop the + // robot. Otherwise, keep moving (moving forward OR moving backwards). + if (Math.abs(currentRightDistance) >= desiredRightDistance) { + rightSpeed = ZERO_SPEED; + } + + // If the robot has reached or surpassed the desired distance, then stop the + // robot. Otherwise, keep moving (moving forward). + if (Math.abs(currentLeftDistance) >= desiredLeftDistance) { + leftSpeed = ZERO_SPEED; + } + + //If the left and right speed are zero, just stop the robot + if ((leftSpeed == ZERO_SPEED && rightSpeed == ZERO_SPEED) || + (desiredLeftDistance >= currentLeftDistance && desiredRightDistance >= currentRightDistance)) { + commandFinished = true; + } + + //Taking the changed left and right speed and adding their updated values to the drivebase + driveBase.tankDrive(leftSpeed, rightSpeed, false); + } + + /** + * Determines when the command is finished using the current distnce and desired distance + * + */ + + @Override + public boolean isFinished() { + return commandFinished; + } +} diff --git a/src/main/java/frc/robot/config/Config.java b/src/main/java/frc/robot/config/Config.java index f387650..74a7737 100644 --- a/src/main/java/frc/robot/config/Config.java +++ b/src/main/java/frc/robot/config/Config.java @@ -136,6 +136,11 @@ private Config() { public static double ARM_PID_D = robotSpecific(0.0); public static double ARM_PID_F = robotSpecific(0.05); + public static final double AUTO_LEFT_MOTOR_SPEED = 0.2; + public static final double AUTO_RIGHT_MOTOR_SPEED = 0.2; + public static final double AUTO_DISTANCE = 1; + public static final double AUTO_DRIVE_TIME = 1.0; + public static final DriveBase.DistanceType DEFAULT_UNIT= DriveBase.DistanceType.METERS; // Define a global constants table for subsystems to use public static NetworkTable constantsTable = NetworkTableInstance.getDefault().getTable("constants"); diff --git a/src/main/java/frc/robot/subsystems/DriveBase.java b/src/main/java/frc/robot/subsystems/DriveBase.java index 78eca91..f5fcea2 100644 --- a/src/main/java/frc/robot/subsystems/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/DriveBase.java @@ -153,6 +153,47 @@ public final NeutralMode getNeutralMode() { */ protected void driveModeUpdated(DriveMode driveMode) { + } + + /** + * Get the distance travelled by the left encoder in encoder ticks + * + * @return The distance of the right encoder (0 before overide) + */ + public double getLeftDistance(){ + return 0; + } + + /** + * Get the distance travelled by the right encoder in encoder ticks + * + * @return The distance of the right encoder (0 before overide) + */ + public double getRightDistance() { + return 0; + } + + /** + * Get the speed of the left encoder in encoder ticks per second + * + * @return The speed of the left encoder (0 before overide) + */ + public double getLeftSpeed() { + return 0; + } + + /** + * Get the speed of the right encoder in encoder ticks per second + * + * @return The speed of the right encoder (0 before overide) + */ + public double getRightSpeed() { + return 0; + } + + //Reset encoder values + public void resetEncoders() { + } /** @@ -163,4 +204,43 @@ protected void driveModeUpdated(DriveMode driveMode) { protected void neutralModeUpdated(NeutralMode neutralMode) { } + + /** + * Measured distance units for converting ticks to units or units to encoder ticks + * + */ + public enum DistanceUnit { + + //Encoder tick is equal to itself + ENCODER_TICKS(1d), + + //Measured value of 8583 ticks per meter + METERS(8583.1d), + + //1 cm = 0.01 meters. 8583 * 0.01 = 858.3. Rounding to 858 ticks. + CENTIMETERS(858.1d), + + //1 foot is equal to 0.3048 meters. 8583 * 0.3048 = 2616.1 (approx). Rounding to 2616 ticks per foot + FEET(2616.1d), + + //39.3701 inches is equal to 1 meter. 8583 / 39.3701 = 218.008 (approx). Rounding to 218 ticks per inch + INCHES(218.1d); + + //Update encoder ticks with unit + public double encoderTicks; + DistanceUnit(double encoderTicks) { + this.encoderTicks = encoderTicks; + } + } + + /** + * Distance unts to travel + * + */ + public enum DistanceType { + + //Different distance units + CENTIMETERS, METERS, INCHES, FEET; + + } } diff --git a/src/main/java/frc/robot/subsystems/DriveBasePre2020.java b/src/main/java/frc/robot/subsystems/DriveBasePre2020.java index 15d9ca0..9025bc8 100644 --- a/src/main/java/frc/robot/subsystems/DriveBasePre2020.java +++ b/src/main/java/frc/robot/subsystems/DriveBasePre2020.java @@ -81,6 +81,48 @@ private void selectEncoderStandard() { } + /** + * Get the distance travelled by the left encoder in encoder ticks + * + * @return The distance of the left encoder + */ + public double getLeftDistance() { + return (-leftFrontTalon.getSensorCollection().getQuadraturePosition()); + } + + /** + * Get the distance travelled by the right encoder in encoder ticks + * + * @return The distance of the right encoder + */ + public double getRightDistance() { + return rightFrontTalon.getSensorCollection().getQuadraturePosition(); + } + + /** + * Get the speed of the left encoder in encoder ticks per second + * + * @return The speed of the left encoder + */ + public double getLeftSpeed() { + return (-leftFrontTalon.getSensorCollection().getQuadratureVelocity()); + } + + /** + * Get the speed of the right encoder in encoder ticks per second + * + * @return The speed of the right encoder + */ + public double getRightSpeed() { + return rightFrontTalon.getSensorCollection().getQuadratureVelocity(); + } + + //Reset encoder values + public void resetEncoders() { + leftFrontTalon.getSensorCollection().setQuadraturePosition(0, Config.CAN_TIMEOUT_SHORT); + rightFrontTalon.getSensorCollection().setQuadraturePosition(0, Config.CAN_TIMEOUT_SHORT); + } + /** * Reset the talons to factory default */