int main(int argc, const char **argv) {
int i, a;
double t;
int i2c_fd;
coordData_t * waypoints;
int numWaypts = NUM_WAYPOINTS;
double w[] = WAYPOINT_ARRAY;
pthread_t thread;
//Set signals to handle
signal(SIGINT, &sighandle);
signal(SIGTERM, &sighandle);
signal(SIGABRT, &sighandle);
//allocate structs
if((hands = malloc(sizeof(api_HANDLES_t))) == NULL) {
fprintf(stderr,"Error calling malloc\n");
return -1;
}
if((filt = malloc(sizeof(FilterHandles_t))) == NULL) {
fprintf(stderr,"Error calling malloc\n");
return -1;
}
if((pids = malloc(sizeof(pidHandles_t))) == NULL) {
fprintf(stderr,"Error calling malloc\n");
return -1;
}
if((waypoints = malloc(sizeof(coordData_t) * numWaypts)) == NULL) {
fprintf(stderr,"Error calling malloc\n");
return -1;
}
if((posData = malloc(sizeof(roboPos_t))) == NULL) {
fprintf(stderr,"Error calling malloc\n");
return -1;
}
for(i=0; i<numWaypts; i++) {
waypoints[i].X = w[i*2];
waypoints[i].Y = w[i*2+1];
}
// allocate devices
if((hands->c = create_create(COMPORT)) == NULL) {
fprintf(stderr,"Error connecting create\n");
return -1;
}
if((hands->t = turret_create()) == NULL) {
fprintf(stderr,"Error connecting turrett\n");
return -1;
}
//open serial com port
if(create_open(hands->c, FULLCONTROL) < 0) {
fprintf(stderr,"Failed to open create\n");
return -1;
}
//open i2c device
if(turret_open(hands->t) < 0) {
fprintf(stderr,"Failed to connect to robostix\n");
return -1;
}
//initialize the robostix board
turret_init(hands->t);
//Initialize Filter structs
filt->sonar0 = initializeFilter(FILT_SONAR);
filt->sonar1 = initializeFilter(FILT_SONAR);
filt->ir0 = initializeFilter(FILT_IR);
filt->ir1 = initializeFilter(FILT_IR);
//Initialize PID structs
pids->trans = initializePID(TRANS_PID);
pids->angle = initializePID(ANGLE_PID);
pids->sonar = initializePID(SONAR_PID);
pids->angleT = initializePID(ANGLET_PID);
// process command line args
i = 1;
while( i < argc ) {
if(!strcmp(argv[i], "-w")) {
//Accept command line waypoints
if(argc >= i+1) {
numWaypts = atoi(argv[++i]);
if(argc >= i+(2*numWaypts)) {
free(waypoints);
if((waypoints = malloc(sizeof(coordData_t) * numWaypts)) == NULL) {
fprintf(stderr,"Error calling malloc\n");
return -1;
}
for(a=0; a<numWaypts; a++) {
waypoints[a].X = atoi(argv[++i]);
waypoints[a].Y = atoi(argv[++i]);
}
i++;
} else {
printf("Error using [-w].\n\tUsage: -w numPoints X1 Y1 X2 Y2...\n");
}
} else {
printf("Error using [-w].\n\tUsage: -w numPoints X1 Y1 X2 Y2...\n");
}
} else {
printf("Unrecognized option [%s]\n",argv[i]);
return -1;
}
}
//Create thread to monitor robot movement
pthread_create(&thread, NULL, mapRobot, (void *)hands);
// robot is ready
#ifdef DEBUG
printf("All connections established, ready to go\n");
#endif
//Rotate the Servo
turret_SetServo(hands->t, T_ANGLE);
for(i=0; i<FILT_IR_SAMPLES; i++) {
filterIR(hands, filt, &t, &t);
#ifndef USE_IR_MODE
filterSonar(hands, filt, &t, &t);
#endif
}
//Iterate over all of the waypoints
for(i=0; i<numWaypts; i++) {
#ifdef DEBUG
printf("Waypoint %d (%f,%f).\n",i+1,waypoints[i].X,waypoints[i].Y);
#endif
Move(hands,filt,pids,waypoints[i].X,waypoints[i].Y);
#ifdef DEBUG
printf("\nArrived at waypoint %d (%f,%f).\n\n", i+1,waypoints[i].X,waypoints[i].Y);
#endif
}
// Shutdown and tidy up. Close all of the proxy handles
cleanup();
return 0;
}
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;
}
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;
}
}
