task initialize()
{
servo[backboard]=backboardDown;
servo[grabber]=grabberUp;
HTGYROsetCal(gyro, 0);
}
void initializeRobot()
{
CloseAllHandles(nIoResult); // make sure everything is closed
wait1Msec(100);
OpenRead(hFileHandle, nIoResult, sFileName , nFileSize); // open the existing file
ReadFloat(hFileHandle, nIoResult, drift); // read a single float value from it
ReadFloat(hFileHandle, nIoResult, Driver_Cal); // read a single float value from it
Close(hFileHandle, nIoResult); // and close the file
HTGYROsetCal(HTGYRO, Driver_Cal); // force the GYRO driver calibration to the value from Autonomous
disableDiagnosticsDisplay();
StartTask(display);
StartTask(buttons);
StartTask(gyro);
StartTask(Lifter);
StartTask(lightFun);
//StartTask(sensors);
StartTask(grabbers);
RequestedScreen=S_GYRO; // default the screen display to the GYRO information
calibrate=1;
grabbers_state = grabbersMid;
servo[shoulder] = SHOULDER_DOWN;
getJoystickSettings(joystick);
motor[lightMotor] = 50; // reset light sensor into UP position for safety
wait1Msec(500);
motor[lightMotor] = 0;
wait1Msec(400);
return;
}
task gyro() {
GYRO_READY = false;
GYRO_ANGLE = 0.0;
// Calibrate -- assume we are stationary
float cal = 0.0;
HTGYROsetCal(gyroSensor, 0);
for (int i = 0; i < GYRO_CAL_SAMPLES; i++) {
cal += HTGYROreadRot(gyroSensor);
wait1Msec(5);
}
HTGYROsetCal(gyroSensor, cal / (float)GYRO_CAL_SAMPLES);
// Loop indefinately
time1[T4] = 0;
while (true) {
// Wait in small slices, giving up the CPU
while (time1[T4] < GYRO_PERIOD) {
abortTimeslice();
}
// Immediately reset the timer in case we get de-scheduled
time1[T4] = 0;
// Read and integrate
float speed = readGyroSpeed();
if (abs(speed) < GYRO_FLOAT_SPEED) {
speed = 0.0;
}
GYRO_ANGLE += speed * ((float)GYRO_PERIOD / 1000.0);
GYRO_READY = true;
// Surrender time to other tasks
EndTimeSlice();
}
}
