Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Dual pid #180

Open
wants to merge 19 commits into
base: develop
Choose a base branch
from
Open

Dual pid #180

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
19 commits
Select commit Hold shift + click to select a range
a844b9d
Merge pull request #5 from wh1ter0se/develop
brogan20 Jan 20, 2018
3577abc
Remove I2C code
brogan20 Jan 20, 2018
57261f0
THE ARRAYS, MASON, WHAT DO THEY EVEN MEAN?!?!
wh1ter0se Mar 12, 2018
6d27202
why does he dophin dive at the end
wh1ter0se Mar 13, 2018
dadddb2
AHHHHHHHH HOW DO I GET RID OF HICCUPS
wh1ter0se Mar 13, 2018
bc06954
All the autonomous
wh1ter0se Mar 14, 2018
cbc6dda
A bit more of all the autonomous
wh1ter0se Mar 15, 2018
8fa262c
Merge branch 'dualPID' into dualPID
brogan20 Mar 15, 2018
ad1b6c7
Merge pull request #45 from wh1ter0se/dualPID
brogan20 Mar 15, 2018
6916821
Report error on PID for each side
brogan20 Mar 15, 2018
0601275
comp day 1
wh1ter0se Mar 16, 2018
a0e636e
Merge branch 'dualPID' of https://github.com/wh1ter0se/PowerUp-2018 i…
wh1ter0se Mar 16, 2018
b3b7183
Merge pull request #46 from wh1ter0se/dualPID
brogan20 Mar 16, 2018
535eb3d
Master time error
brogan20 Mar 16, 2018
fb29a4c
Add master timeouts
brogan20 Mar 16, 2018
ca2d6c7
Command grow fixes
brogan20 Mar 16, 2018
ddd6f63
Pre-takeover.
wh1ter0se Mar 16, 2018
1839c7a
THIS IS SAFE I PROMISE
wh1ter0se Mar 16, 2018
e13a51a
Merge branch 'dualPID' into dualPID
brogan20 Mar 17, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Binary file modified build/org/usfirst/frc/team3695/robot/Constants$AutonomousConstants.class
View file
Open in desktop
Binary file not shown.
Binary file modified build/org/usfirst/frc/team3695/robot/Constants$TEUFELSKIND.class
View file
Open in desktop
Binary file not shown.
Binary file modified build/org/usfirst/frc/team3695/robot/Constants.class
View file
Open in desktop
Binary file not shown.
Binary file modified build/org/usfirst/frc/team3695/robot/OI.class
View file
Open in desktop
Binary file not shown.
Binary file modified build/org/usfirst/frc/team3695/robot/Robot.class
View file
Open in desktop
Binary file not shown.
Binary file modified build/org/usfirst/frc/team3695/robot/auto/CommandGroupAuto$1.class
View file
Open in desktop
Binary file not shown.
Binary file modified build/org/usfirst/frc/team3695/robot/auto/CommandGroupAuto.class
View file
Open in desktop
Binary file not shown.
Binary file modified build/org/usfirst/frc/team3695/robot/commands/CyborgCommandDriveDirect.class
View file
Open in desktop
Binary file not shown.
Binary file modified build/org/usfirst/frc/team3695/robot/commands/CyborgCommandDriveDistance.class
View file
Open in desktop
Binary file not shown.
Binary file modified build/org/usfirst/frc/team3695/robot/commands/CyborgCommandDriveUntilError.class
View file
Open in desktop
Binary file not shown.
Binary file modified build/org/usfirst/frc/team3695/robot/commands/CyborgCommandGrow.class
View file
Open in desktop
Binary file not shown.
Binary file modified build/org/usfirst/frc/team3695/robot/commands/CyborgCommandRotateDegrees.class
View file
Open in desktop
Binary file not shown.
Binary file modified build/org/usfirst/frc/team3695/robot/commands/ManualCommandDrive.class
View file
Open in desktop
Binary file not shown.
Binary file modified build/org/usfirst/frc/team3695/robot/commands/ManualCommandGrow.class
View file
Open in desktop
Binary file not shown.
Binary file modified build/org/usfirst/frc/team3695/robot/commands/ToggleCommandKillPID.class
View file
Open in desktop
Binary file not shown.
Binary file modified build/org/usfirst/frc/team3695/robot/subsystems/SubsystemClamp.class
View file
Open in desktop
Binary file not shown.
Binary file removed build/org/usfirst/frc/team3695/robot/subsystems/SubsystemDrive$1.class
View file
Open in desktop
Binary file not shown.
Binary file removed build/org/usfirst/frc/team3695/robot/subsystems/SubsystemDrive$PID.class
View file
Open in desktop
Binary file not shown.
Binary file added build/org/usfirst/frc/team3695/robot/subsystems/SubsystemDrive$PIDF.class
View file
Open in desktop
Binary file not shown.
Binary file modified build/org/usfirst/frc/team3695/robot/subsystems/SubsystemDrive.class
View file
Open in desktop
Binary file not shown.
Binary file modified build/org/usfirst/frc/team3695/robot/subsystems/SubsystemManipulator.class
View file
Open in desktop
Binary file not shown.
Binary file modified build/org/usfirst/frc/team3695/robot/subsystems/SubsystemMast.class
View file
Open in desktop
Binary file not shown.
Binary file modified dist/FRCUserProgram.jar
View file
Open in desktop
Binary file not shown.
20 changes: 10 additions & 10 deletions src/org/usfirst/frc/team3695/robot/Constants.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,9 +38,9 @@ public class Constants {
RIGHT_PINION_MOTOR = 4,
SCREW_MOTOR = 5,
/// PID
RIGHT_PID = 0,
PIDF_LOOP_ID = 0,
LEFT_PID = 0,
TIMEOUT_PID = 10000;
PIDF_TIMEOUT = 10000;

public static final int
/// I2C
Expand All @@ -53,8 +53,8 @@ public class Constants {

public static final int
/// MANIPULATOR
OPEN_ARMS = 5,
CLOSE_ARMS = 4,
OPEN_ARMS = 4,
CLOSE_ARMS = 5,
/// CANDY CANE
RAISE_HOOK = 6,
LOWER_HOOK = 7;
Expand Down Expand Up @@ -89,8 +89,8 @@ public static class OOF {
public static class TEUFELSKIND {
public static final boolean

LEFT_PINION_MOTOR_INVERT = false,
RIGHT_PINION_MOTOR_INVERT = true,
LEFT_PINION_MOTOR_INVERT = true,
RIGHT_PINION_MOTOR_INVERT = false,
SCREW_MOTOR_INVERT = false,

LEFT_ARM_MOTOR_INVERT = true,
Expand Down Expand Up @@ -118,9 +118,9 @@ public static class AutonomousConstants {
//Save distances in inches

//Distance from the side positions to the middle of the switch on the side
public static final int DIST_TO_SWITCH = 148;
public static final int DIST_TO_SWITCH = 150+5;
//Distance rom the side positions to the middle of the scale on the side
public static final int DIST_TO_SCALE = 324;
public static final int DIST_TO_SCALE = (260);
//Distance from a side to the enemy switch
public static final int DIST_TO_ENEMY_SWITCH = 480;

Expand All @@ -137,9 +137,9 @@ public static class AutonomousConstants {
public static final int DIST_ALLIANCE_WALL_TO_BLOCKS = 98;

// the distance to pass the switch before traveling to the foreign home switch
public static final double DIST_PAST_SWITCH = 238.735;
public static final double DIST_PAST_SWITCH = 220.735+2;

public static final double DIST_TO_FOREIGN_SWITCH = 178.75;
public static final double DIST_TO_FOREIGN_SWITCH = 163.75+5;

public static final double DIST_PAST_SCALE = 240;
public static final double DIST_SCALE_LINEUP = 96;
Expand Down
2 changes: 1 addition & 1 deletion src/org/usfirst/frc/team3695/robot/OI.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,7 @@ public OI() {
/// Cyborg command testers
SmartDashboard.putData("Drive Direct", new CyborgCommandDriveDirect(Util.getAndSetDouble("Drive Direct Power", 0)));
SmartDashboard.putData("Drive Distance", new CyborgCommandDriveDistance(Util.getAndSetDouble("Drive Distance Inches", 0), (int) Util.getAndSetDouble("Drive Distance Timeout", 5000)));
SmartDashboard.putData("Drive Until Error", new CyborgCommandDriveUntilError(500,2));
SmartDashboard.putData("Drive Until Error", new CyborgCommandDriveUntilError(500,2,.25, 3));
SmartDashboard.putData("Rotate Degree", new CyborgCommandRotateDegrees(Util.getAndSetDouble("Rotate Degrees", 0), (int) Util.getAndSetDouble("Rotate Timeout", 5000)));
}

Expand Down
8 changes: 3 additions & 5 deletions src/org/usfirst/frc/team3695/robot/Robot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,3 @@

package org.usfirst.frc.team3695.robot;

import edu.wpi.first.wpilibj.DriverStation;
Expand Down Expand Up @@ -72,7 +71,7 @@ public void robotInit() {
SUB_CLAMP = new SubsystemClamp();
SUB_COMPRESSOR = new SubsystemCompressor();
SUB_DRIVE = new SubsystemDrive();

SUB_DRIVE.setInverts();
SUB_HOOK = new SubsystemHook();
SUB_MAST = new SubsystemMast();
vision = new Vision();
Expand Down Expand Up @@ -106,8 +105,8 @@ public void robotInit() {
/// instantiate third priority chooser
thirdPriorityChooser = new SendableChooser<>();
thirdPriorityChooser.addDefault(Goal.SWITCH.toString(), Goal.SWITCH);
goalChooser.addObject(Goal.SCALE.toString(), Goal.SCALE);
goalChooser.addObject(Goal.ENEMY_SWITCH.toString(), Goal.ENEMY_SWITCH);
thirdPriorityChooser.addObject(Goal.SCALE.toString(), Goal.SCALE);
thirdPriorityChooser.addObject(Goal.ENEMY_SWITCH.toString(), Goal.ENEMY_SWITCH);
SmartDashboard.putData("Third Priority", thirdPriorityChooser);

/// instantiate bot chooser
Expand Down Expand Up @@ -187,4 +186,3 @@ public void testPeriodic() {
LiveWindow.run();
}
}

101 changes: 53 additions & 48 deletions src/org/usfirst/frc/team3695/robot/auto/CommandGroupAuto.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,6 @@ public CommandGroupAuto(Position position, Goal goal, Goal thirdPriority) {
//Get the state of the switches and scale for each round
gameData = DriverStation.getInstance().getGameSpecificMessage();

// make sure everything is in the right state/position up here
Robot.SUB_CLAMP.closeArms();
Robot.SUB_DRIVE.setInverts();

Expand Down Expand Up @@ -99,89 +98,95 @@ public CommandGroupAuto(Position position, Goal goal, Goal thirdPriority) {
}

private void runForIt() {
addSequential(new CyborgCommandDriveUntilError(500, 2));
addSequential(new CyborgCommandDriveDistance(160, 5000));
}

private void leftSwitch() {
if (gameData.charAt(0) == 'L') { //When the switch is on the left
addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_TO_SWITCH, 3500));
addSequential(new CyborgCommandGrow(Mast.SCREW_UP, 1500));
addSequential(new CommandWait(250));
addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CW, 1250));
addParallel(new CyborgCommandGrow(Mast.SCREW_UP, 2000));
addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_TO_SWITCH, 5000));
// addSequential(new CyborgCommandDriveDistance(46, 2000));
addSequential(new CommandWait(350));
addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CW, 8500));
addSequential(new CommandWait(250));
addSequential(new CyborgCommandDriveUntilError(500, 2));
addSequential(new CyborgCommandDriveUntilError(500, 2, 0.5, 3));
addSequential(new CyborgCommandSpit(500));
} else if (gameData.charAt(0) == 'R') { //When the switch is on the right
//CHECK TUNING!!!!!!!
addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_PAST_SWITCH, 4000));
addSequential(new CommandWait(250));
addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CW, 1250));
addSequential(new CommandWait(350));
addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CW, 5000));
addSequential(new CommandWait(250));
addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_TO_FOREIGN_SWITCH, 4000));
addSequential(new CyborgCommandGrow(Mast.SCREW_UP, 1500));
addSequential(new CommandWait(250));
addSequential(new CommandWait(350));
addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CW, 1250));
addSequential(new CommandWait(250));
addSequential(new CyborgCommandDriveUntilError(500, 2));
addSequential(new CyborgCommandDriveUntilError(500, 2, 0.5,3));
addSequential(new CyborgCommandSpit(500));
}
}
}
// TODO right switch terminates early
private void rightSwitch() {
if (gameData.charAt(0) == 'R') { //When the switch is on the right
addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_TO_SWITCH, 3500));
addSequential(new CyborgCommandGrow(Mast.SCREW_UP, 1500));
addSequential(new CommandWait(250));
addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CCW, 1250), 1500);
if (gameData.charAt(0) == 'R') { //When the switch is on the left
addParallel(new CyborgCommandGrow(Mast.SCREW_UP, 2000));
addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_TO_SWITCH, 5000));
// addSequential(new CyborgCommandDriveDistance(46, 2000));
addSequential(new CommandWait(500));
addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CCW, 8500));
addSequential(new CommandWait(250));
addSequential(new CyborgCommandDriveUntilError(500, 2));
addSequential(new CyborgCommandSpit(500L));
} else { //When the switch is on the left
addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_PAST_SWITCH, 4000));
addSequential(new CommandWait(250));
addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CCW, 1250));
addSequential(new CommandWait(250));
addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_TO_FOREIGN_SWITCH, 4000));
addSequential(new CyborgCommandGrow(Mast.SCREW_UP, 1500));
addSequential(new CommandWait(250));
addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CCW, 1250));
addSequential(new CommandWait(250));
addSequential(new CyborgCommandDriveUntilError(500, 2));
addSequential(new CyborgCommandDriveUntilError(500, 2, 0.5, 3));
addSequential(new CyborgCommandSpit(500));
}
else {
addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_TO_SWITCH, 5000));
// addSequential(new CyborgCommandDriveDistance(46, 2000));
}
// else if (gameData.charAt(0) == 'L') { //When the switch is on the right
// //CHECK TUNING!!!!!!!
// addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_PAST_SWITCH, 4000));
// addSequential(new CommandWait(500));
// addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CCW, 5000));
// addSequential(new CommandWait(250));
// addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_TO_FOREIGN_SWITCH, 4000));
// addSequential(new CyborgCommandGrow(Mast.SCREW_UP, 1500));
// addSequential(new CommandWait(350));
// addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CCW, 1250));
// addSequential(new CommandWait(250));
// addSequential(new CyborgCommandDriveUntilError(500, 2, 0.5, 4));
// addSequential(new CyborgCommandSpit(500));
// }
}

private void centerSwitch() {
addParallel(new CyborgCommandGrow(Mast.SCREW_UP, 1500));
addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_PASS_PORTAL, 5000));
if (gameData.charAt(0) == 'L') { //When the switch is on the left
addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CCW, 5000));
addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_CENTER_LINE_SWITCH_ALIGN, 5000));
addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CW, 5000));
addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_ALLIANCE_WALL_TO_BLOCKS
+ AutonomousConstants.DIST_BLOCKS_TO_SWITCH
- AutonomousConstants.DIST_PASS_PORTAL, 5000));

} else if (gameData.charAt(0) == 'L') { //When the switch is on the right
addSequential(new CyborgCommandDriveUntilError(500, 2, 0.5, 3));
} else if (gameData.charAt(0) == 'R') { //When the switch is on the right
addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CW, 5000));
addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_CENTER_LINE_SWITCH_ALIGN, 5000));
addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CCW, 5000));
// addParallel(new CyborgCommandGoToMid());
addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_ALLIANCE_WALL_TO_BLOCKS
+ AutonomousConstants.DIST_BLOCKS_TO_SWITCH
- AutonomousConstants.DIST_PASS_PORTAL, 5000));
addSequential(new CyborgCommandDriveUntilError(500, 2, 0.5, 3));
} else {
addSequential(new CyborgCommandDriveUntilError(500, 2));
addSequential(new CyborgCommandDriveUntilError(500, 2, 0.3, 3));
}
addSequential(new CyborgCommandSpit(500));
}

private void leftScale() {
if (gameData.charAt(1) == 'L') { //When scale is on the left
addParallel(new CyborgCommandGrow(Mast.SCREW_UP, 3000));
addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_TO_SCALE, 4000));
addSequential(new CommandWait(250));
addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_TO_SCALE, 6000));
addSequential(new CommandWait(250));
addSequential(new CyborgCommandRotateDegrees(45, 4000));
addSequential(new CyborgCommandGrow(Mast.PINION_UP, 3500));
addSequential(new CommandWait(250));
addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CW, 1250));
addSequential(new CyborgCommandDriveDistance(15, 3000));
addSequential(new CyborgCommandSpit(500));
addSequential(new CyborgCommandDriveUntilError(500, 500000, -0.15, 3));
} else { //When scale is on the right
addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_PAST_SWITCH, 4000));
addSequential(new CommandWait(250));
Expand All @@ -197,19 +202,19 @@ private void leftScale() {
addSequential(new CommandWait(250));
addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CCW, 1250));
addSequential(new CommandWait(250));
addSequential(new CyborgCommandDriveUntilError(500, 2));
addSequential(new CyborgCommandDriveUntilError(500, 2, 0.15, 3));
addSequential(new CyborgCommandSpit(500));
}
}

private void rightScale() {
if (gameData.charAt(1) == 'R') { //When scale is on the right
addParallel(new CyborgCommandGrow(Mast.SCREW_UP, 3000));
addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_TO_SCALE, 4000));
addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_TO_SCALE, 7000));
addSequential(new CommandWait(250));
addSequential(new CyborgCommandGrow(Mast.PINION_UP, 3500));
addSequential(new CommandWait(250));
addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CCW, 1250));
addSequential(new CyborgCommandRotateDegrees(45, 1250));
addSequential(new CyborgCommandSpit(500));
} else { //When scale is on the left
addSequential(new CyborgCommandDriveDistance(AutonomousConstants.DIST_PAST_SWITCH, 4000));
Expand All @@ -226,7 +231,7 @@ private void rightScale() {
addSequential(new CommandWait(250));
addSequential(new CyborgCommandRotateDegrees(AutonomousConstants.ROT_90_CW, 1250));
addSequential(new CommandWait(250));
addSequential(new CyborgCommandDriveUntilError(500, 2));
addSequential(new CyborgCommandDriveUntilError(500, 2, 0.15, 3));
addSequential(new CyborgCommandSpit(500));
}
}
Expand Down
2 changes: 1 addition & 1 deletion src/org/usfirst/frc/team3695/robot/commands/CyborgCommandDriveDirect.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@ public CyborgCommandDriveDirect(double percent) {

protected void initialize() {
DriverStation.reportWarning("DRIVING BY POWER", false);
Robot.SUB_DRIVE.pid.reset();
Robot.SUB_DRIVE.pidf.reset();
time = System.currentTimeMillis() + TIME_WAIT;
}

Expand Down
31 changes: 13 additions & 18 deletions src/org/usfirst/frc/team3695/robot/commands/CyborgCommandDriveDistance.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,14 +6,14 @@
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

import org.usfirst.frc.team3695.robot.Robot;
import org.usfirst.frc.team3695.robot.subsystems.SubsystemDrive.PID;
import org.usfirst.frc.team3695.robot.subsystems.SubsystemDrive.PIDF;
import org.usfirst.frc.team3695.robot.util.Util;

public class CyborgCommandDriveDistance extends Command {

public static final long TIME_WAIT = 3000;
public double inches;
public static final int ALLOWABLE_ERROR = 1;
public static final int ALLOWABLE_ERROR = 2;

Boolean isFinished;
long runTime;
Expand All @@ -22,46 +22,41 @@ public class CyborgCommandDriveDistance extends Command {
public CyborgCommandDriveDistance(double inches, int timeoutms) {
this.inches = inches;
requires(Robot.SUB_DRIVE);
Robot.SUB_DRIVE.pid.reset();
Robot.SUB_DRIVE.pidf.reset();
runTime = timeoutms;
}

protected void initialize() {
Robot.SUB_DRIVE.pid.reset();
Robot.SUB_DRIVE.pidf.reset();
// inches = Util.getAndSetDouble("Drive Distance Inches", 10); // take out in final version
PID.setPIDF(0,
Util.getAndSetDouble("Distance-P", .5),
Util.getAndSetDouble("Distance-I", 0),
Util.getAndSetDouble("Distance-D", 0),
Util.getAndSetDouble("Distance-F", 0));
PIDF.setPIDF(0);
Robot.SUB_DRIVE.driveDistance(inches, inches);
startTime = System.currentTimeMillis();
}

protected void execute() {

DriverStation.reportWarning("DRIVING " + inches + " INCHES", false);
SmartDashboard.putNumber("Left Encoder Inches", Robot.SUB_DRIVE.pid.getLeftInches());
SmartDashboard.putNumber("Right Encoder Inches", Robot.SUB_DRIVE.pid.getRightInches());


SmartDashboard.putNumber("Error", Robot.SUB_DRIVE.pid.getError());
SmartDashboard.putNumber("Left Encoder Inches", Robot.SUB_DRIVE.pidf.getLeftInches());
SmartDashboard.putNumber("Right Encoder Inches", Robot.SUB_DRIVE.pidf.getRightInches());
Robot.SUB_DRIVE.pidf.getError();
}

protected boolean isFinished() {
isFinished = startTime + runTime < System.currentTimeMillis();
boolean leftInRange =
Robot.SUB_DRIVE.pid.getLeftInches() > (inches) - ALLOWABLE_ERROR &&
Robot.SUB_DRIVE.pid.getLeftInches() < (inches) + ALLOWABLE_ERROR;
Robot.SUB_DRIVE.pidf.getLeftInches() > (inches) - ALLOWABLE_ERROR &&
Robot.SUB_DRIVE.pidf.getLeftInches() < (inches) + ALLOWABLE_ERROR;
boolean rightInRange =
Robot.SUB_DRIVE.pid.getRightInches() > inches - ALLOWABLE_ERROR &&
Robot.SUB_DRIVE.pid.getRightInches() < inches + ALLOWABLE_ERROR;
Robot.SUB_DRIVE.pidf.getRightInches() > inches - ALLOWABLE_ERROR &&
Robot.SUB_DRIVE.pidf.getRightInches() < inches + ALLOWABLE_ERROR;
return (leftInRange && rightInRange) || isFinished;
}

protected void end() {
DriverStation.reportWarning("CyborgCommandDriveDistance finished", false);
Robot.SUB_DRIVE.driveDirect(0, 0);
DriverStation.reportWarning("LEFT:" + Robot.SUB_DRIVE.pidf.getLeftInches() + ", " + "RIGHT:" + Robot.SUB_DRIVE.pidf.getRightInches(), false);
isFinished = false;
}

Expand Down
Loading