// the loop routine runs over and over again forever:
void loop()
{
wprintf(L"Calibration in 3 seconds\n");
delay(3000);
wprintf(L"Calibrating...");
gyroscope.calibrate();
wprintf(L"Cycle tyme in microseconds: %ld \n", TICK_LENGTH_MICROSECONDS);
// main cycle
delay(100);
unsigned long lastIteration = micros();
unsigned long nextIteration = lastIteration + TICK_LENGTH_MICROSECONDS;
int n = 0;
do {
float actualAngle, deltaAngle;
while (micros() < nextIteration)
{
// do nothing
}
unsigned long now = micros();
gyroscope.getAngleFiltered(actualAngle, deltaAngle, now);
targetAngle = angleRegulator.getResult(speedIntegrator.getSum(), 0, now - lastIteration);
float motorSpeed = 0;
motorSpeed = speedRegulator.getResult(actualAngle - targetAngle, deltaAngle, now - lastIteration);
n++;
if (n % 20 == 0) // Print on every 20th iteration
{
wprintf(L"%d\t%f\t%f\t%f\t%f\n", n, targetAngle, actualAngle, deltaAngle, motorSpeed);
}
motors.setSpeedBoth(motorSpeed);
speedIntegrator.pushValue(motorSpeed);
lastIteration = now;
nextIteration = now + TICK_LENGTH_MICROSECONDS;
keyboard.processEvents();
if (keyboard.shouldExit())
{
break;
}
} while (true);
motors.stopAll();
wprintf(L"Exiting...\n");
exit(0);
}
void turn(int target, int speed) {
GYRO.calibrate(10);
float zero = gyro(4);
float value;
MOTORS.setMotorSpeed(speed, -speed);
do {
value = abs(zero - gyro(4));
} while (value < target);
MOTORS.setMotorSpeed(0, 0);
}
