void faceHeading(int heading) {
while(heading < getGyroHeading(Gyro)) {
setMotorSpeed(LeftMotor, -10);
setMotorSpeed(RightMotor, 10);
}
}
void seekBounds() {
//Seek the left side of the line.
faceHeading(0);
setMotorSpeed(LeftMotor, -10);
setMotorSpeed(RightMotor, 10);
while(avgReflectedLight(5) < threshold || getGyroHeading(Gyro) > forwardsHeading*0.90) {
}
setMotorSpeed(LeftMotor, 0);
setMotorSpeed(RightMotor, 0);
resetGyro(Gyro);
//Seek the right side of the line.
setMotorSpeed(LeftMotor, 10);
setMotorSpeed(RightMotor, -10);
delay(500);
while(avgReflectedLight(5) < threshold) {
rightBound = getGyroHeading(Gyro);
}
forwardsHeading = rightBound/2;
}
/// rotate with counterclockwise (left) as positive
void rotate(int degrees, bool reset){
if(reset)
resetGyro(gyro);
float kP = 1;
if(degrees > 0){
// counterclockwise turn
while(getGyroHeading(gyro) < degrees){
setMotorSpeed(leftMotor, -proportionalOutput(kP, degrees, getGyroDegreesFloat(gyro)));
setMotorSpeed(rightMotor, proportionalOutput(kP, degrees, getGyroDegreesFloat(gyro)));
}
} else {
while(getGyroHeading(gyro) > degrees){
setMotorSpeed(leftMotor, -proportionalOutput(kP, degrees, getGyroDegreesFloat(gyro)));
setMotorSpeed(rightMotor, proportionalOutput(kP, degrees, getGyroDegreesFloat(gyro)));
}
}
setMotorSpeed(leftMotor, 0);
setMotorSpeed(rightMotor, 0);
sleep(200);
}
