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); }