private double driveI;
private double driveD;
+ public DriveDistance(double distance, double maxTimeOut) {
+ requires(driveTrain);
+ this.maxTimeOut = maxTimeOut;
+ this.target = distance;
+
+ private double target;
+ private double gyroP;
+ private double gyroI;
+ private double gyroD;
+
+ private double driveP;
+ private double driveI;
+ private double driveD;
+
public DriveDistance(double distance, double motorVal) {
requires(driveTrain);
this.driveP = driveTrain.driveP;
xVal = -this.gyroController
.calcPID(this.driveTrain.getAngle() - this.driveTrain.getZeroAngle());
}
+ double leftDrive = yVal - xVal;
+ double rightDrive = yVal + xVal;
+
+ this.driveTrain.setMotorValues(leftDrive, rightDrive);
+
+ driveTrain.printEncoderOutput();
// System.out.println("turn: " + xVal);
double leftDrive = MathLib.calcLeftTankDrive(-xVal, yVal);
double rightDrive = MathLib.calcRightTankDrive(xVal, -yVal);