-
Notifications
You must be signed in to change notification settings - Fork 0
/
hallnavigator.c
193 lines (169 loc) · 5.2 KB
/
hallnavigator.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
//Justin Murray
//Dean Pantages
//Main Hallway Navigator Executable
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <signal.h>
#include <pthread.h>
#include "types.h"
#define FULLCONTROL 1
#include "PIDlib.h"
#include "FIRlib.h"
//Define the default set of waypoints
#define NUM_WAYPOINTS 9
#define WAYPOINT_ARRAY {7.3152,0.0, 7.3152,7.62, 26.21,7.62, 26.21,-3.048, 30.1752,-3.048, 30.1752,-12.192, 7.3152,-12.192, 7.3152,0, 0,0}
//Global handles so they can be used by sighandle
api_HANDLES_t * hands;
FilterHandles_t * filt;
pidHandles_t * pids;
extern roboPos_t * posData;
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);
}
void sighandle(int sig) {
//stop the robot when program dies
printf("Signal caught (%d), shutting down\n",sig);
cleanup();
exit(-2);
}
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;
}