task main () {
displayTextLine(0, "HT Gyro");
displayTextLine(1, "Test 1");
displayTextLine(5, "Press enter");
displayTextLine(6, "to set relative");
displayTextLine(7, "heading");
sleep(2000);
eraseDisplay();
// Create struct to hold sensor data
tHTGYRO gyroSensor;
// Initialise and configure struct and port
initSensor(&gyroSensor, S1);
time1[T1] = 0;
while(true) {
if (time1[T1] > 1000) {
eraseDisplay();
displayTextLine(1, "Resetting");
displayTextLine(1, "offset");
sleep(500);
// Start the calibration and display the offset
sensorCalibrate(&gyroSensor);
displayTextLine(2, "Offset: %f", gyroSensor.offset);
playSound(soundBlip);
while(bSoundActive) sleep(1);
time1[T1] = 0;
}
while(!getXbuttonValue(xButtonEnter)) {
eraseDisplay();
displayTextLine(1, "Reading");
// Read the current rotational speed
readSensor(&gyroSensor);
// Read the current calibration offset and display it
displayTextLine(2, "Offset: %4f", gyroSensor.offset);
displayClearTextLine(4);
// Read the current rotational speed and display it
displayTextLine(4, "Gyro: %4f", gyroSensor.rotation);
displayTextLine(6, "Press enter");
displayTextLine(7, "to recalibrate");
sleep(100);
}
}
}
task main () {
displayCenteredTextLine(0, "HiTechnic");
displayCenteredBigTextLine(1, "MAGNETIC");
displayCenteredTextLine(3, "Field Sensor");
displayCenteredTextLine(4, "SMUX Test");
displayCenteredTextLine(5, "Connect SMUX to");
displayCenteredTextLine(6, "S1 and sensor to");
displayCenteredTextLine(7, "SMUX Port 1");
sleep(2000);
displayCenteredTextLine(5, "Press enter");
displayCenteredTextLine(6, "to set bias");
sleep(2000);
eraseDisplay();
// Create struct to hold sensor data
tHTMAG magneticSensor;
// The sensor is connected to the first port
// of the SMUX which is connected to the NXT port S1.
// To access that sensor, we must use msensor_S1_1. If the sensor
// were connected to 3rd port of the SMUX connected to the NXT port S4,
// we would use msensor_S4_3
// Initialise and configure struct and port
initSensor(&magneticSensor, msensor_S1_1);
while(true) {
eraseDisplay();
displayTextLine(1, "Resetting");
displayTextLine(2, "bias");
sleep(500);
// Start the calibration and display the offset
sensorCalibrate(&magneticSensor);
displayTextLine(2, "Bias: %4d", magneticSensor.bias);
playSound(soundBlip);
while(bSoundActive) sleep(1);
while(getXbuttonValue(xButtonAny)) sleep(1);
while(!getXbuttonValue(xButtonEnter)) {
eraseDisplay();
// Read the sensor data
readSensor(&magneticSensor);
displayTextLine(1, "Reading");
// Display the current calibration value
displayTextLine(2, "Bias: %4d", magneticSensor.bias);
displayClearTextLine(4);
// Display the current magnetic field strength
displayTextLine(4, "Mag: %4d", magneticSensor.strength);
displayTextLine(6, "Press enter");
displayTextLine(7, "to recalibrate");
sleep(100);
}
}
}
task gyro_loop () {
gyro_loop_state=INIT;
int dt=10;
unsigned long prevTime,currTime;
// Create struct to hold sensor data
tHTGYRO gyroSensor;
while(gyro_loop_state!=STOPPED) {
switch(gyro_loop_state)
{
case INIT:
// Initialise and configure struct and port
hogCPU();
initSensor(&gyroSensor, S3);
gyro_loop_state=CALIBRATION;
releaseCPU();
break;
case CALIBRATION:
sleep(1000);//let it settle down
#ifdef TESTING
eraseDisplay();
displayTextLine(1, "Resetting");
displayTextLine(2, "offset");
sleep(500);
#endif
hogCPU();
// Start the calibration
sensorCalibrate(&gyroSensor);
#ifdef TESTING
//and display the offset in testing mode
displayTextLine(2, "Offset: %f", gyroSensor.offset);
clearDebugStream();
writeDebugStreamLine("Offset: %f deadband: %f",gyroSensor.offset,gyroSensor.deadband);
playSound(soundBlip);
while(bSoundActive) sleep(1);
#endif
gHeading=0.0;
prevTime =nSysTime;
gyro_loop_state=READING;
releaseCPU();
break;
case READING:
while(gyro_loop_state==READING){
clearTimer(T2);
hogCPU();
// Read the current rotational speed
readSensor(&gyroSensor);
currTime = nSysTime;
gRot = gyroSensor.rotation;
//There is a possibility that nSysTime would reach max value of 32 bit long integer
//then wrapped around. But it would be NXT has run over 1000 hours
gHeading += gRot* (currTime-prevTime)/1000.0;
prevTime=currTime;
releaseCPU();
#ifdef TESTING
eraseDisplay();
displayTextLine(1, "Reading");
// Read the current calibration offset and display it
displayTextLine(2, "Offset: %f", gyroSensor.offset);
writeDebugStreamLine("Offset: %f deadband: %f", gyroSensor.offset, gyroSensor.deadband);
displayClearTextLine(4);
// Read the current rotational speed and display it
displayTextLine(4, "Gyro: %f", gyroSensor.rotation);
writeDebugStreamLine("Rotation: %f",gyroSensor.rotation);
displayTextLine(5, "Degrees: %f", gHeading);
writeDebugStreamLine("Heading: %f",gHeading);
displayTextLine(6, "Press enter");
displayTextLine(7, "to recalibrate");
#endif
while(time1[T2]<dt && gyro_loop_state==READING){
sleep(1);
}
}
break;
default:
//should never happen
break;
}
}
}
