void FollowSegment()
{
int steering;
T("enter FollowSegment");
ClearTimer(T1);
long lastTime = time1[T1];
while (true) {
int timeNow = time1[T1];
int elapsedTime = timeNow - lastTime;
lastTime = timeNow;
int llAvg = LLreadAverage(LINELEADER);
unsigned byte llResult = LLreadResult(LINELEADER);
if (llAvg != 0)
{
steering = DoPID(llAvg, 45, Kp, Ki, Kd, elapsedTime); // 45 is our goal, set point
//steering = llAvg;
motor[LEFT] = clip((basePower + steering), Min, Max);
motor[RIGHT] = clip((basePower - steering), Min, Max);
// data logging
int heading = HTMCreadHeading(COMPASS);
WriteShort(hFileHandle, nIoResult, timeNow);
WriteShort(hFileHandle, nIoResult, llAvg);
WriteShort(hFileHandle, nIoResult, steering);
WriteFloat(hFileHandle, nIoResult, integral);
WriteFloat(hFileHandle, nIoResult, derivative);
WriteShort(hFileHandle, nIoResult, elapsedTime);
WriteShort(hFileHandle, nIoResult, heading);
WriteByte(hFileHandle, nIoResult, llResult);
}
else
{
// stuff gone wrong, we lost the line
// this may be where we check result for an intersection
PlaySound(soundException);
}
if (TSreadState(BUMPER))
break;
lastTime = timeNow; // using the variable, not the actual clock
//wait1Msec(10); // some throttling
}
T("leave FollowSegment");
}
task main () {
int _target = 0;
eraseDisplay();
nxtDisplayCenteredTextLine(0, "HiTechnic");
nxtDisplayCenteredBigTextLine(1, "Compass");
nxtDisplayCenteredTextLine(3, "SMUX Test");
nxtDisplayCenteredTextLine(5, "Connect SMUX to");
nxtDisplayCenteredTextLine(6, "S1 and sensor to");
nxtDisplayCenteredTextLine(7, "SMUX Port 1");
wait1Msec(2000);
eraseDisplay();
time1[T1] = 0;
while(true) {
// Reset the target no more than once a second
// This also helps with debouncing the [enter] button.
if (time1[T1] > 1000) {
eraseDisplay();
nxtDisplayTextLine(1, "Changing");
nxtDisplayTextLine(2, "target");
wait1Msec(500);
// Set the current heading as the value for the offset to be used as the
// new zero-point for the relative heading returned by
// HTMCreadRelativeHeading()
_target = HTMCsetTarget(HTCOMPASS);
PlaySound(soundBlip);
while(bSoundActive) EndTimeSlice();
time1[T1] = 0;
}
// Get the true heading and relative heading from the sensor and
// display them on the screen.
while(nNxtButtonPressed != kEnterButton) {
eraseDisplay();
nxtDisplayTextLine(1, "Reading");
nxtDisplayTextLine(2, "Target: %4d", _target);
nxtDisplayTextLine(4, "Abs: %4d", HTMCreadHeading(HTCOMPASS));
nxtDisplayTextLine(5, "Rel: %4d", HTMCreadRelativeHeading(HTCOMPASS));
nxtDisplayTextLine(6, "Press enter");
nxtDisplayTextLine(7, "to set target");
wait1Msec(100);
}
}
}
task main()
{
while(true){
nxtDisplayCenteredTextLine(4, "Heading: %d", HTMCreadHeading(compass));
}
}
task main () {
int _target = 0;
eraseDisplay();
nxtDisplayCenteredTextLine(0, "HiTechnic");
nxtDisplayCenteredBigTextLine(1, "Compass");
nxtDisplayCenteredTextLine(3, "SMUX Test");
nxtDisplayCenteredTextLine(5, "Connect SMUX to");
nxtDisplayCenteredTextLine(6, "S1 and sensor to");
nxtDisplayCenteredTextLine(7, "SMUX Port 1");
wait1Msec(2000);
nxtDisplayCenteredTextLine(5, "Press enter");
nxtDisplayCenteredTextLine(6, "to set target");
nxtDisplayCenteredTextLine(7, " ");
wait1Msec(2000);
eraseDisplay();
// Before using the SMUX, you need to initialise the driver
HTSMUXinit();
// Tell the SMUX to scan its ports for connected sensors
HTSMUXscanPorts(HTSMUX);
// 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
wait1Msec(2000);
eraseDisplay();
time1[T1] = 0;
while(true) {
// Reset the target no more than once a second
// This also helps with debouncing the [enter] button.
if (time1[T1] > 1000) {
eraseDisplay();
nxtDisplayTextLine(1, "Changing");
nxtDisplayTextLine(2, "target");
wait1Msec(500);
// Set the current heading as the value for the offset to be used as the
// new zero-point for the relative heading returned by
// HTMCreadRelativeHeading()
_target = HTMCsetTarget(msensor_S1_1);
PlaySound(soundBlip);
while(bSoundActive);
time1[T1] = 0;
}
// Get the true heading and relative heading from the sensor and
// display them on the screen.
while(nNxtButtonPressed != kEnterButton) {
eraseDisplay();
nxtDisplayTextLine(1, "Reading");
nxtDisplayTextLine(2, "Target: %4d", _target);
nxtDisplayTextLine(4, "Abs: %4d", HTMCreadHeading(msensor_S1_1));
nxtDisplayTextLine(5, "Rel: %4d", HTMCreadRelativeHeading(msensor_S1_1));
nxtDisplayTextLine(6, "Press enter");
nxtDisplayTextLine(7, "to set target");
wait1Msec(100);
}
}
}
int getHeading() {
return HTMCreadHeading(COMPASS);
}
float readCompass()
{
return result[normalizeTheta360(HTMCreadHeading(HTcompass)-baseHeading)];
}
