void cleanup() { create_set_speeds(hands->c, 0, 0); //Stop moving create_close(hands->c); turret_close(hands->t); free(filt->sonar0); free(filt->sonar1); free(filt->ir0); free(filt->ir1); free(pids->trans); free(pids->sonar); free(pids->angle); free(pids->angleT); free(posData); posData = NULL; free(hands); free(filt); free(pids); }
int main(int argc, const char **argv) { printf("entered program\n"); /* allocate device objects */ c = create_create("/dev/ttyS2"); r = turret_create(); /* open the create serial comm */ if(create_open(c,FULLCONTROL) < 0) { printf("create open failed\n"); return(-1); } /* Open the i2c device */ if(turret_open(r) < 0) { printf("failed to connect to robostix\n"); return(-1); } /* init the robostix board interfaces */ printf("initialize turret\n"); turret_init(r); // sonar if(!WHICH_SENSOR){ turret_SetServo(r,90); } //ir else{ turret_SetServo(r,0); } filter = firFilterCreate(); waypoints[0].x = 7.3; waypoints[0].y = 0.0; waypoints[1].x = 7.3; waypoints[1].y = 7.62; waypoints[2].x = 26.19; waypoints[2].y = 7.62; waypoints[3].x = 26.19; waypoints[3].y = -3.04; waypoints[4].x = 30.15; waypoints[4].y = -3.04; waypoints[5].x = 30.15; waypoints[5].y = -12.18; waypoints[6].x = 7.3; waypoints[6].y = -12.18; waypoints[7].x = 7.3; waypoints[7].y = 0.0; waypoints[8].x = 0.0; waypoints[8].y = 0.0; int i; for(i = 0; i < 9; i++) { Move(c, r, waypoints, i); } /* // read points from file FILE *file = fopen(argv[1], "r"); printf("just read file\n"); if(file != NULL) { char line[32]; char num_lines; // read the first line in the file. // this gives the number of waypoints printf("fgetting line nums\n"); fgets(num_lines, 1, file); num_waypoints = atoi(num_lines); printf("allocating waypoint size\n"); // allocate the size of *waypoints waypoints = malloc(num_waypoints * sizeof(waypoint)); printf("malloc'd\n"); // store the (x,y) coordinates in *waypoints int i = 0; printf("fgetting lines\n"); while(fgets(line,sizeof(line),file) != NULL) { printf("foo\n"); file_tokens = strtok(line, ","); printf("bar\n"); waypoints[i].x = atof(file_tokens[0]); printf("freep\n"); waypoints[i].y = atof(file_tokens[1]); printf("froop\n"); i++; } fclose(file); // Move to each waypoint int j; for(j = 0; j < num_waypoints; j++) { Move(c, r, waypoints[j], j); } } else { perror(argv[1]); }*/ // Shutdown and tidy up create_set_speeds(c,0.0,0.0); create_close(c); create_destroy(c); turret_close(r); turret_destroy(r); exit(0); return 0; }
/* Move() * client: client to connect to robot * distance: x-coordinate that robot should aim for; * angle: angle that robot should stay at; */ void Move(create_comm_t *client, turret_comm_t *t, waypoint point[], int pos) { printf("entering move\n"); // get ir error_dist = error_t(client, point[pos]); error_a = error_ta(client, curr_angle); while(error_dist > BUFFER_DIST) { position = create_get_sensors(client, TIMEOUT); error_dist = error_t(client, point[pos]); printf("error_dist: %f\n", error_dist); vx = PID(error_dist); printf("vx: %f\n", vx); va = PID_A(error_a); printf("va: %f\n", va); create_set_speeds(client, vx, va); if(client->bumper_left == 1 || client->bumper_right == 1) { create_set_speeds(client, 0.0, 0.0); break; } sonar_error = error_sonar(); printf("sonar error: %f\n", sonar_error); if(sonar_error > 0.0) { va += (M_PI/16); create_set_speeds(client,vx,va); } else if(sonar_error < 0.0) { va -= (M_PI/16); create_set_speeds(client,vx,va); } //usleep(500); } //if(error_dist <= BUFFER_DIST) //{ dist = getDistance(point[pos], point[pos+1]); printf("dist: %f\n", dist); new_angle = getAngle(dist); printf("new_angle: %f\n", new_angle); if( ((point[pos+1].x > point[pos].x) && (point[pos+1].y >= point[pos].y)) || ((point[pos+1].x > point[pos].x) && (point[pos+1].y <= point[pos].y)) || ((point[pos+1].x < point[pos].x) && (point[pos+1].y >= point[pos].y)) || ((point[pos+1].x < point[pos].x) && (point[pos+1].y <= point[pos].y)) || ((point[pos+1].y < point[pos].y) && (point[pos+1].x >= point[pos].x)) || ((point[pos+1].y < point[pos].y) && (point[pos+1].x <= point[pos].x)) || ((point[pos+1].y > point[pos].y) && (point[pos+1].x <= point[pos].x)) || ((point[pos+1].y > point[pos].y) && (point[pos+1].x >= point[pos].x)) ); { new_angle = new_angle * -1; } //} va += new_angle; curr_angle = va; create_set_speeds(client, vx, va); /* printf("Enter Move\ngetting error_t\n"); create_get_sensors(client, TIMEOUT); error_dist = error_t(client, point); printf("just got error_t\n"); while(1) { create_get_sensors(client, TIMEOUT); printf("in while loop\n"); error_dist = error_t(client, point); vx = PID(error_dist); error_a = error_ta(client, 0.0); va = PID_A(error_a); float sensor_error; if(WHICH_SENSOR){ sensor_error = error_ir(t); } else{ sensor_error = error_sonar(t); } printf("getting sensor_error %d\n",sensor_error); if(sensor_error >= 3.0){ //too far to the left printf("decrementing va\n"); va--; } if(sensor_error <= -3.0){ //too far to the right printf("incrementing va\n"); va++; } printf("setting speeds\n"); create_set_speeds(client, vx, va); printf("test bumpers\n"); if(client->bumper_left || client->bumper_right) { create_set_speeds(client, 0.0, 0.0); break; } printf("Moving : x = %f y = %f a = %f\n", client->ox, client->oy, client->oa); printf("ir = %d %d sonar = %d %d\n",r->ir[0],r->ir[1],r->sonar[0],r->sonar[1]); usleep(10000); } */ printf("Leave Move\n"); }
int main(int argc,char **argv) { turret_comm_t *r; create_comm_t *c; int i2c_fd; int command; int i,angle; double vx,va; char ch; /* allocate device objects */ c = create_create("/dev/ttyS2"); r = turret_create(); /* open the create serial comm */ if(create_open(c,FULLCONTROL) < 0) { printf("create open failed\n"); return(-1); } /* Open the i2c device */ if(turret_open(r) < 0) { printf("failed to connect to robostix\n"); return(-1); } /* init the robostix board interfaces */ turret_init(r); /* robot is set up and ready -- select tests and run */ for(command = show_menu(); command != 0; command = show_menu() ) switch(command) { case TEST_SONAR: printf("testing Sonars, next 500 readings\n"); for(i=0;i<5;i++) { if((!turret_get_sonar(r))) printf("sensor read timeout\n"); else printf("%d\t%d\n",r->sonar[0],r->sonar[1]); } break; case TEST_IR: printf("testing IR, next 5 readings\n"); for(i=0;i<5;i++) { if(!turret_get_ir(r)) printf("sensor read timeout\n"); else printf("%d\t%d\n",r->ir[0],r->ir[1]); } break; case TEST_TURRET: printf("testing Servo\nEnter angle: "); for(angle == 0;(angle <180) | (angle >180); scanf("%d",&angle)) { printf("setting servo to %d degrees\n",angle); turret_SetServo(r,angle); } break; case TEST_MOVE: printf("i- speed up\nn-slow down\nk- angle right\nj - angle left\n, x- stop\n"); vx=0.;va=0.; for( ch=getchar(); ch != 'x'; ch=getchar()) { switch(ch) { case 'i': vx += INCREMENT; break; case 'n': vx -= INCREMENT; break; case 'j': va += INCREMENT; break; case 'k': va -= INCREMENT; break; case 'x': va = 0.; vx = 0.; break; default: break; } if(vx > 1.) vx = 1.; if(va > 1.) va = 1.; create_set_speeds(c,vx,va); } break; default: command = -1; break; } }