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"); */ } }