I was told that Dijkstras algorithm could help, finding the shortest path to the object that we want to pick up, but I don't really know how to implement that into my problem!

I attached my code so far and what the second assignment is exactly... any suggestions are appreciated, I'm stumped!

/* Lance K Robotics Lab One 2/1/2010 */ #include <stdio.h> #include <math.h> #define POST_LENGTH 100 #define ARM_LENGTH 80 #define PI 3.141592654 struct configInfo { //Struct containing the necessary information that will be stored in the 81x81 array. int allowed; double a; double b; double x; double y; }; double getHandX(double alpha, double beta) { //Calculates the x coordinate of the hand given 2 angles, alpha and beta. double delta = PI - (beta + alpha); return(ARM_LENGTH * sin(-alpha) + ARM_LENGTH * sin(-delta)); } double getHandY(double alpha, double beta) { //Calculates the x coordinate of the hand given 2 angles, alpha and beta. double delta = PI - (beta + alpha); return(ARM_LENGTH * cos(-alpha) + POST_LENGTH - ARM_LENGTH * cos(-delta)); } void getLoc() { //Prints the location of the robot gripper in x-y space given 2 angles by the user. double inputAlpha; double inputBeta; printf("Enter the angle of the first joint"); scanf("%lg", &inputAlpha); printf("Enter the angle of the second joint"); scanf("%lg", &inputBeta); printf("the location of the hand is (%f , %f) \n,", getHandX(inputAlpha,inputBeta), getHandY(inputAlpha,inputBeta)); } double getJoint2XLocation(double alpha) { return(-80*sin(alpha)); } double getJoint2YLocation(double alpha) { return(80*cos(alpha) + 100); } void fillMap(struct configInfo map[81][81]) { //Breaks the configuration space from -pi to +pi into 81 configInfo objects in each direction(x and y). int i,j; for(i = 0; i < 81; i++) { for(j = 0; j < 81; j++) { //Assigns an alpha and beta angle into each of the configInfo nodes depending on where in the //array the node lies. map[i][j].b = -PI + j*(PI/40); map[i][j].a = -PI + i*(PI/40); //Determines the (x,y) coordinates of the hand depending on where the hand is in configuration space and //assigns the values into the corresponding node in the configInfo array. map[i][j].x = getHandX(map[i][j].a,map[i][j].B)/>; map[i][j].y = getHandY(map[i][j].a,map[i][j].B)/>; //Sets the value of allowed for each node to false if these conditions apply... //Under the ground. if(map[i][j].y < 0) { map[i][j].allowed = 0; } //Through base of arm. else if(map[i][j].a < 0 && map[i][j].b < 0 && map[i][j].x < 0) { map[i][j].allowed = 0; } //Through base of arm. else if(map[i][j].a > 0 && map[i][j].b > 0 && map[i][j].x > 0) { map[i][j].allowed = 0; } //Arm is only allowed a flexibility of 7/8 pi. else if(map[i][j].a > (7*PI/8) || map[i][j].a < (-7*PI/8)) { map[i][j].allowed = 0; } //Arm is only allowed a flexibility of 7/8 pi. else if(map[i][j].b > (7*PI/8) || map[i][j].b < (-7*PI/8)) { map[i][j].allowed = 0; } else if(crossLine(map[i][j].a, map[i][j].B)/> == 1) { map[i][j].allowed = 0; } else { map[i][j].allowed = 1; } } } } int crossLine(double alpha, double beta) { //x1, y1 and x2,y2 are the endpoints of a line extending from joint 2 to the robot's hand. int isAllowed = 1; double x1, y1, x2, y2; x1 = getJoint2XLocation(alpha); y1 = getJoint2YLocation(alpha); x2 = getHandX(alpha, beta); y2 = getHandY(alpha, beta); double z = (100 - x1)/(x2 - x1); double temp = z*(y2 - y1) + y1; /* printf("Line 1 endpoint 1: %f, %f", x1, y1); printf("\nLine 1 endpont 2: %f, %f", x2, y2); printf("\nLine 2 endpoint1: %f, %f", u1, v1); printf("\nLine 2 endpoint2: %f, %f", u2, v2); printf(""); */ if (x1 < 100 && x2 < 100) { isAllowed = 0; } else if(y1 < 150 && y2 < 150) { isAllowed = 1; } else if(temp > 150) { isAllowed = 0; } else isAllowed = 0; return isAllowed; } void printMap(struct configInfo map[81][81]) { //Prints the entire 81x81 array showing a "." if the coordinates are accessable and "#" if they are not with the top //left of the printout corresponding to -pi for the alpha and beta angles and bottom right being +pi. int i,j; for(i = 80; i >= 0; i--) { for(j = 0; j < 81; j++) { if(map[i][j].allowed == 1) { printf("."); } else { printf("#"); } } printf("\n"); } } int main() { struct configInfo myMap[81][81]; fillMap(myMap); printMap(myMap); //int x = getJoint2XLocation(-PI/4); //int y = getJoint2YLocation(-PI/4); //crossLine(-PI/8, -3*PI/8); //printf("X: %d", x); //printf("\nY: %d", y); return 0; }

http://castor.augsbu...p/1885/two.html