Try the following code changes:
To robotInit():
Code:
@Override
public void robotInit() {
gyro.reset();
// gyro.calibrate();
// Allow kP to be changed on the fly in the smart dashboard
SmartDashboard.putNumber("kP", kP);
}
To teleopPeriodic():
Code:
public void teleopPeriodic() {
double err = (kAngleSetpoint - gyro.getAngle());
double turningValue = err * SmartDashboard.getNumber("kP", kP);
double compensation = Math.copySign(turningValue, joystick.getY());
SmartDashboard.putNumber("compensation", compensation);
SmartDashboard.putNumber("err", err);
myRobot.arcadeDrive(joystick.getY(), compensation);
}
The above changes should allow you to adjust your "kP" value on the fly, show you how much error is being detected and show the corresponding compensation. If you go to edit mode on your smart dashboard you can change the err and compensation output to line plots to see it over time as you drive.
Sometimes just seeing the values being produced can lead to some insight.
Hope that helps,
Paul