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();
}
示例#2
0
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;
  }
}