int main(){
connect_to_robot();
initialize_robot();
set_origin();
set_ir_angle(LEFT, -45);
set_ir_angle(RIGHT, 45);
initialize_maze();
reset_motor_encoders();
int i;
for (i = 0; i < 17; i++){
set_point(nodes[i]->x, nodes[i]->y);
}
double curr_coord[2] = {0, 0};
map(curr_coord, nodes[0]);
breadthFirstSearch(nodes[0]);
reversePath(nodes[16]);
printPath(nodes[0]);
struct point* tail = malloc(sizeof(struct point));
tail->x = nodes[0]->x;
tail->y = nodes[0]->y;
struct point* startpoint = tail;
build_path(tail, nodes[0]);
// Traverse to end node.
while(tail->next){
set_point(tail->x, tail->y); // Visual display for Simulator only.
tail = tail->next;
}
tail->next = NULL; // Final node point to null.
printf("tail: X = %f Y = %f \n", tail->x, tail->y);
parallel(curr_coord);
spin(curr_coord, to_rad(180));
sleep(2);
set_ir_angle(LEFT, 45);
set_ir_angle(RIGHT, -45);
mazeRace(curr_coord, nodes[0]);
return 0;
}
void initialize_robot() {
printf("initializing\n");
set_ir_angle(LEFT, 0);
set_ir_angle(RIGHT, 0);
reset_motor_encoders();
}
void centering(){
set_motors(-14, -14);
sleep(3);
reset_motor_encoders();
}
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");
*/
}
}
