Revision 1301
Control code added, not tested
branches/encoders/code/behaviors/spline/server/internals.h | ||
---|---|---|
1 |
#ifndef INTERNALS_H |
|
2 |
#define INTERNALS_H |
|
3 |
|
|
4 |
#define LS 0 |
|
5 |
#define LD 1 |
|
6 |
#define RS 2 |
|
7 |
#define RD 3 |
|
8 |
|
|
9 |
#define FORWARD 1 |
|
10 |
#define BACKWARD 0 |
|
11 |
#define SENDER 1 |
|
12 |
#define RECEIVER 0 |
|
13 |
|
|
14 |
#define INFINITE -1 |
|
15 |
#define L 10.0 |
|
16 |
|
|
17 |
#define DT .1 |
|
18 |
|
|
19 |
#define DIST(x1, y1, x2, y2) (sqrt(pow(((x1)-(x2)), 2) + pow(((y1)-(y2)), 2))) |
|
20 |
#define LOOK_AHEAD 1 |
|
21 |
#define LOOK_AHEAD_ERR_THRES 0.05 |
|
22 |
#define VELOCITY 1 |
|
23 |
|
|
24 |
typedef struct state |
|
25 |
{ |
|
26 |
double x; |
|
27 |
double y; |
|
28 |
double theta; |
|
29 |
|
|
30 |
short vl; |
|
31 |
short vr; |
|
32 |
|
|
33 |
int vl_ref; |
|
34 |
int vr_ref; |
|
35 |
} state; |
|
36 |
|
|
37 |
typedef struct { |
|
38 |
double *x; |
|
39 |
double *y; |
|
40 |
} curv_t; |
|
41 |
|
|
42 |
state robot; |
|
43 |
|
|
44 |
void updatePosition(); |
|
45 |
|
|
46 |
void setAttriv(int file); |
|
47 |
|
|
48 |
void pos_control_curv(curv_t *ref, int len); |
|
49 |
|
|
50 |
void velocityControl(double*, double*); |
|
51 |
|
|
52 |
|
|
53 |
|
|
54 |
#endif |
branches/encoders/code/behaviors/spline/server/server.c | ||
---|---|---|
5 | 5 |
#include <sys/stat.h> |
6 | 6 |
#include <fcntl.h> |
7 | 7 |
#include "encoders.h" |
8 |
#include "internals.h" |
|
8 | 9 |
#include <math.h> |
9 | 10 |
|
10 |
#define LS 0 |
|
11 |
#define LD 1 |
|
12 |
#define RS 2 |
|
13 |
#define RD 3 |
|
14 |
#define FORWARD 1 |
|
15 |
#define BACKWARD 0 |
|
16 |
#define SENDER 1 |
|
17 |
#define RECEIVER 0 |
|
18 | 11 |
|
19 |
#define INFINITE -1 |
|
20 |
#define L 10.0 |
|
12 |
#define PG 150 |
|
13 |
#define DG 100 |
|
14 |
#define IG 10 |
|
21 | 15 |
|
22 |
double position[3] = {0,0,0}; |
|
16 |
state robot; |
|
17 |
double vel_l_i; |
|
18 |
double vel_r_i; |
|
19 |
double vel_l_old; |
|
20 |
double vel_r_old; |
|
23 | 21 |
|
24 |
void updatePosition(int vl, int vr, double dt); |
|
25 | 22 |
|
23 |
void init(double x, double y, double theta) |
|
24 |
{ |
|
25 |
robot.x = x; |
|
26 |
robot.y = y; |
|
27 |
robot.theta = theta; |
|
28 |
|
|
29 |
robot.vl = 0; |
|
30 |
robot.vr = 0; |
|
31 |
robot.vl_ref = 0; |
|
32 |
robot.vr_ref = 0; |
|
33 |
} |
|
34 |
|
|
26 | 35 |
void setAttrib(int file) |
27 | 36 |
{ |
28 | 37 |
struct termios attributes; |
... | ... | |
73 | 82 |
int tempCount = 190; |
74 | 83 |
|
75 | 84 |
int dx, left_v; |
76 |
|
|
85 |
double Ul, Ur; |
|
86 |
|
|
77 | 87 |
while(1) |
78 | 88 |
{ |
89 |
|
|
79 | 90 |
printf("SENDING DATA\n"); |
80 |
encoders[LS] = tempCount; |
|
81 |
encoders[LD] = 1; |
|
82 |
encoders[RS] = tempCount; |
|
83 |
encoders[RD] = 0; |
|
84 |
//tempCount += 50; |
|
91 |
|
|
92 |
velocityControl(&Ul, &Ur); |
|
93 |
if(Ul > 0) |
|
94 |
encoders[LD] = FORWARD; |
|
95 |
else |
|
96 |
encoders[LD] = BACKWARD; |
|
97 |
encoders[LS] = (unsigned char)abs(Ul); |
|
98 |
|
|
99 |
if(Ur > 0) |
|
100 |
encoders[RD] = FORWARD; |
|
101 |
else |
|
102 |
encoders[RD] = BACKWARD; |
|
103 |
encoders[RS] = (unsigned char)abs(Ur); |
|
104 |
|
|
105 |
|
|
85 | 106 |
int temp = 0; |
86 | 107 |
int count = 0; |
87 | 108 |
do |
88 | 109 |
{ |
89 |
// tcflush(serialFile, TCIOFLUSH); |
|
90 | 110 |
temp = write(serialFileOut, encoders + count, 1); |
91 | 111 |
if(temp < 0) |
92 | 112 |
perror("Write Error"); |
... | ... | |
101 | 121 |
count += read(serialFileIn, encoders, 4); |
102 | 122 |
}while(count < 4); |
103 | 123 |
|
104 |
short leftEncoder = (encoders[0] << 8) | encoders[1]; |
|
105 |
short rightEncoder = (encoders[2] << 8) | encoders[3]; |
|
106 |
printf("%d %d\n", leftEncoder, rightEncoder); |
|
124 |
robot.vl = (encoders[0] << 8) | encoders[1]; |
|
125 |
robot.vr = (encoders[2] << 8) | encoders[3]; |
|
126 |
|
|
127 |
//printf("%d %d\n", robot.vl, robot.vr); |
|
107 | 128 |
|
108 |
updatePosition(leftEncoder, rightEncoder, .1);
|
|
109 |
printf("Location is x: %g \t y: %g \t o: %g\n", position[0], position[1], position[2]);
|
|
129 |
updatePosition(); |
|
130 |
printf("Location is x: %g \t y: %g \t o: %g\n", robot.x,robot.y, robot.theta);
|
|
110 | 131 |
|
111 | 132 |
} |
112 | 133 |
|
113 | 134 |
} |
114 | 135 |
|
115 |
void updatePosition(int vl, int vr, double dt)
|
|
136 |
void scaleInput(double* input)
|
|
116 | 137 |
{ |
117 |
double wdt = (vr - vl)*dt/L; |
|
138 |
if(*input > 0) |
|
139 |
{ |
|
140 |
(*input) += 170; |
|
141 |
if(*input > 210) |
|
142 |
*input = 210; |
|
143 |
} |
|
144 |
else |
|
145 |
{ |
|
146 |
(*input) -= 170; |
|
147 |
if(*input < -210) |
|
148 |
*input = -210; |
|
149 |
} |
|
150 |
} |
|
151 |
|
|
152 |
void velocityControl(double* Ul, double* Ur) |
|
153 |
{ |
|
154 |
double vel_l_e = robot.vl_ref - robot.vl; |
|
155 |
double vel_r_e = robot.vr_ref - robot.vr; |
|
156 |
|
|
157 |
vel_l_i = vel_l_i + vel_l_e; |
|
158 |
vel_r_i = vel_r_i + vel_r_e; |
|
159 |
|
|
160 |
(*Ul) = PG * vel_l_e + DG * (vel_l_e - vel_l_old) + IG * vel_l_i; |
|
161 |
(*Ur) = PG * vel_r_e + DG * (vel_r_e - vel_r_old) + IG * vel_r_i; |
|
162 |
|
|
163 |
scaleInput(Ul); |
|
164 |
scaleInput(Ur); |
|
165 |
|
|
166 |
vel_l_old = vel_l_e; |
|
167 |
vel_l_old = vel_r_e; |
|
168 |
|
|
169 |
} |
|
170 |
|
|
171 |
void updatePosition() |
|
172 |
{ |
|
173 |
double wdt = (robot.vr - robot.vl)*DT/L; |
|
118 | 174 |
double R; |
119 | 175 |
|
120 |
if(vr - vl > 0)
|
|
121 |
R = (L/2)*(vr + vl)/(vr - vl);
|
|
176 |
if(robot.vr - robot.vl > 0)
|
|
177 |
R = (L/2)*(robot.vr + robot.vl)/(robot.vr - robot.vl);
|
|
122 | 178 |
|
123 | 179 |
if(wdt != 0) |
124 | 180 |
{ |
125 |
double ICC[2] = {position[0] - R * sin(position[2]), position[1] + R*cos(position[2])};
|
|
126 |
position[0] = cos(wdt)*(position[0]-ICC[0]) - sin(wdt)*(position[1]-ICC[1]) + ICC[0];
|
|
127 |
position[1] = sin(wdt)*(position[0]-ICC[0]) + cos(wdt)*(position[1]-ICC[1]) + ICC[1];
|
|
128 |
position[2] = position[2] + wdt;
|
|
181 |
double ICC[2] = {robot.x - R * sin(robot.theta), robot.y + R*cos(robot.theta)};
|
|
182 |
robot.x = cos(wdt)*(robot.x-ICC[0]) - sin(wdt)*(robot.y-ICC[1]) + ICC[0];
|
|
183 |
robot.y = sin(wdt)*(robot.x-ICC[0]) + cos(wdt)*(robot.y-ICC[1]) + ICC[1];
|
|
184 |
robot.theta = robot.theta + wdt;
|
|
129 | 185 |
} |
130 | 186 |
else |
131 | 187 |
{ |
132 |
position[0] += cos(position[2])*vr;
|
|
133 |
position[1] += sin(position[2])*vr;
|
|
188 |
robot.x += cos(robot.theta)*robot.vr;
|
|
189 |
robot.y += sin(robot.theta)*robot.vr;
|
|
134 | 190 |
} |
135 | 191 |
} |
192 |
|
|
193 |
|
|
194 |
static int find_goal(curv_t *ref, int len); |
|
195 |
|
|
196 |
void pos_control_curv(curv_t *ref, int len) { |
|
197 |
int goal_idx = find_goal(ref, len); |
|
198 |
//did a transformation here. |
|
199 |
double goal_y = cos(robot.theta)*(ref->y[goal_idx]-robot.y) |
|
200 |
- sin(robot.theta)*(ref->x[goal_idx]-robot.x); |
|
201 |
double goal_rad = 2*goal_y / LOOK_AHEAD; |
|
202 |
|
|
203 |
robot.vl_ref = VELOCITY*(1-0.5*goal_rad); |
|
204 |
robot.vr_ref = VELOCITY*(1+0.5*goal_rad); |
|
205 |
} |
|
206 |
|
|
207 |
static int find_goal(curv_t *ref, int len) { |
|
208 |
int i, goal_idx; |
|
209 |
double min_dist = 100000000; |
|
210 |
double temp; |
|
211 |
int min_idx; |
|
212 |
for (i = 0; i < len; i++) { |
|
213 |
temp = DIST(ref->x[i], ref->y[i], robot.x, robot.y); |
|
214 |
if (temp < min_dist) { |
|
215 |
min_dist = temp; |
|
216 |
min_idx = i; |
|
217 |
} |
|
218 |
} |
|
219 |
goal_idx = -1; |
|
220 |
for (i = min_idx; i < len; i++) { |
|
221 |
temp = DIST(ref->x[i], ref->y[i], robot.x, robot.y); |
|
222 |
if (abs(temp - LOOK_AHEAD) < LOOK_AHEAD_ERR_THRES) { |
|
223 |
goal_idx = i; |
|
224 |
break; |
|
225 |
} |
|
226 |
} |
|
227 |
return goal_idx; |
|
228 |
} |
Also available in: Unified diff