public void autoDrive() {
double speed = 0;
if (distPID.isEnable()) {
speed = distPID.get();
}
double turn = 0;
if (turnPID.isEnable()) {
turn = turnPID.get();
}
if (speed > 0) {
drive.drive(speed, turn);
} else {
if (turn > 0) {
// turn right
tankDrive(turn, -turn);
} else if (turn < 0) {
tankDrive(-turn, turn);
}
}
SmartDashboard.putDouble("left pwm", leftJag.get());
SmartDashboard.putDouble("right pwm", rightJag.get());
SmartDashboard.putDouble("speed", RobotMap.roundtoTwo(speed));
SmartDashboard.putDouble("turn", RobotMap.roundtoTwo(turn));
SmartDashboard.putDouble("turn error", turnPID.getError());
// System.out.println("driving " + speed + " " + turn);
}
protected void usePIDOutput(double output) {
double speed = shootingMotor.get() + output;
if (speed > .75 && this.getSetpoint() < 2600) {
speed = .7;
} else if (speed > .85 && this.getSetpoint() < 3600) {
speed = .8;
} else if (speed < 0) {
speed = 0;
}
shooterSet(speed);
}
/**
* Gets the current PWM value of the motor component.
*
* @return the current PWM value of the component
*/
public double get() {
return jag.get();
}