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