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