/* The message is returned in a static buffer, so don't call this
again until you're done with the previous response or it will be overwritten. */
const char* recv_msg(char *buf, int bufsize) {
int val;
fd_set read_fdset;
fd_set except_fdset;
struct timeval tv;
char readbuf[bufsize];
unsigned int totalbytes;
/* Is there already a reply in the buffer from before? (can happen if
we get multiple lines in one read) */
const char *reply_msg = extract_reply(buf);
if (reply_msg != NULL)
return reply_msg;
while (1) {
tv.tv_sec = 1;
tv.tv_usec = 0;
FD_ZERO(&read_fdset);
FD_ZERO(&except_fdset);
FD_SET(sock, &read_fdset);
FD_SET(sock, &except_fdset);
if (select(sock+1, &read_fdset, NULL, &except_fdset, &tv) == 0) {
/* we've waited 2 seconds and got no response - too long - conclude
the socket is dead */
printf("timed out waiting response\n");
//connect_to_robot();
//initialize_robot();
return 0;
}
if (FD_ISSET(sock, &except_fdset)) {
connect_to_robot();
re_initialize_robot();
return 0;
}
assert(FD_ISSET(sock, &read_fdset));
val = read(sock, readbuf, bufsize-1);
if (val > 0) {
} else {
/* the write failed - likely the robot was switched off - attempt
to reconnect and reinitialize */
connect_to_robot();
re_initialize_robot();
buf[0]='\0';
return 0;
}
/* ensure readbuf is null terminated */
readbuf[val] = '\0';
totalbytes = strlen(buf);
add_to_buffer(buf, &totalbytes, readbuf);
reply_msg = extract_reply(buf);
if (reply_msg != NULL)
return reply_msg;
}
}
int main(){
connect_to_robot();
initialize_robot();
setAngles();
firsteight();
}
int main ()
{
connect_to_robot();
initialize_robot();
setAnglesStart();
//wallFollowingR();
wallFollowingL();
}
int main(){
connect_to_robot();
initialize_robot();
set_origin();
double curr_coord[2] = {0, 0};
spin(curr_coord, to_rad(90));
sleep(1);
spin(curr_coord, to_rad(180));
return 0;
}
int send_msg(char* msg, int len) {
if (write(sock, msg, len) <= 0) {
/* the write failed - likely the robot was switched off - attempt
to reconnect and reinitialize */
printf("send failed - reconnecting\n");
connect_to_robot();
re_initialize_robot();
return 0;
} else {
return 1;
}
}
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.
}
int main()
{
connect_to_robot();
initialize_robot();
setAngles();
inputMotors();
//wallFollowing();
int i;
for (i=0; i<4; i++){
printf("%d\n", i);
thetaAcc = 0.0;
SPEED = SPEED + 8;
//wallFollowing();
wallFollowingR();
}
}
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;
}
int main() {
connect_to_robot();
initialize_robot();
set_origin();
printf("Ghandy-Bot activated, destroy!!!\n");
//Set front IR sensors to to face left and right
set_ir_angle(LEFT, -45);
set_ir_angle(RIGHT, 45);
init_map();
print_map();
robot.map[1][1].walls[0] = 1;
robot.map[1][2].walls[2] = 1;
robot.map[1][1].walls[1] = 1;
robot.map[2][1].walls[3] = 1;
robot.map[1][3].walls[2] = 1;
robot.map[1][2].walls[0] = 1;
robot.map[0][4].walls[2] = 1;
robot.map[0][3].walls[0] = 1;
robot.map[3][4].walls[3] = 1;
robot.map[2][4].walls[1] = 1;
robot.map[3][2].walls[2] = 1;
robot.map[3][1].walls[0] = 1;
robot.map[3][2].walls[3] = 1;
robot.map[2][2].walls[1] = 1;
int i;
for(i=1;i<=4;i++)
robot.map[0][i].walls[3] = 1;
for(i=1;i<=4;i++)
robot.map[i][1].walls[2] = 1;
for(i=1;i<=4;i++)
robot.map[3][i].walls[1] = 1;
print_map();
/* Phase 1 */
//Perform depth first search on maze
//dfs(0, 0, M_PI / 2);
/* Phase 2 */
lee(0,1); //Update matrix with Lee costs from node [0,1]
/* Generate shortest path to node [3,4] (finish line) */
generate_path(3,4);
follow_path(); //Follow the generated path to finish
//Phew, did we win???
printf("Ghandy-Bot deactivated!\n");
return 0;
}
