void followMap(double map[MAP_SIZE][2])
{
int lastindex = 0;
while(getDistance(xpos, ypos, 3 * SQUARE_DIST, 4 * SQUARE_DIST - START_OFFSET) > PHASE2_STOP)
{
//selects target point - the first point in the map that is just above a certain distance away from the robot
while((getDistance(xpos, ypos, map[lastindex][0], map[lastindex][1]) < PHASE2_GOAL) && (map[lastindex + 1][0] != 0 || map[lastindex + 1][1] != 0))
{
lastindex++;
}
double angle_to_point = getAngle(xpos, ypos, map[lastindex][0], map[lastindex][1]); //angle between the robot position, the target point, and the vertical axis
if(angle_to_point > 5.5) { angle_to_point -= 2 * M_PI; }
if(bearing > 5.5) { bearing -= 2 * M_PI; }
double angle_to_turn = angle_to_point - bearing; //angle needed for the robot to turn so that it heads towards targeted point
int leftspd = PHASE2_SPEED, rightspd = PHASE2_SPEED;
if(angle_to_turn > 0) //targeted point is to the right of robot's current heading
{
//turn right
rightspd = PHASE2_SPEED - (PHASE2_SPEED * angle_to_turn * PHASE2_STEER);
if(rightspd < 0) { rightspd = 0; }
}
else if(angle_to_turn < 0) //targeted point is to the left of robot's current heading
{
//turn left
leftspd = PHASE2_SPEED + (PHASE2_SPEED * angle_to_turn * PHASE2_STEER);
if(leftspd < 0) { leftspd = 0; }
}
set_motors(leftspd, rightspd);
positioncalc();
log_trail();
}
}
void slowDown()
{
//code to slow down once the robot nears finish
int i;
for(i = PHASE2_STOP; i > 0; i--)
{
double slowspeed = PHASE2_SPEED * ((double) i / (double) PHASE2_STOP);
set_motors(slowspeed, slowspeed);
log_trail();
}
}
char* getPath(int startx, int starty, int destx, int desty)
{
//moves the robot from a square to another (in the grid where start = (0, 0)), following left wall, and records the path taken.
int left, right, index = 0, currentx = startx, currenty = starty; //stores current position of robot in the grid in currentx and currenty
int idealBearing = getIdealBearing();
char* path = malloc(sizeof(char) * PATH_MAX_LENGTH);
while (currentx != destx || currenty != desty)
{
get_front_ir_dists(&left, &right);
int front = get_us_dist();
/*if(front < MAX_DIST_TO_WALL)
{
checkFrontWall(front);
}*/
//checks accurancy of the robot's calculated current position vs its real position
log_trail();
set_point(xpos, ypos);
if(left > MAX_DIST_TO_WALL)
{
//turn left
turnLeft(PHASE1_SPEED, PHASE1_SLOW, 90);
idealBearing -= 90;
if(idealBearing < 0) { idealBearing += 360; }
path[index] = 'L';
}
else if(front > MAX_DIST_TO_WALL)
{
//go straight
path[index] = 'S';
}
else if(right > MAX_DIST_TO_WALL)
{
//turn right
turnRight(PHASE1_SPEED, PHASE1_SLOW, 90);
idealBearing += 90;
if(idealBearing >= 360) { idealBearing -= 360; }
path[index] = 'R';
}
else
{
//turn around
turnRight(PHASE1_SPEED, PHASE1_SLOW, 180);
idealBearing += 180;
if(idealBearing >= 360) { idealBearing -= 360; }
path[index] = 'B';
}
fixBearing(idealBearing);
moveForwards(PHASE1_SPEED, PHASE1_SLOW, SQUARE_DIST * CM_TO_ENCODER);
//uses robot's idealBearing to figure out which square in the grid the robot is currently in after moving
switch(idealBearing)
{
case 0: currenty++; break;
case 90: currentx++; break;
case 180: currenty--; break;
case 270: currentx--; break;
default: printf("Error: unexpected value for idealBearing\n"); break;
}
index++;
}
path[index] = '\0';
return path;
}
void followPath(Location path[], int pathLength, Location* currentLocation) {
// Draw the path formed from collected points
drawPath(path, pathLength);
// Initialise the target location
Location targetLocation = path[40];
Measurement measurement = {0, 0, 0, 0};
// Reset the measurements
reset_motor_encoders();
usleep(20000);
// Initialise some variables
double targetBearing = 0;
double bearingDifference = 0;
double closestDistance = 0;
int closestLocationIndex = 0;
int speedAddition = 0;
int speedReduction = 0;
int changeX = 0;
int changeY = 0;
// 120 -> 50, 70
// 90 -> 40, 70
int speed = 120;
int speedLeft = speed;
int speedRight = speed;
// Get the robot moving
set_motors(speed, speed);
// Main control loop
while(1) {
// Take the measurement
measurement.startLeft = measurement.endLeft;
measurement.startRight = measurement.endRight;
get_motor_encoders(&measurement.endLeft, &measurement.endRight);
// Update the robot's bearing and location
updateBearingAndLocation(&measurement, currentLocation);
// Draw out the path taken
set_point(currentLocation->x, currentLocation->y - 17.0);
log_trail();
if (speed - speedAddition < 1) {
speedReduction = speed - 1;
} else {
speedReduction = speedAddition;
}
if (speed + speedAddition > 127) {
speedAddition = 127 - speed;
}
// Adjust the wheel speeds according to to how wrong the robot's current direction is
if (bearingDifference < 0) {
speedLeft = speed - speedReduction;
speedRight = speed + speedAddition;
} else {
speedLeft = speed + speedAddition;
speedRight = speed - speedReduction;
}
set_motors(speedLeft, speedRight);
// If closer than X cm to the last point in the path, stop the robot
if (distance(&path[closestLocationIndex], &path[pathLength - 1]) < 23.0 || atFinalSquare(currentLocation)) {
set_motors(0, 0);
break;
}
// Find the point on the path that is closest to the robot
closestDistance = distance(currentLocation, &path[0]);
closestLocationIndex = 0;
int i;
for (i = 1; i < pathLength; i++) {
if (closestDistance > distance(currentLocation, &path[i])) {
closestDistance = distance(currentLocation, &path[i]);
closestLocationIndex = i;
}
}
// Find the point on the path that is about X cm away from the robot
targetLocation = path[closestLocationIndex];
i = 1;
while (distance(currentLocation, &targetLocation) < 50.0) {
if (closestLocationIndex + i >= pathLength) {
break;
}
targetLocation = path[closestLocationIndex + i];
i++;
}
// Calculate difference between the current and target
// location in terms of the x and y components
//set_point(targetLocation.x, targetLocation.y);
changeX = targetLocation.x - currentLocation->x;
changeY = targetLocation.y - currentLocation->y;
// Determine the target bearing and deal with some edge cases
double angle = atan2(changeX, changeY);
if (fabs(angle - currentLocation->bearing) <= fabs((2*M_PI - currentLocation->bearing) + angle)) {
angle = angle - currentLocation->bearing;
} else {
angle = (2*M_PI - currentLocation->bearing) + angle;
}
if (angle > M_PI) {
angle = currentLocation->bearing - 2*M_PI;
}
targetBearing = angle;
// Adjust speed according to how wrong the robots current direction is
bearingDifference = targetBearing - currentLocation->bearing;
bearingDifference = angle;
speedAddition = abs((int) (bearingDifference * 75.0));
/*
printf("Following the map \n");
printf("Closest x = %f; y = %f \n", path[closestLocationIndex].x, path[closestLocationIndex].y);
printf("Setting motor speeds: left = %d; right = %d \n", speedLeft, speedRight);
printf("Current x = %f; y = %f; bearing %f %f \n", currentLocation->x, currentLocation->y, currentLocation->bearing, currentLocation->bearing * (180/M_PI));
printf("Target x = %f; y = %f; bearing to target %f %f \n", targetLocation.x, targetLocation.y, targetBearing, targetBearing * (180/M_PI));
printf("bearingDifference = %f %f; \n", bearingDifference, bearingDifference * (180/M_PI));
printf("\n");
*/
}
}
