From 2840727fbf1efa953e10e30b1e2407043bada664 Mon Sep 17 00:00:00 2001 From: Ishan Date: Sat, 29 Feb 2020 13:08:28 -0500 Subject: [PATCH 01/10] Drive with Time command complete Tested and completed driving with distance at any speed. --- src/main/java/frc/robot/RobotContainer.java | 28 ++- .../frc/robot/commands/DriveWithDistance.java | 203 ++++++++++++++++++ src/main/java/frc/robot/config/Config.java | 7 +- .../java/frc/robot/subsystems/DriveBase.java | 82 +++++++ 4 files changed, 310 insertions(+), 10 deletions(-) create mode 100644 src/main/java/frc/robot/commands/DriveWithDistance.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 73791da..b3ec13b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,6 +19,7 @@ import frc.robot.sensors.AnalogSelector; import frc.robot.subsystems.DriveBase; import frc.robot.commands.ArcadeDriveWithJoystick; +import frc.robot.commands.DriveWithDistance; import frc.robot.commands.DriveWithTime; import frc.robot.commands.SensitiveDriverControl; import edu.wpi.first.wpilibj2.command.Command; @@ -50,6 +51,8 @@ public class RobotContainer { private Command sensitiveDriverControlCommand; private Logger logger = Logger.getLogger("RobotContainer"); + private final double RIGHT_SPEED = 0.2; + private final double LEFT_SPEED = 0.2; /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -96,7 +99,7 @@ private void configureButtonBindings() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - int selectorOne = 1, selectorTwo = 1; + int selectorOne = 0, selectorTwo = 0; if (analogSelectorOne != null) selectorOne = analogSelectorOne.getIndex(); if (analogSelectorTwo != null) @@ -109,14 +112,25 @@ public Command getAutonomousCommand() { } - if (selectorOne == 1 || selectorTwo == 1) { - /* - * When the selector is set to one, the robot will run for x seconds at y left motor speed and z right motor speed - */ + else if (selectorOne == 1) { + /* + * When the selector is set to one, the robot will run for x seconds at y left motor speed and z right motor speed + */ + + // for the inputed variables: DriveWithTime(seconds (double), left motors speed (double), right motors speed (double)) + return new DriveWithTime(1.0, 0.2, 0.2); + } + + else if(selectorOne == 2){ + + + + //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) + return new DriveWithDistance(53, DriveBase.DistanceType.INCHES, RIGHT_SPEED, LEFT_SPEED); - // for the inputed variables: DriveWithTime(seconds (double), left motors speed (double), right motors speed (double)) - return new DriveWithTime(1.0, 0.2, 0.2); } + + // Also return null if this ever gets to here because safety return null; } 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..f15c07d --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveWithDistance.java @@ -0,0 +1,203 @@ +/*----------------------------------------------------------------------------*/ +/* 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.DriveBase.DistanceType; + + +public class DriveWithDistance extends CommandBase { + +//Initializing private version of drivebase object +private final DriveBase driveBase; + +// initialization of the variables + public double currentRightDistance = 0; + public double currentLeftDistance = 0; + public double desiredLeftDistance = 0; + public double desiredRightDistance = 0; + public double encoderTicks; + public double leftSpeed = 0; + public double rightSpeed = 0; + public DistanceType currentType; + public double currentConversion; + + /** + * 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 currentType The distance unit type to drive in + * @param rightSpeed The right motor speed + * @param leftSpeed The left motor speed + */ + + public DriveWithDistance(double distance, DistanceType currentType) { + + //Setting current unit type and drive base variables + this.currentType = currentType; + this.driveBase = DriveBase.getInstance(); + addRequirements(driveBase); + + //Setting the current conversion type according to the chosen distance unit + switch (currentType) { + 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; + + } + + //default speed is 0.5 for the left and right talons + rightSpeed = 0.5; + leftSpeed = 0.5; + this.encoderTicks = distance * this.currentConversion; + } + + /** + * Constructor to be used when the input gives the right and left distance + * + * @param distance The distance to drive + * @param currentType The distance unit type to drive in + * @param rightSpeed The right motor speed + * @param leftSpeed The left motor speed + */ + public DriveWithDistance(double distance, DistanceType currentType, double rightSpeed, double leftSpeed) { + + //Setting current distance unit + this.currentType = currentType; + + //Initating drivebase + this.driveBase = DriveBase.getInstance(); + addRequirements(driveBase); + + //Setting the current conversion type according to the chosen distance unit + switch (currentType) { + 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; + + } + + //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.encoderTicks = distance * this.currentConversion; + + + } + + + /** + * 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() + encoderTicks; + addRequirements(DriveBase.getInstance()); + + //Dashboard + SmartDashboard.putBoolean("Running", true); + } + + /** + * 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 = DriveBase.getInstance().getRightDistance(); + + //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); + + } + + } + + /** + * Determines when the command is finished using the current distnce and desired distance + * + */ + + @Override + public boolean isFinished() { + + //Update to the current right distance that the robot has driven + currentRightDistance = DriveBase.getInstance().getRightDistance(); + + //If the robot has reached or surpassed the desired distance, then stop the robot. Otherwise, keep moving. + if(currentRightDistance >= desiredRightDistance){ + + driveBase.tankDrive(0, 0, false); + return true; + + }else{ + return false; + } + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + SmartDashboard.putBoolean("Running", false); + } + +} diff --git a/src/main/java/frc/robot/config/Config.java b/src/main/java/frc/robot/config/Config.java index c1803c9..34eb62b 100644 --- a/src/main/java/frc/robot/config/Config.java +++ b/src/main/java/frc/robot/config/Config.java @@ -4,6 +4,7 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; import frc.robot.Robot; +import frc.robot.subsystems.DriveBase; import java.io.BufferedReader; import java.io.IOException; @@ -70,10 +71,10 @@ private Config() { } // Static Constants - public static int RIGHT_FRONT_TALON = robotSpecific(3, 3, 3, 2, 2); + public static int RIGHT_FRONT_TALON = robotSpecific(3, 3, 3, 3, 3); public static int RIGHT_REAR_TALON = robotSpecific(4, 4, 4, 4, 4); public static int LEFT_FRONT_TALON = robotSpecific(1, 1, 1, 1, 1); - public static int LEFT_REAR_TALON = robotSpecific(2, 2, 2, 3, 3); + public static int LEFT_REAR_TALON = robotSpecific(2, 2, 2, 2, 2); public static int INTAKE_MOTOR = robotSpecific(-1, -1, -1, 6, -1); public static int TALON_5_PLYBOY = robotSpecific(-1, -1, -1, -1, -1, 5); @@ -94,7 +95,7 @@ private Config() { public static int RIGHT_CONTROL_STICK_X = 4; public static boolean INVERT_FIRST_AXIS = robotSpecific(true, true, true); - public static boolean INVERT_SECOND_AXIS = robotSpecific(true, true, true); + public static boolean INVERT_SECOND_AXIS = robotSpecific(false, false, false); public static double CONTROLLER_DEADBAND = 0.05; diff --git a/src/main/java/frc/robot/subsystems/DriveBase.java b/src/main/java/frc/robot/subsystems/DriveBase.java index c3a5197..fd8320b 100644 --- a/src/main/java/frc/robot/subsystems/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/DriveBase.java @@ -263,4 +263,86 @@ public void curvatureDrive(double forwardSpeed, double curveSpeed, boolean overr public boolean isBrakeMode() { return brakeMode; } + + /** + * 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); + } + + + /** + * 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; + + } } From 3dfc4c0c7f02f4f2c7d78a8a918ea30ec5034cd8 Mon Sep 17 00:00:00 2001 From: Ishan Date: Sat, 29 Feb 2020 13:50:45 -0500 Subject: [PATCH 02/10] Added driving backwards distance functionality Tested and completed. --- src/main/java/frc/robot/RobotContainer.java | 9 ++-- .../frc/robot/commands/DriveWithDistance.java | 45 ++++++++++++++++--- 2 files changed, 41 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b3ec13b..952c6f1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -53,6 +53,8 @@ public class RobotContainer { private final double RIGHT_SPEED = 0.2; private final double LEFT_SPEED = 0.2; + private final double DISTANCE = 1; + private final DriveBase.DistanceType DEFAULT_UNIT= DriveBase.DistanceType.METERS; /** * The container for the robot. Contains subsystems, OI devices, and commands. @@ -111,7 +113,6 @@ public Command getAutonomousCommand() { return null; } - else if (selectorOne == 1) { /* * When the selector is set to one, the robot will run for x seconds at y left motor speed and z right motor speed @@ -123,14 +124,10 @@ else if (selectorOne == 1) { else if(selectorOne == 2){ - - //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) - return new DriveWithDistance(53, DriveBase.DistanceType.INCHES, RIGHT_SPEED, LEFT_SPEED); + return new DriveWithDistance(-DISTANCE, DEFAULT_UNIT, RIGHT_SPEED, LEFT_SPEED); } - - // Also return null if this ever gets to here because safety return null; } diff --git a/src/main/java/frc/robot/commands/DriveWithDistance.java b/src/main/java/frc/robot/commands/DriveWithDistance.java index f15c07d..2f35844 100644 --- a/src/main/java/frc/robot/commands/DriveWithDistance.java +++ b/src/main/java/frc/robot/commands/DriveWithDistance.java @@ -28,6 +28,8 @@ public class DriveWithDistance extends CommandBase { public double rightSpeed = 0; public DistanceType currentType; public double currentConversion; + public boolean backWards = false; + 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) @@ -40,6 +42,13 @@ public class DriveWithDistance extends CommandBase { public DriveWithDistance(double distance, DistanceType currentType) { + //If you have to move backwards + if(distance < 0){ + //Error since you can't move backwards with positive speeds (0.5 is default in this constructor) + commandFinished = true; + + } + //Setting current unit type and drive base variables this.currentType = currentType; this.driveBase = DriveBase.getInstance(); @@ -88,6 +97,21 @@ public DriveWithDistance(double distance, DistanceType currentType) { */ public DriveWithDistance(double distance, DistanceType currentType, double rightSpeed, double leftSpeed) { + + //If all the values for distance and speeds are the same magnitude (+/-) then run the robot, otherwise do not. + if(distance < 0 && rightSpeed < 0 && leftSpeed < 0){ + backWards = true; + } + else if(distance < 0 && rightSpeed < 0 && leftSpeed < 0){ + backWards = false; + } + //The robot should not move if it has inverse speeds and desired distances since that must be an error + else{ + commandFinished = true; + } + + + //Setting current distance unit this.currentType = currentType; @@ -169,7 +193,7 @@ public void execute() { } - } + /** * Determines when the command is finished using the current distnce and desired distance @@ -182,16 +206,23 @@ public boolean isFinished() { //Update to the current right distance that the robot has driven currentRightDistance = DriveBase.getInstance().getRightDistance(); - //If the robot has reached or surpassed the desired distance, then stop the robot. Otherwise, keep moving. - if(currentRightDistance >= desiredRightDistance){ - + //If the robot has reached or surpassed the desired distance, then stop the robot. Otherwise, keep moving (moving forward). + if(!backWards && currentRightDistance >= desiredRightDistance){ driveBase.tankDrive(0, 0, false); - return true; + commandFinished = true; - }else{ - return false; } + //If the robot has reached or surpassed the desired distance, then stop the robot. Otherwise, keep moving (moving backward). + else if(backWards && currentRightDistance <= desiredRightDistance){ + driveBase.tankDrive(0, 0, false); + commandFinished = true; + } + else{ + commandFinished = false; + } + + return commandFinished; } // Called once the command ends or is interrupted. From bdc6586259d1de8ffc1783eddbef91745d2fc19a Mon Sep 17 00:00:00 2001 From: Ishan Date: Sat, 29 Feb 2020 15:54:25 -0500 Subject: [PATCH 03/10] Added some features to Drive with Distance --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../frc/robot/commands/DriveWithDistance.java | 203 +++++++++++++++--- 2 files changed, 180 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 952c6f1..7ace115 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -125,7 +125,7 @@ else if (selectorOne == 1) { else if(selectorOne == 2){ //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) - return new DriveWithDistance(-DISTANCE, DEFAULT_UNIT, RIGHT_SPEED, LEFT_SPEED); + return new DriveWithDistance(0, 1, DEFAULT_UNIT, 0, 0.3); } // Also return null if this ever gets to here because safety diff --git a/src/main/java/frc/robot/commands/DriveWithDistance.java b/src/main/java/frc/robot/commands/DriveWithDistance.java index 2f35844..9a7f9e8 100644 --- a/src/main/java/frc/robot/commands/DriveWithDistance.java +++ b/src/main/java/frc/robot/commands/DriveWithDistance.java @@ -24,12 +24,17 @@ public class DriveWithDistance extends CommandBase { public double desiredLeftDistance = 0; public double desiredRightDistance = 0; public double encoderTicks; + public double rightEncoderTicks, leftEncoderTicks; public double leftSpeed = 0; public double rightSpeed = 0; public DistanceType currentType; public double currentConversion; + public boolean rightBackwards = false; + public boolean leftBackwards = false; public boolean backWards = false; public boolean commandFinished = false; + public boolean isTurning = false; + public boolean end = false; /** * Constructor to be used when the input specifications do not provide the right and left speed (assumed to be 0.5) @@ -46,6 +51,7 @@ public DriveWithDistance(double distance, DistanceType currentType) { if(distance < 0){ //Error since you can't move backwards with positive speeds (0.5 is default in this constructor) commandFinished = true; + end = true; } @@ -88,7 +94,7 @@ public DriveWithDistance(double distance, DistanceType currentType) { } /** - * Constructor to be used when the input gives the right and left distance + * Constructor to be used when the input gives the right and left speed * * @param distance The distance to drive * @param currentType The distance unit type to drive in @@ -97,17 +103,103 @@ public DriveWithDistance(double distance, DistanceType currentType) { */ public DriveWithDistance(double distance, DistanceType currentType, double rightSpeed, double leftSpeed) { + //Initating drivebase + this.driveBase = DriveBase.getInstance(); //If all the values for distance and speeds are the same magnitude (+/-) then run the robot, otherwise do not. if(distance < 0 && rightSpeed < 0 && leftSpeed < 0){ backWards = true; } - else if(distance < 0 && rightSpeed < 0 && leftSpeed < 0){ + else if(distance > 0 && rightSpeed > 0 && leftSpeed > 0){ backWards = false; } //The robot should not move if it has inverse speeds and desired distances since that must be an error else{ + driveBase.tankDrive(0, 0, false); commandFinished = true; + end = true; + + } + + //Setting current distance unit + this.currentType = currentType; + + addRequirements(driveBase); + + //Setting the current conversion type according to the chosen distance unit + switch (currentType) { + 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; + + } + + //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.rightEncoderTicks = distance * this.currentConversion; + this.encoderTicks = distance * this.currentConversion; + } + + + /** + * Constructor to be used when the input gives the right and left distance + * + * @param distance The distance to drive + * @param currentType 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 currentType, double rightSpeed, double leftSpeed) { + isTurning = true; + + if(rightDistance < 0 && rightSpeed < 0){ + rightBackwards = true; + } + else if(rightDistance > 0 && rightSpeed > 0){ + rightBackwards = false; + } + //The robot should not move if it has inverse speeds and desired distances since that must be an error + else{ + DriveBase.getInstance().tankDrive(0, 0, false); + commandFinished = true; + end = true; + } + + if(leftDistance < 0 && leftSpeed < 0){ + leftBackwards = true; + + } + else if(leftDistance > 0 && leftSpeed > 0){ + + leftBackwards = false; + } + //The robot should not move if it has inverse speeds and desired distances since that must be an error + else{ + DriveBase.getInstance().tankDrive(0, 0, false); + commandFinished = true; + end = true; } @@ -151,8 +243,8 @@ else if(distance < 0 && rightSpeed < 0 && leftSpeed < 0){ this.leftSpeed = leftSpeed; //Determing the amount of desired encoder ticks to drive by getting the distnace and multiplying by the chosen distance unit - this.encoderTicks = distance * this.currentConversion; - + this.rightEncoderTicks = rightDistance * this.currentConversion; + this.leftEncoderTicks = leftDistance * this.currentConversion; } @@ -165,7 +257,9 @@ else if(distance < 0 && rightSpeed < 0 && leftSpeed < 0){ 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() + encoderTicks; + desiredRightDistance = driveBase.getRightDistance() + rightEncoderTicks; + desiredLeftDistance = driveBase.getLeftDistance() + leftEncoderTicks; + addRequirements(DriveBase.getInstance()); //Dashboard @@ -180,7 +274,8 @@ public void initialize() { public void execute() { //Get the current distance of the right encoder and store value in variable - double currentRightDistance = DriveBase.getInstance().getRightDistance(); + double currentRightDistance = DriveBase.getInstance().getRightDistance(); + double currentLeftDistance = DriveBase.getInstance().getLeftDistance(); //Run motors according to output of the speeds driveBase.tankDrive(leftSpeed, rightSpeed, false); @@ -202,26 +297,86 @@ public void execute() { @Override public boolean isFinished() { - - //Update to the current right distance that the robot has driven - currentRightDistance = DriveBase.getInstance().getRightDistance(); - //If the robot has reached or surpassed the desired distance, then stop the robot. Otherwise, keep moving (moving forward). - if(!backWards && currentRightDistance >= desiredRightDistance){ - driveBase.tankDrive(0, 0, false); - commandFinished = true; - - } - //If the robot has reached or surpassed the desired distance, then stop the robot. Otherwise, keep moving (moving backward). - else if(backWards && currentRightDistance <= desiredRightDistance){ - driveBase.tankDrive(0, 0, false); - commandFinished = true; + if (!end) { + // Update to the current right/left distance that the robot has driven + currentRightDistance = DriveBase.getInstance().getRightDistance(); + currentLeftDistance = DriveBase.getInstance().getLeftDistance(); + + // The robot is moving forward + if (!isTurning) { + // If the robot has reached or surpassed the desired distance, then stop the + // robot. Otherwise, keep moving (moving forward). + if (!backWards && currentRightDistance >= desiredRightDistance) { + driveBase.tankDrive(0, 0, false); + commandFinished = true; + + } + // If the robot has reached or surpassed the desired distance, then stop the + // robot. Otherwise, keep moving (moving backward). + else if (backWards && currentRightDistance <= desiredRightDistance) { + driveBase.tankDrive(0, 0, false); + commandFinished = true; + } else { + commandFinished = false; + } + + } + // If the robot is turning, then move the robot for the left side too + else { + + // If the robot has reached or surpassed the desired distance, then stop the + // robot. Otherwise, keep moving (moving forward). + if (!rightBackwards && currentRightDistance >= desiredRightDistance) { + driveBase.tankDrive(leftSpeed, 0, false); + commandFinished = true; + + } + // If the robot has reached or surpassed the desired distance, then stop the + // robot. Otherwise, keep moving (moving backward). + else if (rightBackwards && currentRightDistance <= desiredRightDistance) { + System.out.println("Left speed is: " + leftSpeed); + driveBase.tankDrive(leftSpeed, 0, false); + commandFinished = true; + } else { + commandFinished = false; + } + + // If the robot has reached or surpassed the desired distance, then stop the + // robot. Otherwise, keep moving (moving forward). + if (!leftBackwards && currentLeftDistance >= desiredLeftDistance) { + driveBase.tankDrive(0, rightSpeed, false); + commandFinished = true; + + } + // If the robot has reached or surpassed the desired distance, then stop the + // robot. Otherwise, keep moving (moving backward). + else if (leftBackwards && currentLeftDistance <= desiredLeftDistance) { + driveBase.tankDrive(0, rightSpeed, false); + commandFinished = true; + } else { + commandFinished = false; + } + + if(leftSpeed == 0 && rightSpeed == 0){ + commandFinished = true; + end = true; + } + + if(desiredLeftDistance == currentLeftDistance && desiredRightDistance == currentRightDistance) + { + commandFinished = true; + end = true; + } + + if((leftSpeed == 0 && rightSpeed == 0) || (desiredLeftDistance >= currentLeftDistance && (desiredRightDistance >= currentRightDistance))){ + commandFinished = false; + + } + } + + } - else{ - commandFinished = false; - } - - return commandFinished; } From fbe749beaca7dbc86d8a7491bdadcd6965b32011 Mon Sep 17 00:00:00 2001 From: Ishan Date: Sat, 29 Feb 2020 16:40:18 -0500 Subject: [PATCH 04/10] Fixed not moving bug errors Will test Monday and then do pull request/merge. --- .../frc/robot/commands/DriveWithDistance.java | 25 +++++++------------ 1 file changed, 9 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/commands/DriveWithDistance.java b/src/main/java/frc/robot/commands/DriveWithDistance.java index 9a7f9e8..e8b758b 100644 --- a/src/main/java/frc/robot/commands/DriveWithDistance.java +++ b/src/main/java/frc/robot/commands/DriveWithDistance.java @@ -328,16 +328,13 @@ else if (backWards && currentRightDistance <= desiredRightDistance) { // If the robot has reached or surpassed the desired distance, then stop the // robot. Otherwise, keep moving (moving forward). if (!rightBackwards && currentRightDistance >= desiredRightDistance) { - driveBase.tankDrive(leftSpeed, 0, false); - commandFinished = true; - + rightSpeed = 0; } // If the robot has reached or surpassed the desired distance, then stop the // robot. Otherwise, keep moving (moving backward). else if (rightBackwards && currentRightDistance <= desiredRightDistance) { - System.out.println("Left speed is: " + leftSpeed); - driveBase.tankDrive(leftSpeed, 0, false); - commandFinished = true; + rightSpeed = 0; + } else { commandFinished = false; } @@ -345,37 +342,33 @@ else if (rightBackwards && currentRightDistance <= desiredRightDistance) { // If the robot has reached or surpassed the desired distance, then stop the // robot. Otherwise, keep moving (moving forward). if (!leftBackwards && currentLeftDistance >= desiredLeftDistance) { - driveBase.tankDrive(0, rightSpeed, false); - commandFinished = true; - + leftSpeed = 0; } // If the robot has reached or surpassed the desired distance, then stop the // robot. Otherwise, keep moving (moving backward). else if (leftBackwards && currentLeftDistance <= desiredLeftDistance) { - driveBase.tankDrive(0, rightSpeed, false); - commandFinished = true; + leftSpeed = 0; } else { commandFinished = false; } + //If the left and right speed are zero, just stop the robot if(leftSpeed == 0 && rightSpeed == 0){ commandFinished = true; end = true; } + //If the desired and current distance are equal, don't moce the robot if(desiredLeftDistance == currentLeftDistance && desiredRightDistance == currentRightDistance) { commandFinished = true; end = true; } - if((leftSpeed == 0 && rightSpeed == 0) || (desiredLeftDistance >= currentLeftDistance && (desiredRightDistance >= currentRightDistance))){ - commandFinished = false; + //Taking the changed left and right speed and adding their updated values to the drivebase + driveBase.tankDrive(leftSpeed, rightSpeed, false); - } } - - } return commandFinished; } From 760954b794338255e52026d97266f64f7748ceeb Mon Sep 17 00:00:00 2001 From: Ishan Date: Wed, 4 Mar 2020 20:57:18 -0500 Subject: [PATCH 05/10] Resolved some changes for reviewed code Resolved changes for code but still need to implement more changes. --- src/main/java/frc/robot/RobotContainer.java | 23 +-- .../frc/robot/commands/DriveWithDistance.java | 137 +++++++++--------- src/main/java/frc/robot/config/Config.java | 10 +- 3 files changed, 84 insertions(+), 86 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7ace115..62f465c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -50,12 +50,6 @@ public class RobotContainer { private Command intakeCommand; private Command sensitiveDriverControlCommand; private Logger logger = Logger.getLogger("RobotContainer"); - - private final double RIGHT_SPEED = 0.2; - private final double LEFT_SPEED = 0.2; - private final double DISTANCE = 1; - private final DriveBase.DistanceType DEFAULT_UNIT= DriveBase.DistanceType.METERS; - /** * The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -101,14 +95,14 @@ private void configureButtonBindings() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - int selectorOne = 0, selectorTwo = 0; + + //Robot has only one selector + int selectorOne = 0; if (analogSelectorOne != null) selectorOne = analogSelectorOne.getIndex(); - if (analogSelectorTwo != null) - selectorTwo = analogSelectorTwo.getIndex(); - logger.info("Selectors: " + selectorOne + " " + selectorTwo); + logger.info("Selectors: " + selectorOne); - if (selectorOne == 0 && selectorTwo == 0) { + if (selectorOne == 0) { // This is our 'do nothing' selector return null; } @@ -119,16 +113,15 @@ else if (selectorOne == 1) { */ // for the inputed variables: DriveWithTime(seconds (double), left motors speed (double), right motors speed (double)) - return new DriveWithTime(1.0, 0.2, 0.2); + return new DriveWithTime(Config.AUTO_DRIVE_TIME, Config.AUTO_LEFT_MOTOR_SPEED, Config.AUTO_RIGHT_MOTOR_SPEED); } else if(selectorOne == 2){ //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) - return new DriveWithDistance(0, 1, DEFAULT_UNIT, 0, 0.3); - + 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 + // Also return null (do nothing) if this ever gets to here because safety return null; } diff --git a/src/main/java/frc/robot/commands/DriveWithDistance.java b/src/main/java/frc/robot/commands/DriveWithDistance.java index e8b758b..c86336d 100644 --- a/src/main/java/frc/robot/commands/DriveWithDistance.java +++ b/src/main/java/frc/robot/commands/DriveWithDistance.java @@ -18,37 +18,45 @@ public class DriveWithDistance extends CommandBase { //Initializing private version of drivebase object private final DriveBase driveBase; +//The distance that the robot must move below to move backwards +private final double DISTANCE_NOT_MOVING = 0; + +//The speed that the robot must move below to move backwards +private final double SPEED_NOT_MOVING = 0; + +private final double DEFAULT_RIGHT_SPEED = 0.5; +private final double DEFAULT_LEFT_SPEED = 0.5; + // initialization of the variables public double currentRightDistance = 0; public double currentLeftDistance = 0; public double desiredLeftDistance = 0; public double desiredRightDistance = 0; public double encoderTicks; - public double rightEncoderTicks, leftEncoderTicks; + public double desiredRightEncoderTicks, desiredLeftEncoderTicks; public double leftSpeed = 0; public double rightSpeed = 0; - public DistanceType currentType; + public DistanceType distanceType; public double currentConversion; - public boolean rightBackwards = false; - public boolean leftBackwards = false; - public boolean backWards = false; - public boolean commandFinished = false; + public boolean rightMotorMovingBackwards = false; + public boolean leftMotorMovingBackwards = false; + public boolean robotMovingBackWards = false; public boolean isTurning = false; public boolean end = false; + 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 currentType The distance unit type to drive in - * @param rightSpeed The right motor speed - * @param leftSpeed The left motor speed + * @param distanceType The distance unit type to drive in */ - public DriveWithDistance(double distance, DistanceType currentType) { + public DriveWithDistance(double distance, DistanceType distanceType) { //If you have to move backwards - if(distance < 0){ + if(distance < DISTANCE_NOT_MOVING){ //Error since you can't move backwards with positive speeds (0.5 is default in this constructor) commandFinished = true; end = true; @@ -56,12 +64,12 @@ public DriveWithDistance(double distance, DistanceType currentType) { } //Setting current unit type and drive base variables - this.currentType = currentType; + this.distanceType = distanceType; this.driveBase = DriveBase.getInstance(); addRequirements(driveBase); //Setting the current conversion type according to the chosen distance unit - switch (currentType) { + switch (distanceType) { case FEET: this.currentConversion = DriveBase.DistanceUnit.FEET.encoderTicks; @@ -88,8 +96,8 @@ public DriveWithDistance(double distance, DistanceType currentType) { } //default speed is 0.5 for the left and right talons - rightSpeed = 0.5; - leftSpeed = 0.5; + rightSpeed = DEFAULT_RIGHT_SPEED; + leftSpeed = DEFAULT_LEFT_SPEED; this.encoderTicks = distance * this.currentConversion; } @@ -97,37 +105,37 @@ public DriveWithDistance(double distance, DistanceType currentType) { * Constructor to be used when the input gives the right and left speed * * @param distance The distance to drive - * @param currentType The distance unit type to drive in + * @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 currentType, double rightSpeed, double leftSpeed) { + public DriveWithDistance(double distance, DistanceType distanceType, double rightSpeed, double leftSpeed) { //Initating drivebase this.driveBase = DriveBase.getInstance(); //If all the values for distance and speeds are the same magnitude (+/-) then run the robot, otherwise do not. - if(distance < 0 && rightSpeed < 0 && leftSpeed < 0){ - backWards = true; + if(distance < DISTANCE_NOT_MOVING && rightSpeed < SPEED_NOT_MOVING && leftSpeed < SPEED_NOT_MOVING){ + robotMovingBackWards = true; } - else if(distance > 0 && rightSpeed > 0 && leftSpeed > 0){ - backWards = false; + else if(distance > DISTANCE_NOT_MOVING && rightSpeed > SPEED_NOT_MOVING && leftSpeed > SPEED_NOT_MOVING){ + robotMovingBackWards = false; } //The robot should not move if it has inverse speeds and desired distances since that must be an error else{ - driveBase.tankDrive(0, 0, false); + driveBase.tankDrive(SPEED_NOT_MOVING, SPEED_NOT_MOVING, false); commandFinished = true; end = true; } //Setting current distance unit - this.currentType = currentType; + this.distanceType = distanceType; addRequirements(driveBase); //Setting the current conversion type according to the chosen distance unit - switch (currentType) { + switch (distanceType) { case FEET: this.currentConversion = DriveBase.DistanceUnit.FEET.encoderTicks; @@ -158,7 +166,7 @@ else if(distance > 0 && rightSpeed > 0 && leftSpeed > 0){ this.leftSpeed = leftSpeed; //Determing the amount of desired encoder ticks to drive by getting the distnace and multiplying by the chosen distance unit - this.rightEncoderTicks = distance * this.currentConversion; + this.desiredRightEncoderTicks = distance * this.currentConversion; this.encoderTicks = distance * this.currentConversion; } @@ -166,53 +174,52 @@ else if(distance > 0 && rightSpeed > 0 && leftSpeed > 0){ /** * Constructor to be used when the input gives the right and left distance * - * @param distance The distance to drive - * @param currentType The distance unit type to drive in + * @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 currentType, double rightSpeed, double leftSpeed) { + public DriveWithDistance(double rightDistance, double leftDistance, DistanceType distanceType, double rightSpeed, double leftSpeed) { isTurning = true; - if(rightDistance < 0 && rightSpeed < 0){ - rightBackwards = true; + if(rightDistance < DISTANCE_NOT_MOVING && rightSpeed < DISTANCE_NOT_MOVING){ + rightMotorMovingBackwards = true; } - else if(rightDistance > 0 && rightSpeed > 0){ - rightBackwards = false; + else if(rightDistance > DISTANCE_NOT_MOVING && rightSpeed > SPEED_NOT_MOVING){ + rightMotorMovingBackwards = false; } //The robot should not move if it has inverse speeds and desired distances since that must be an error else{ - DriveBase.getInstance().tankDrive(0, 0, false); + DriveBase.getInstance().tankDrive(SPEED_NOT_MOVING, SPEED_NOT_MOVING, false); commandFinished = true; end = true; } - if(leftDistance < 0 && leftSpeed < 0){ - leftBackwards = true; + if(leftDistance < SPEED_NOT_MOVING && leftSpeed < SPEED_NOT_MOVING){ + leftMotorMovingBackwards = true; } - else if(leftDistance > 0 && leftSpeed > 0){ + else if(leftDistance > DISTANCE_NOT_MOVING && leftSpeed > SPEED_NOT_MOVING){ - leftBackwards = false; + leftMotorMovingBackwards = false; } //The robot should not move if it has inverse speeds and desired distances since that must be an error else{ - DriveBase.getInstance().tankDrive(0, 0, false); + DriveBase.getInstance().tankDrive(SPEED_NOT_MOVING, SPEED_NOT_MOVING, false); commandFinished = true; end = true; } - - //Setting current distance unit - this.currentType = currentType; + this.distanceType = distanceType; //Initating drivebase this.driveBase = DriveBase.getInstance(); addRequirements(driveBase); //Setting the current conversion type according to the chosen distance unit - switch (currentType) { + switch (distanceType) { case FEET: this.currentConversion = DriveBase.DistanceUnit.FEET.encoderTicks; @@ -243,11 +250,10 @@ else if(leftDistance > 0 && leftSpeed > 0){ this.leftSpeed = leftSpeed; //Determing the amount of desired encoder ticks to drive by getting the distnace and multiplying by the chosen distance unit - this.rightEncoderTicks = rightDistance * this.currentConversion; - this.leftEncoderTicks = leftDistance * this.currentConversion; + this.desiredRightEncoderTicks = rightDistance * this.currentConversion; + this.desiredLeftEncoderTicks = leftDistance * this.currentConversion; } - /** * Called one when the command is initially scheduled and determines the desired distance to travel @@ -257,13 +263,11 @@ else if(leftDistance > 0 && leftSpeed > 0){ 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() + rightEncoderTicks; - desiredLeftDistance = driveBase.getLeftDistance() + leftEncoderTicks; + desiredRightDistance = driveBase.getRightDistance() + desiredRightEncoderTicks; + desiredLeftDistance = driveBase.getLeftDistance() + desiredLeftEncoderTicks; addRequirements(DriveBase.getInstance()); - //Dashboard - SmartDashboard.putBoolean("Running", true); } /** @@ -298,6 +302,8 @@ public void execute() { @Override public boolean isFinished() { + + if (!end) { // Update to the current right/left distance that the robot has driven currentRightDistance = DriveBase.getInstance().getRightDistance(); @@ -307,15 +313,15 @@ public boolean isFinished() { if (!isTurning) { // If the robot has reached or surpassed the desired distance, then stop the // robot. Otherwise, keep moving (moving forward). - if (!backWards && currentRightDistance >= desiredRightDistance) { - driveBase.tankDrive(0, 0, false); + if (!robotMovingBackWards && currentRightDistance >= desiredRightDistance) { + driveBase.tankDrive(SPEED_NOT_MOVING, SPEED_NOT_MOVING, false); commandFinished = true; } // If the robot has reached or surpassed the desired distance, then stop the // robot. Otherwise, keep moving (moving backward). - else if (backWards && currentRightDistance <= desiredRightDistance) { - driveBase.tankDrive(0, 0, false); + else if (robotMovingBackWards && currentRightDistance <= desiredRightDistance) { + driveBase.tankDrive(SPEED_NOT_MOVING, SPEED_NOT_MOVING, false); commandFinished = true; } else { commandFinished = false; @@ -326,34 +332,25 @@ else if (backWards && currentRightDistance <= desiredRightDistance) { else { // If the robot has reached or surpassed the desired distance, then stop the - // robot. Otherwise, keep moving (moving forward). - if (!rightBackwards && currentRightDistance >= desiredRightDistance) { - rightSpeed = 0; + // robot. Otherwise, keep moving (moving forward OR moving backwards). + if ((!rightMotorMovingBackwards && currentRightDistance >= desiredRightDistance) || (rightMotorMovingBackwards && currentRightDistance <= desiredRightDistance)) { + rightSpeed = SPEED_NOT_MOVING; } - // If the robot has reached or surpassed the desired distance, then stop the - // robot. Otherwise, keep moving (moving backward). - else if (rightBackwards && currentRightDistance <= desiredRightDistance) { - rightSpeed = 0; - - } else { + else { commandFinished = false; } // If the robot has reached or surpassed the desired distance, then stop the // robot. Otherwise, keep moving (moving forward). - if (!leftBackwards && currentLeftDistance >= desiredLeftDistance) { - leftSpeed = 0; + if ((!leftMotorMovingBackwards && currentLeftDistance >= desiredLeftDistance) || (leftMotorMovingBackwards && currentLeftDistance <= desiredLeftDistance)) { + leftSpeed = SPEED_NOT_MOVING; } - // If the robot has reached or surpassed the desired distance, then stop the - // robot. Otherwise, keep moving (moving backward). - else if (leftBackwards && currentLeftDistance <= desiredLeftDistance) { - leftSpeed = 0; - } else { + else { commandFinished = false; } //If the left and right speed are zero, just stop the robot - if(leftSpeed == 0 && rightSpeed == 0){ + if(leftSpeed == SPEED_NOT_MOVING && rightSpeed == SPEED_NOT_MOVING){ commandFinished = true; end = true; } diff --git a/src/main/java/frc/robot/config/Config.java b/src/main/java/frc/robot/config/Config.java index 34eb62b..670aca8 100644 --- a/src/main/java/frc/robot/config/Config.java +++ b/src/main/java/frc/robot/config/Config.java @@ -118,7 +118,15 @@ private Config() { public static double ARM_PID_I = robotSpecific(0.0); public static double ARM_PID_D = robotSpecific(0.1); public static double ARM_PID_F = robotSpecific(0.0); - + + 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"); From ce7113875b39902417863ab0638ffa8de128403c Mon Sep 17 00:00:00 2001 From: Ishan Date: Sat, 7 Mar 2020 12:55:56 -0500 Subject: [PATCH 06/10] Rebasing changes onto drive with distance Handles the changes in master for pre2020 and 2020 drivebase. --- .../frc/robot/commands/DriveWithDistance.java | 155 +++++++----------- .../java/frc/robot/subsystems/DriveBase.java | 40 +++-- .../robot/subsystems/DriveBasePre2020.java | 42 +++++ 3 files changed, 118 insertions(+), 119 deletions(-) diff --git a/src/main/java/frc/robot/commands/DriveWithDistance.java b/src/main/java/frc/robot/commands/DriveWithDistance.java index c86336d..3761742 100644 --- a/src/main/java/frc/robot/commands/DriveWithDistance.java +++ b/src/main/java/frc/robot/commands/DriveWithDistance.java @@ -10,6 +10,7 @@ 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; @@ -62,13 +63,26 @@ public DriveWithDistance(double distance, DistanceType distanceType) { end = true; } - //Setting current unit type and drive base variables this.distanceType = distanceType; - this.driveBase = DriveBase.getInstance(); + this.driveBase = DriveBaseHolder.getInstance(); addRequirements(driveBase); - - //Setting the current conversion type according to the chosen distance unit + assignDistanceType(distanceType); + + //default speed is 0.5 for the left and right talons + rightSpeed = DEFAULT_RIGHT_SPEED; + leftSpeed = DEFAULT_LEFT_SPEED; + this.encoderTicks = distance * 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; @@ -90,76 +104,47 @@ public DriveWithDistance(double distance, DistanceType distanceType) { //Default is meters default: - this.currentConversion = DriveBase.DistanceUnit.METERS.encoderTicks; - break; - - } + this.currentConversion = DriveBase.DistanceUnit.METERS.encoderTicks; + break; - //default speed is 0.5 for the left and right talons - rightSpeed = DEFAULT_RIGHT_SPEED; - leftSpeed = DEFAULT_LEFT_SPEED; - this.encoderTicks = distance * this.currentConversion; + } } /** * Constructor to be used when the input gives the right and left speed * - * @param distance The distance to drive + * @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 + * @param rightSpeed The right motor speed + * @param leftSpeed The left motor speed */ public DriveWithDistance(double distance, DistanceType distanceType, double rightSpeed, double leftSpeed) { - //Initating drivebase - this.driveBase = DriveBase.getInstance(); + // Initating drivebase + this.driveBase = DriveBaseHolder.getInstance(); - //If all the values for distance and speeds are the same magnitude (+/-) then run the robot, otherwise do not. - if(distance < DISTANCE_NOT_MOVING && rightSpeed < SPEED_NOT_MOVING && leftSpeed < SPEED_NOT_MOVING){ + // If all the values for distance and speeds are the same magnitude (+/-) then + // run the robot, otherwise do not. + if (distance < DISTANCE_NOT_MOVING && rightSpeed < SPEED_NOT_MOVING && leftSpeed < SPEED_NOT_MOVING) { robotMovingBackWards = true; - } - else if(distance > DISTANCE_NOT_MOVING && rightSpeed > SPEED_NOT_MOVING && leftSpeed > SPEED_NOT_MOVING){ + } else if (distance > DISTANCE_NOT_MOVING && rightSpeed > SPEED_NOT_MOVING && leftSpeed > SPEED_NOT_MOVING) { robotMovingBackWards = false; } - //The robot should not move if it has inverse speeds and desired distances since that must be an error - else{ + // The robot should not move if it has inverse speeds and desired distances + // since that must be an error + else { driveBase.tankDrive(SPEED_NOT_MOVING, SPEED_NOT_MOVING, false); commandFinished = true; end = true; } - //Setting current distance unit + // Setting current distance unit this.distanceType = distanceType; addRequirements(driveBase); - //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; - - } + assignDistanceType(distanceType); //Setting the right speed and left speed according to input specifications this.rightSpeed = rightSpeed; @@ -191,7 +176,7 @@ else if(rightDistance > DISTANCE_NOT_MOVING && rightSpeed > SPEED_NOT_MOVING){ } //The robot should not move if it has inverse speeds and desired distances since that must be an error else{ - DriveBase.getInstance().tankDrive(SPEED_NOT_MOVING, SPEED_NOT_MOVING, false); + DriveBaseHolder.getInstance().tankDrive(SPEED_NOT_MOVING, SPEED_NOT_MOVING, false); commandFinished = true; end = true; } @@ -206,7 +191,7 @@ else if(leftDistance > DISTANCE_NOT_MOVING && leftSpeed > SPEED_NOT_MOVING){ } //The robot should not move if it has inverse speeds and desired distances since that must be an error else{ - DriveBase.getInstance().tankDrive(SPEED_NOT_MOVING, SPEED_NOT_MOVING, false); + DriveBaseHolder.getInstance().tankDrive(SPEED_NOT_MOVING, SPEED_NOT_MOVING, false); commandFinished = true; end = true; } @@ -215,35 +200,10 @@ else if(leftDistance > DISTANCE_NOT_MOVING && leftSpeed > SPEED_NOT_MOVING){ this.distanceType = distanceType; //Initating drivebase - this.driveBase = DriveBase.getInstance(); + this.driveBase = DriveBaseHolder.getInstance(); addRequirements(driveBase); - //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; - - } + assignDistanceType(distanceType); //Setting the right speed and left speed according to input specifications this.rightSpeed = rightSpeed; @@ -266,7 +226,7 @@ public void initialize() { desiredRightDistance = driveBase.getRightDistance() + desiredRightEncoderTicks; desiredLeftDistance = driveBase.getLeftDistance() + desiredLeftEncoderTicks; - addRequirements(DriveBase.getInstance()); + addRequirements(DriveBaseHolder.getInstance()); } @@ -278,8 +238,8 @@ public void initialize() { public void execute() { //Get the current distance of the right encoder and store value in variable - double currentRightDistance = DriveBase.getInstance().getRightDistance(); - double currentLeftDistance = DriveBase.getInstance().getLeftDistance(); + double currentRightDistance = DriveBaseHolder.getInstance().getRightDistance(); + double currentLeftDistance = DriveBaseHolder.getInstance().getLeftDistance(); //Run motors according to output of the speeds driveBase.tankDrive(leftSpeed, rightSpeed, false); @@ -289,25 +249,14 @@ public void execute() { SmartDashboard.putNumber("Right Speed", rightSpeed); SmartDashboard.putNumber("Left Error", desiredLeftDistance - currentLeftDistance); SmartDashboard.putNumber("Right Error", desiredRightDistance - currentRightDistance); - - } - - - /** - * Determines when the command is finished using the current distnce and desired distance - * - */ - @Override - public boolean isFinished() { - - + //Code for figuring out when the robot is finished driving and handeling user input validation if (!end) { // Update to the current right/left distance that the robot has driven - currentRightDistance = DriveBase.getInstance().getRightDistance(); - currentLeftDistance = DriveBase.getInstance().getLeftDistance(); + currentRightDistance = DriveBaseHolder.getInstance().getRightDistance(); + currentLeftDistance = DriveBaseHolder.getInstance().getLeftDistance(); // The robot is moving forward if (!isTurning) { @@ -355,7 +304,7 @@ else if (robotMovingBackWards && currentRightDistance <= desiredRightDistance) { end = true; } - //If the desired and current distance are equal, don't moce the robot + //If the desired and current distance are equal, don't move the robot if(desiredLeftDistance == currentLeftDistance && desiredRightDistance == currentRightDistance) { commandFinished = true; @@ -367,13 +316,23 @@ else if (robotMovingBackWards && currentRightDistance <= desiredRightDistance) { } } + + } + + /** + * Determines when the command is finished using the current distnce and desired distance + * + */ + + @Override + public boolean isFinished() { + return commandFinished; } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - SmartDashboard.putBoolean("Running", false); } } diff --git a/src/main/java/frc/robot/subsystems/DriveBase.java b/src/main/java/frc/robot/subsystems/DriveBase.java index a1858cf..db23e2b 100644 --- a/src/main/java/frc/robot/subsystems/DriveBase.java +++ b/src/main/java/frc/robot/subsystems/DriveBase.java @@ -153,59 +153,57 @@ public final NeutralMode getNeutralMode() { */ protected void driveModeUpdated(DriveMode driveMode) { - } - - /** - * This is a callback for the drive base to update their motors with the new neutral mode. - * It's not required to override this if you don't need it. - * @param neutralMode the mode we switched into - */ - protected void neutralModeUpdated(NeutralMode neutralMode) { - } /** * Get the distance travelled by the left encoder in encoder ticks * - * @return The distance of the left encoder + * @return The distance of the right encoder (0 before overide) */ - public double getLeftDistance() { - return (-leftFrontTalon.getSensorCollection().getQuadraturePosition()); + public double getLeftDistance(){ + return 0; } /** * Get the distance travelled by the right encoder in encoder ticks * - * @return The distance of the right encoder + * @return The distance of the right encoder (0 before overide) */ public double getRightDistance() { - return rightFrontTalon.getSensorCollection().getQuadraturePosition(); + return 0; } /** * Get the speed of the left encoder in encoder ticks per second * - * @return The speed of the left encoder + * @return The speed of the left encoder (0 before overide) */ public double getLeftSpeed() { - return (-leftFrontTalon.getSensorCollection().getQuadratureVelocity()); + return 0; } /** * Get the speed of the right encoder in encoder ticks per second * - * @return The speed of the right encoder + * @return The speed of the right encoder (0 before overide) */ public double getRightSpeed() { - return rightFrontTalon.getSensorCollection().getQuadratureVelocity(); + return 0; } //Reset encoder values public void resetEncoders() { - leftFrontTalon.getSensorCollection().setQuadraturePosition(0, Config.CAN_TIMEOUT_SHORT); - rightFrontTalon.getSensorCollection().setQuadraturePosition(0, Config.CAN_TIMEOUT_SHORT); - } + } + + /** + * This is a callback for the drive base to update their motors with the new neutral mode. + * It's not required to override this if you don't need it. + * @param neutralMode the mode we switched into + */ + protected void neutralModeUpdated(NeutralMode neutralMode) { + + } /** * Measured distance units for converting ticks to units or units to encoder ticks diff --git a/src/main/java/frc/robot/subsystems/DriveBasePre2020.java b/src/main/java/frc/robot/subsystems/DriveBasePre2020.java index bc398d2..296ce23 100644 --- a/src/main/java/frc/robot/subsystems/DriveBasePre2020.java +++ b/src/main/java/frc/robot/subsystems/DriveBasePre2020.java @@ -79,6 +79,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 */ From f40e40b07405fabe4351cba56fac4eba1afc38ae Mon Sep 17 00:00:00 2001 From: George Tzavelas Date: Sat, 7 Mar 2020 14:14:17 -0500 Subject: [PATCH 07/10] Pair programming Ishan and George, Power team --- .../frc/robot/commands/DriveWithDistance.java | 283 +++++------------- 1 file changed, 70 insertions(+), 213 deletions(-) diff --git a/src/main/java/frc/robot/commands/DriveWithDistance.java b/src/main/java/frc/robot/commands/DriveWithDistance.java index 3761742..1cf0012 100644 --- a/src/main/java/frc/robot/commands/DriveWithDistance.java +++ b/src/main/java/frc/robot/commands/DriveWithDistance.java @@ -7,8 +7,6 @@ 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; @@ -16,36 +14,24 @@ public class DriveWithDistance extends CommandBase { -//Initializing private version of drivebase object -private final DriveBase driveBase; + //Initializing private version of drivebase object + private final DriveBase driveBase; -//The distance that the robot must move below to move backwards -private final double DISTANCE_NOT_MOVING = 0; + //The speed that the robot must move below to move backwards + private final static double ZERO_SPEED = 0; -//The speed that the robot must move below to move backwards -private final double SPEED_NOT_MOVING = 0; + private final static double DEFAULT_RIGHT_SPEED = 0.5; + private final static double DEFAULT_LEFT_SPEED = 0.5; -private final double DEFAULT_RIGHT_SPEED = 0.5; -private final double DEFAULT_LEFT_SPEED = 0.5; - -// initialization of the variables - public double currentRightDistance = 0; - public double currentLeftDistance = 0; + // initialization of the variables public double desiredLeftDistance = 0; public double desiredRightDistance = 0; - public double encoderTicks; public double desiredRightEncoderTicks, desiredLeftEncoderTicks; public double leftSpeed = 0; public double rightSpeed = 0; public DistanceType distanceType; public double currentConversion; - public boolean rightMotorMovingBackwards = false; - public boolean leftMotorMovingBackwards = false; - public boolean robotMovingBackWards = false; - public boolean isTurning = false; - public boolean end = false; 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) @@ -55,59 +41,7 @@ public class DriveWithDistance extends CommandBase { */ public DriveWithDistance(double distance, DistanceType distanceType) { - - //If you have to move backwards - if(distance < DISTANCE_NOT_MOVING){ - //Error since you can't move backwards with positive speeds (0.5 is default in this constructor) - commandFinished = true; - end = true; - - } - //Setting current unit type and drive base variables - this.distanceType = distanceType; - this.driveBase = DriveBaseHolder.getInstance(); - addRequirements(driveBase); - assignDistanceType(distanceType); - - //default speed is 0.5 for the left and right talons - rightSpeed = DEFAULT_RIGHT_SPEED; - leftSpeed = DEFAULT_LEFT_SPEED; - this.encoderTicks = distance * 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; - - } + this(distance, distanceType, DEFAULT_RIGHT_SPEED, DEFAULT_LEFT_SPEED); } /** @@ -119,40 +53,7 @@ private void assignDistanceType(DistanceType distanceType) { * @param leftSpeed The left motor speed */ public DriveWithDistance(double distance, DistanceType distanceType, double rightSpeed, double leftSpeed) { - - // Initating drivebase - this.driveBase = DriveBaseHolder.getInstance(); - - // If all the values for distance and speeds are the same magnitude (+/-) then - // run the robot, otherwise do not. - if (distance < DISTANCE_NOT_MOVING && rightSpeed < SPEED_NOT_MOVING && leftSpeed < SPEED_NOT_MOVING) { - robotMovingBackWards = true; - } else if (distance > DISTANCE_NOT_MOVING && rightSpeed > SPEED_NOT_MOVING && leftSpeed > SPEED_NOT_MOVING) { - robotMovingBackWards = false; - } - // The robot should not move if it has inverse speeds and desired distances - // since that must be an error - else { - driveBase.tankDrive(SPEED_NOT_MOVING, SPEED_NOT_MOVING, false); - commandFinished = true; - end = true; - - } - - // 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 = distance * this.currentConversion; - this.encoderTicks = distance * this.currentConversion; + this(distance, distance, distanceType, rightSpeed, leftSpeed); } @@ -166,41 +67,12 @@ public DriveWithDistance(double distance, DistanceType distanceType, double righ * @param leftSpeed The left motor speed */ public DriveWithDistance(double rightDistance, double leftDistance, DistanceType distanceType, double rightSpeed, double leftSpeed) { - isTurning = true; - - if(rightDistance < DISTANCE_NOT_MOVING && rightSpeed < DISTANCE_NOT_MOVING){ - rightMotorMovingBackwards = true; - } - else if(rightDistance > DISTANCE_NOT_MOVING && rightSpeed > SPEED_NOT_MOVING){ - rightMotorMovingBackwards = false; - } - //The robot should not move if it has inverse speeds and desired distances since that must be an error - else{ - DriveBaseHolder.getInstance().tankDrive(SPEED_NOT_MOVING, SPEED_NOT_MOVING, false); - commandFinished = true; - end = true; - } - - if(leftDistance < SPEED_NOT_MOVING && leftSpeed < SPEED_NOT_MOVING){ - leftMotorMovingBackwards = true; - - } - else if(leftDistance > DISTANCE_NOT_MOVING && leftSpeed > SPEED_NOT_MOVING){ - - leftMotorMovingBackwards = false; - } - //The robot should not move if it has inverse speeds and desired distances since that must be an error - else{ - DriveBaseHolder.getInstance().tankDrive(SPEED_NOT_MOVING, SPEED_NOT_MOVING, false); - commandFinished = true; - end = true; - } + //Initating drivebase + this.driveBase = DriveBaseHolder.getInstance(); //Setting current distance unit this.distanceType = distanceType; - //Initating drivebase - this.driveBase = DriveBaseHolder.getInstance(); addRequirements(driveBase); assignDistanceType(distanceType); @@ -214,7 +86,41 @@ else if(leftDistance > DISTANCE_NOT_MOVING && leftSpeed > SPEED_NOT_MOVING){ 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 * @@ -227,7 +133,6 @@ public void initialize() { desiredLeftDistance = driveBase.getLeftDistance() + desiredLeftEncoderTicks; addRequirements(DriveBaseHolder.getInstance()); - } /** @@ -236,7 +141,7 @@ public void initialize() { */ @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(); @@ -250,73 +155,32 @@ public void execute() { SmartDashboard.putNumber("Left Error", desiredLeftDistance - currentLeftDistance); SmartDashboard.putNumber("Right Error", desiredRightDistance - currentRightDistance); - //Code for figuring out when the robot is finished driving and handeling user input validation - if (!end) { - // Update to the current right/left distance that the robot has driven - currentRightDistance = DriveBaseHolder.getInstance().getRightDistance(); - currentLeftDistance = DriveBaseHolder.getInstance().getLeftDistance(); - - // The robot is moving forward - if (!isTurning) { - // If the robot has reached or surpassed the desired distance, then stop the - // robot. Otherwise, keep moving (moving forward). - if (!robotMovingBackWards && currentRightDistance >= desiredRightDistance) { - driveBase.tankDrive(SPEED_NOT_MOVING, SPEED_NOT_MOVING, false); - commandFinished = true; - - } - // If the robot has reached or surpassed the desired distance, then stop the - // robot. Otherwise, keep moving (moving backward). - else if (robotMovingBackWards && currentRightDistance <= desiredRightDistance) { - driveBase.tankDrive(SPEED_NOT_MOVING, SPEED_NOT_MOVING, false); - commandFinished = true; - } else { - commandFinished = false; - } - - } - // If the robot is turning, then move the robot for the left side too - else { - - // If the robot has reached or surpassed the desired distance, then stop the - // robot. Otherwise, keep moving (moving forward OR moving backwards). - if ((!rightMotorMovingBackwards && currentRightDistance >= desiredRightDistance) || (rightMotorMovingBackwards && currentRightDistance <= desiredRightDistance)) { - rightSpeed = SPEED_NOT_MOVING; - } - else { - commandFinished = false; - } - - // If the robot has reached or surpassed the desired distance, then stop the - // robot. Otherwise, keep moving (moving forward). - if ((!leftMotorMovingBackwards && currentLeftDistance >= desiredLeftDistance) || (leftMotorMovingBackwards && currentLeftDistance <= desiredLeftDistance)) { - leftSpeed = SPEED_NOT_MOVING; - } - else { - commandFinished = false; - } - - //If the left and right speed are zero, just stop the robot - if(leftSpeed == SPEED_NOT_MOVING && rightSpeed == SPEED_NOT_MOVING){ - commandFinished = true; - end = true; - } - - //If the desired and current distance are equal, don't move the robot - if(desiredLeftDistance == currentLeftDistance && desiredRightDistance == currentRightDistance) - { - commandFinished = true; - end = true; - } - - //Taking the changed left and right speed and adding their updated values to the drivebase - driveBase.tankDrive(leftSpeed, rightSpeed, false); - - } + // 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); } /** @@ -326,13 +190,6 @@ else if (robotMovingBackWards && currentRightDistance <= desiredRightDistance) { @Override public boolean isFinished() { - return commandFinished; } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - } - } From b34f7b31c2e3cf9b3509f7f89523dcb7e3ab59ba Mon Sep 17 00:00:00 2001 From: Ishan Date: Sat, 7 Mar 2020 14:30:21 -0500 Subject: [PATCH 08/10] Quick fix on import statements --- src/main/java/frc/robot/commands/DriveWithDistance.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/DriveWithDistance.java b/src/main/java/frc/robot/commands/DriveWithDistance.java index 1cf0012..dbc88d5 100644 --- a/src/main/java/frc/robot/commands/DriveWithDistance.java +++ b/src/main/java/frc/robot/commands/DriveWithDistance.java @@ -7,11 +7,12 @@ 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 From 4c4928d6941c15106c4aff9bd0901158c29da260 Mon Sep 17 00:00:00 2001 From: IshanGitHub <43831507+IshanGitHub@users.noreply.github.com> Date: Sun, 8 Mar 2020 13:52:04 -0400 Subject: [PATCH 09/10] Quick fix for typo --- src/main/java/frc/robot/commands/DriveWithDistance.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/DriveWithDistance.java b/src/main/java/frc/robot/commands/DriveWithDistance.java index dbc88d5..604b444 100644 --- a/src/main/java/frc/robot/commands/DriveWithDistance.java +++ b/src/main/java/frc/robot/commands/DriveWithDistance.java @@ -156,7 +156,7 @@ public void execute() { SmartDashboard.putNumber("Left Error", desiredLeftDistance - currentLeftDistance); SmartDashboard.putNumber("Right Error", desiredRightDistance - currentRightDistance); - //Code for figuring out when the robot is finished driving and handeling user input validation + //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(); From 3bffc9a38c7ed5fa7e2c69bec99fd183cf2586bc Mon Sep 17 00:00:00 2001 From: Ishan Date: Wed, 11 Mar 2020 18:37:57 -0400 Subject: [PATCH 10/10] Quick fix in syntax --- src/main/java/frc/robot/RobotContainer.java | 4 +++- src/main/java/frc/robot/config/Config.java | 2 -- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f12c313..9bcef90 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -129,7 +129,7 @@ public Command getAutonomousCommand() { //Robot has only one selector int selectorOne = 0; - if (analogSelectorOne != null) + if (analogSelectorOne != null){ selectorOne = analogSelectorOne.getIndex(); } @@ -156,6 +156,8 @@ public Command getAutonomousCommand() { // 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/config/Config.java b/src/main/java/frc/robot/config/Config.java index d628945..74a7737 100644 --- a/src/main/java/frc/robot/config/Config.java +++ b/src/main/java/frc/robot/config/Config.java @@ -141,8 +141,6 @@ private Config() { 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; - - public static final int RPM = 1200; // Define a global constants table for subsystems to use public static NetworkTable constantsTable = NetworkTableInstance.getDefault().getTable("constants");