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;
}
