// Turns according to the parameter dir, which should be 'L'
// (left), 'R' (right), 'S' (straight), or 'B' (back). We turn
// most of the way using the gyro, and then use one of the line
// sensors to finish the turn. We use the inner line sensor that
// is closer to the target line in order to reduce overshoot.
void turn(char dir)
{
if (dir == 'S')
{
// Don't do anything!
return;
}
turnSensorReset();
uint8_t sensorIndex;
switch(dir)
{
case 'B':
// Turn left 125 degrees using the gyro.
motors.setSpeeds(-turnSpeed, turnSpeed);
while((int32_t)turnAngle < turnAngle45 * 3)
{
turnSensorUpdate();
}
sensorIndex = 1;
break;
case 'L':
// Turn left 45 degrees using the gyro.
motors.setSpeeds(-turnSpeed, turnSpeed);
while((int32_t)turnAngle < turnAngle45)
{
turnSensorUpdate();
}
sensorIndex = 1;
break;
case 'R':
// Turn right 45 degrees using the gyro.
motors.setSpeeds(turnSpeed, -turnSpeed);
while((int32_t)turnAngle > -turnAngle45)
{
turnSensorUpdate();
}
sensorIndex = 3;
break;
default:
// This should not happen.
return;
}
// Turn the rest of the way using the line sensors.
while(1)
{
readSensors();
if (aboveLine(sensorIndex))
{
// We found the line again, so the turn is done.
break;
}
}
}
// Calibrates the line sensors by turning left and right, then
// displays a bar graph of calibrated sensor readings on the LCD.
// Returns after the user presses A.
static void lineSensorSetup()
{
lcd.clear();
lcd.print(F("Line cal"));
// Delay so the robot does not move while the user is still
// touching the button.
delay(1000);
// We use the gyro to turn so that we don't turn more than
// necessary, and so that if there are issues with the gyro
// then you will know before actually starting the robot.
turnSensorReset();
// Turn to the left 90 degrees.
motors.setSpeeds(-calibrationSpeed, calibrationSpeed);
while((int32_t)turnAngle < turnAngle45 * 2)
{
lineSensors.calibrate();
turnSensorUpdate();
}
// Turn to the right 90 degrees.
motors.setSpeeds(calibrationSpeed, -calibrationSpeed);
while((int32_t)turnAngle > -turnAngle45 * 2)
{
lineSensors.calibrate();
turnSensorUpdate();
}
// Turn back to center using the gyro.
motors.setSpeeds(-calibrationSpeed, calibrationSpeed);
while((int32_t)turnAngle < 0)
{
lineSensors.calibrate();
turnSensorUpdate();
}
// Stop the motors.
motors.setSpeeds(0, 0);
// Show the line sensor readings on the LCD until button A is
// pressed.
lcd.clear();
while(!buttonA.getSingleDebouncedPress())
{
readSensors();
lcd.gotoXY(0, 0);
for (uint8_t i = 0; i < numSensors; i++)
{
uint8_t barHeight = map(lineSensorValues[i], 0, 1000, 0, 8);
printBar(barHeight);
}
}
lcd.clear();
}
/* This should be called in setup() to enable and calibrate the
gyro. It uses the LCD, yellow LED, and button A. While the LCD
is displaying "Gyro cal", you should be careful to hold the robot
still.
The digital zero-rate level of the L3GD20H gyro can be as high as
25 degrees per second, and this calibration helps us correct for
that. */
void turnSensorSetup()
{
Wire.begin();
gyro.init();
// 800 Hz output data rate,
// low-pass filter cutoff 100 Hz
gyro.writeReg(L3G::CTRL1, 0b11111111);
// 2000 dps full scale
gyro.writeReg(L3G::CTRL4, 0b00100000);
// High-pass filter disabled
gyro.writeReg(L3G::CTRL5, 0b00000000);
// Delay to give the user time to remove their finger.
delay(500);
// Calibrate the gyro.
int32_t total = 0;
for (uint16_t i = 0; i < 1024; i++)
{
// Wait for new data to be available, then read it.
while(!gyro.readReg(L3G::STATUS_REG) & 0x08);
gyro.read();
// Add the Z axis reading to the total.
total += gyro.g.z;
}
gyroOffset = total / 1024;
// Display the angle (in degrees from -180 to 180) until the
// user presses A.
turnSensorReset();
turnSensorUpdate();
}
