follow_line.c

#pragma config(Sensor, S2,     light,          sensorLightActive)
#pragma config(Sensor, S4,     sonar,          sensorSONAR)
#pragma config(Motor,  motorB,          right,         tmotorNormal, PIDControl, encoder)
#pragma config(Motor,  motorC,          left,          tmotorNormal, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "config.h"
#include "compass.c"
#include "light_sensor.c"
#include "types.h"
#include "error.c"

#define LEFT false
#define RIGHT true

#define SONAR_THRESHOLD 20

#define FOUND_ERROR -1
#define FOUND_CAN 1
#define FOUND_NODE 2
#define FOUND_HOTEL 4
#define FOUND_LEAF 8

bool followingEdge = true;

bool direction = LEFT;
int sweeps;
void edgeFollow(bool& running){
	sweeps = 0;
	if(direction == LEFT) {
		motor[left] = -SPEED;
		motor[right] = SPEED;
	} else {
		motor[left] = SPEED;
		motor[right] = -SPEED;
	}
	bool wasDark = isDark();

	do{
		//Find border point
		if(wasDark != isDark()) {
			wasDark = isDark();
			wait10Msec(3);
			if(direction == LEFT) {
				direction = RIGHT;
				motor[left] = SPEED;
				motor[right] = 0;
			} else if( direction == RIGHT) {
				direction = LEFT;
				motor[right] = SPEED;
				motor[left] = 0;
			}
			sweeps++;
			abortTimeslice();
		}
	}while(running);
}

bool detectCan(void) {
	return SensorValue[sonar] < SONAR_THRESHOLD;
}

bool detectNode(int forwards) {
	return abs(angleDifference(currentDirection(), forwards)) > ANGLE_THRESHOLD;
}

int gatherForwardsData(void) {
  wait10Msec(100);
  PlaySound(soundBlip);
	time100[T2] = 0;
	int timeToWait = minDistance*10/(2*sweepSpeed) - 10;

	int count = 0;
	int total = 0;
	int forwards = 0;

	while(time100[T2] < timeToWait) {
		//forwards = forwards + (angleDifference(currentDirection(), forwards))/++count;
	  count+= 2;
	  total += count;
	  //This formula give more weight to recent samples
	  forwards = forwards + (count * (angleDifference(currentDirection(), forwards))/total);

	  nxtDisplayCenteredTextLine(5, "%i", forwards);
		wait10Msec(5);
	}
	PlaySound(soundBlip);
	PlaySound(soundBlip);
	return forwards;
}

task FollowEdge(){
  if(!isDark()){
    panic("Not on an edge!");
    wait10Msec(1000);
    StopAllTasks();
  }
	followingEdge = true;
	edgeFollow(followingEdge);
}

bool checkIsLeaf(void) {
	int direction = currentDirection();
	while(!isDark()) {
		if( abs(angleDifference(currentDirection(), direction)) > 170) {
			return true;
		}
	}
	return false;
}

void spinInDirection(void) {
  if(direction == LEFT) {
		motor[left] = -SPIN_SPEED;
		motor[right] = SPIN_SPEED;
	} else {
		motor[left] = SPIN_SPEED;
		motor[right] = -SPIN_SPEED;
	}
}


void turnToAngle(int angle, int undershoot) {
  if( angleDifference(angle, currentDirection()) < 0 ) {
      direction = LEFT;
  } else {
    direction = RIGHT;
  }
  spinInDirection();

	while(abs(angleDifference(currentDirection(), angle)) > undershoot){
		wait1Msec(1);
	}
	motor[left] = 0;
	motor[right] = 0;
}

/**
 * Turns around until black is found
 */
void turnToLine(void) {
  spinInDirection();

	while(!isDark()) {
		abortTimeslice();
	}
	int startDir = currentDirection();

	while(isDark()) {
	  abortTimeslice();
  }
	motor[left] = 0;
	motor[right] = 0;
	int centre = startDir + (angleDifference(currentDirection(), startDir)/2);
	nxtDisplayCenteredTextLine(6, "cen: %i", centre);
	turnToAngle(centre, 0);
}

int FollowLineTilLeaf(void) {
	StartTask(FollowEdge);
	int forwards = gatherForwardsData();
	for(;;) {
		if(detectNode(forwards)) {
			if(checkIsLeaf()) {
				followingEdge = false;
				StopTask(FollowEdge);
				return FOUND_LEAF;
			} else {
				forwards = gatherForwardsData();
			}
		}
		abortTimeslice();
	}
}


/**

*/
void faceForward(int forwards){
	if(angleDifference(currentDirection(), forwards) > 0){//too far to the right
		motor[left] = -SPEED; //turn left
	} else {
		motor[right] = -SPEED; //turn right
	}
	while(abs(angleDifference(currentDirection(), forwards)) != 0){
		wait1Msec(1);
	}
}

/**
  Moves forwards so that the centre of rotation is approximately where the light sensor was.
*/
void moveToSensor() {
	int timeToWait = (sensorToAxle * 1000)/cruiseSpeed;
	time1[T3] = 0;
	motor[left] = SPEED;
	motor[right] = SPEED;

	while(time1[T3] < timeToWait) {
	  wait1Msec(1);
	};


	motor[left] = 0;
	motor[right] = 0;

}

int FollowSegmentTilEnd(Edge &edge) {
	StartTask(FollowEdge);
	int forwards = gatherForwardsData();
	edge.angle = forwards;

	int found = FOUND_ERROR;

	while(found == FOUND_ERROR) {
		if(detectCan()) {
			found = FOUND_CAN;
		}
		if(detectNode(forwards)) {
			found = FOUND_NODE;
	  }
		//abortTimeslice();
	}

	followingEdge = false;
	StopTask(FollowEdge);
	edge.length = sweeps;
	faceForward(forwards);
	moveToSensor();

	return found;
}