void turnLeft(int maxspd, int minspd, int degrees)
{
//causes robot to spin left to a certain angle.
int left1, left2, right1, right2, drotated = 0;
get_motor_encoders(&left1, &right1);
int turnLength = RIGHT_ANGLE_TURN_ENC * degrees / 90;
while(drotated < turnLength)
{
get_motor_encoders(&left2, &right2);
drotated = ((left1 - left2) + (right2 - right1)) / 2;
if(drotated < (maxspd - minspd) * 2)
{
//accelerate phase
set_motors(-(minspd + drotated / 2), minspd + drotated / 2);
}
else if(drotated < turnLength - (maxspd - minspd) * 2)
{
//max speed phase
set_motors(-maxspd, maxspd);
}
else
{
//decelerate phase
set_motors(-(minspd + (turnLength - drotated) / 2), minspd + (turnLength - drotated) / 2);
}
positioncalc();
}
set_motors(0, 0);
}
void moveBackwards(int maxspd, int minspd, int dist)
{
//speeds up, moves the robot backwards a certain distance, and slows down.
int left1, left2, right1, right2;
get_motor_encoders(&left1, &right1);
while(1)
{
get_motor_encoders(&left2, &right2);
int dtravelled = ((left1 - left2) + (right1 - right2)) / 2;
if(dtravelled < (maxspd - minspd) * 2)
{
//accelerate phase
set_motors(-(minspd + dtravelled / 2), -(minspd + dtravelled / 2));
}
else if(dtravelled < dist - (maxspd - minspd) * 2)
{
//max speed phase
set_motors(-maxspd, -maxspd);
}
else if(dtravelled < dist)
{
//decelerate phase
set_motors(-(minspd + (dist - dtravelled) / 2), -(minspd + (dist - dtravelled) / 2));
}
else
{
break;
}
positioncalc();
}
set_motors(0, 0);
}
//Position tracking on servo encoder reading.
double position_tracker(double curr_coord[2]) {
// Function update global var face_angle.
// Function update current coordinates.
// Return the distance traveled.
int currenc[2] = {0, 0};
get_motor_encoders(¤c[LEFT], ¤c[RIGHT]);
double dl = enc_to_dist(currenc[LEFT] - prevenc[LEFT]);
double dr = enc_to_dist(currenc[RIGHT] - prevenc[RIGHT]);
double dx = 0, dy = 0;
double dangle = ( dl - dr )/ width;
if (dangle != 0) {
double rl = dl / dangle;
double rr = dr / dangle;
double rm = ( rl + rr ) / 2;
dx = rm * ( cos(face_angle) - cos(face_angle + dangle) );
dy = rm * ( sin(face_angle + dangle) - sin(face_angle) );
face_angle += dangle;
face_angle += (face_angle > to_rad(180)) ? -to_rad(360) : (face_angle < -to_rad(180)) ? to_rad(360) : 0;
}
else {
dy = dl * cos(face_angle);
dx = dr * sin(face_angle);
}
// Encoders update
prevenc[LEFT] = currenc[LEFT];
prevenc[RIGHT] = currenc[RIGHT];
// Coordinates update
curr_coord[0] += dx;
curr_coord[1] += dy;
return sqrt(dx*dx + dy*dy);
}
void moveForwards(int maxspd, int minspd, int dist)
{
//speeds up, moves the robot forwards a certain distance, and slows down.
int left1, left2, right1, right2;
get_motor_encoders(&left1, &right1);
while(1)
{
get_motor_encoders(&left2, &right2);
int dtravelled = ((left2 - left1) + (right2 - right1)) / 2;
if(dtravelled < (maxspd - minspd) * 2)
{
//accelerate phase
set_motors(minspd + dtravelled / 2, minspd + dtravelled / 2);
}
else if(dtravelled < dist - (maxspd - minspd) * 2)
{
//max speed phase
set_motors(maxspd, maxspd);
}
else if(dtravelled < dist)
{
//decelerate phase
set_motors(minspd + (dist - dtravelled) / 2, minspd + (dist - dtravelled) / 2);
}
else
{
break;
}
positioncalc();
int ultrasound = get_us_dist();
if(ultrasound < FRONT_WALL_MIN_DIST) { break; }
}
set_motors(0, 0);
}
void setup()
{
connect_to_robot();
initialize_robot();
set_ir_angle(LEFT, -45);
set_ir_angle(RIGHT, 45);
set_origin();
set_point(0, 0);
print_square_centres();
get_motor_encoders(&leftprev, &rightprev);
moveBackwards(PHASE1_SPEED, PHASE1_SLOW, START_OFFSET * CM_TO_ENCODER); //move robot to centre of first square.
}
void positioncalc()
{
//calculates robot position in each step/call.
int leftenc, rightenc;
get_motor_encoders(&leftenc, &rightenc);
//gets change in left encoder and right encoder readings, convert to cm, and calculate angle turned by robot
double leftdiff = (leftenc - leftprev) / CM_TO_ENCODER;
double rightdiff = (rightenc - rightprev) / CM_TO_ENCODER;
double anglediff = (leftdiff - rightdiff) / WIDTH;
//saves current encoder values to global variables for next function call.
leftprev = leftenc;
rightprev = rightenc;
//calculate change in x- and y- position.
double deltax = 0.0, deltay = 0.0;
if(anglediff != 0)
{
double radiusl = leftdiff / anglediff;
double radiusr = rightdiff / anglediff;
double radius = (radiusl + radiusr) / 2;
deltax = (radius * cos(bearing)) - (radius * cos(bearing + anglediff));
deltay = (radius * sin(bearing + anglediff)) - (radius * sin(bearing));
}
else if(anglediff == 0)
{
double dist = (leftdiff + rightdiff) / 2;
deltax = dist * sin(bearing);
deltay = dist * cos(bearing);
}
//saves current position and bearing to global variables.
bearing += anglediff;
if(bearing < 0) { bearing += 2 * M_PI; }
else if(bearing > 2 * M_PI) { bearing -= 2 * M_PI; }
xpos += deltax;
ypos += deltay;
//checks accurancy of the robot's calculated current position vs its real position
//log_trail();
//set_point(xpos, ypos);
}
void spin(double curr_coord[2], double angle){
if (angle == 0)
return;
printf("Start Spinning: from %f, with :%f\n",to_degree(face_angle), to_degree(angle));
double initial_angle = face_angle;
int speed = 15;
int tempspeed = 0;
double angle_turned = 0;
double abs_angle = fabs(angle);
double side = angle/fabs(angle);
int tempenc[2] = {0, 0};
while (angle_turned < abs_angle){
get_motor_encoders(&tempenc[0], &tempenc[1]);
double dl = enc_to_dist(tempenc[0] - prevenc[0]);
double dr = enc_to_dist(tempenc[1] - prevenc[1]);
face_angle += ( dl - dr )/ width;
if (abs_angle < to_rad(30)){
tempspeed = side * 1;
set_motors(tempspeed, -tempspeed);
}
else if(angle_turned >= fabs(0.92*angle)){
tempspeed = (tempspeed < 2) ? side : side * speed * (1 - fabs(angle_turned/angle));
set_motors(tempspeed, -tempspeed);
}
else {
set_motors(side*speed, side*-speed);
}
angle_turned = fabs(face_angle - initial_angle);
prevenc[0] = tempenc[0];
prevenc[1] = tempenc[1];
//printf("Monitoring: angle: %f X: %f , Y: %f \n", to_degree(face_angle), curr_coord[0], curr_coord[1] );
}
position_tracker(curr_coord);
set_motors(0, 0);
printf("(Spinning done ! %f \n", to_degree(face_angle) );
usleep(10000);
}
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");
*/
}
}
