void setDrive (float left, float right){
LBM.move_velocity(left);
LFM.move_velocity(left);
RBM.move_velocity(right);
RFM.move_velocity(right);
}
void turn (double target){
double currentAngle=gyro.get_rotation();
double power, threshold = 1.2;
double kP = .835;
while (fabs(currentAngle) > threshold ){
currentAngle = gyro.get_rotation(); //ditto
power = kP*(target-currentAngle); //error*constant
if (currentAngle > target) {
setDrive(power,-power); //turn right
}else{
setDrive(-power,power); //ditto
}
//debug
printf("Target (%f)", target);
printf("CURRENTANGLE (%f)", currentAngle);
printf("Power (%f)", power);
pros::delay(5);
}
setDrive(0,0);
}
Here, I simplified your code and made it easier for you to debug. Notice how I eliminated redundancies –– you’ll want to do that so you can catch your mistakes more easily. You also might want to include comments.