So I have some code here that should make our robot turn at a minimum of PI radians/second but I’ve never used a feedforward before and I was wondering if I’m missing something glaringly obvious:
kP = SmartDashboard.getNumber("Rotational P", Constants.r_kP);
kI = SmartDashboard.getNumber("Rotational I", Constants.r_kI);
kD = SmartDashboard.getNumber("Rotational D", Constants.r_kD);
double currentAngle = gyro.getAngle();
double finalAngle = angle + desiredAngle;
SmartDashboard.putNumber("final angle", finalAngle);
SmartDashboard.putNumber("current angle", currentAngle);
SmartDashboard.putNumber("Rotation Rate", gyro.getRate());
gyroController.setPID(kP, kI, kD);
if (finalAngle < 0) {
if (currentAngle <= finalAngle) {
drive.arcadeDrive(0, 0);
return true;
}
drive.arcadeDrive(gyroController.calculate(currentAngle, finalAngle) + feedforward.calculate(Math.PI, 2), 0);
}
if (finalAngle > 0) {
if (currentAngle >= finalAngle) {
drive.arcadeDrive(0, 0);
return true;
}
drive.arcadeDrive(gyroController.calculate(currentAngle, finalAngle) + feedforward.calculate(Math.PI, 2), 0);
}
return false;
}
also, with this setup, is the robot capped at rotating PI radians per second or will the PID controller allow for faster rotation? Thanks!