task main()
{
//Create and configure struct for the compass:
// Create struct to hold sensor data:
tHTMC compass;
// Initialise and configure struct and port:
initSensor(&compass, S4);
//Define destination coordinates:
long destLat = 0; // Add the latitude destination here.
long destLong = 0; // Add the longitude destination here.
long GPSrelHeading;
long distance;
DGPSsetDestination(DGPS, destLat, destLong);
while (DGPSreadDistToDestination(DGPS) > 0) //Travelling to destination
{
GPSrelHeading = DGPSreadRelHeading(DGPS);
distance = DGPSreadDistToDestination(DGPS);
// Here, the relative heading of the GPS is set as the target heading of the compass;
// a compass.relativeHeading of 0 means the destination is straight ahead, > 0 heading means it
// is to the left, and < 0 means the dest is to the right.
readSensor(&compass);
compass.offset = GPSrelHeading;
displayTextLine(3, "GPSRelHead: %d", GPSrelHeading);
readSensor(&compass);
displayTextLine(5, "CompRelHead: %d", compass.relativeHeading);
displayTextLine(7, "Distance: %d", distance);
setMotorSpeed(LEFT, 75);
setMotorSpeed(RIGHT, 75);
readSensor(&compass);
if (compass.relativeHeading > 5) //If car is right of dest., turn left
{
steerLeft();
}
else if (compass.relativeHeading < -5) //If car is left of dest., turn right
{
steerRight();
}
else //dest. is forward
{
steerRecenter();
}
sleep(1000);
}
//Destination reached
setMotorSpeed(LEFT, 0);
setMotorSpeed(RIGHT, 0);
steerRecenter();
}
int moveToColor() {
int r,g,b,colorSeen;
while(true)
{
if (isSensorSeesLight()){
steerLeft();
}
else {
steerRight();
}
getColorRGB(colorSensor,r,g,b);
colorSeen = detectColor(r,g,b);
displayTextLine(0,"color seen : %d -> %d %d %d", colorSeen,r,g,b);
if ( colorSeen != bw ) return colorSeen;
}
}