root / branches / encoders / code / behaviors / spline / server / server.c @ 1301
History | View | Annotate | Download (4.59 KB)
1 | 1197 | ayeager | #include <termios.h> |
---|---|---|---|
2 | #include <stdio.h> |
||
3 | #include <stdlib.h> |
||
4 | #include <sys/types.h> |
||
5 | #include <sys/stat.h> |
||
6 | #include <fcntl.h> |
||
7 | 1237 | ayeager | #include "encoders.h" |
8 | 1301 | ayeager | #include "internals.h" |
9 | 1241 | ayeager | #include <math.h> |
10 | 1197 | ayeager | |
11 | 1216 | chihsiuh | |
12 | 1301 | ayeager | #define PG 150 |
13 | #define DG 100 |
||
14 | #define IG 10 |
||
15 | 1237 | ayeager | |
16 | 1301 | ayeager | state robot; |
17 | double vel_l_i;
|
||
18 | double vel_r_i;
|
||
19 | double vel_l_old;
|
||
20 | double vel_r_old;
|
||
21 | 1241 | ayeager | |
22 | |||
23 | 1301 | ayeager | 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 | |||
35 | 1224 | chihsiuh | void setAttrib(int file) |
36 | 1197 | ayeager | { |
37 | 1216 | chihsiuh | struct termios attributes;
|
38 | 1224 | chihsiuh | tcgetattr(file, &attributes); |
39 | 1197 | ayeager | |
40 | 1216 | chihsiuh | cfsetispeed(&attributes, B115200); |
41 | cfsetospeed(&attributes, B115200); |
||
42 | attributes.c_cflag &= ~PARENB; |
||
43 | attributes.c_cflag &= ~CSTOPB; |
||
44 | attributes.c_cflag &= ~CSIZE; |
||
45 | attributes.c_cflag |= CS8; |
||
46 | |||
47 | 1237 | ayeager | attributes.c_cflag &= ~ICRNL; |
48 | attributes.c_cflag &= ~OCRNL; |
||
49 | attributes.c_cflag |= (CLOCAL | CREAD); |
||
50 | attributes.c_lflag &= ~ICANON; |
||
51 | 1216 | chihsiuh | |
52 | |||
53 | 1224 | chihsiuh | if (tcsetattr(file, TCSANOW, &attributes) < 0){ |
54 | 1216 | chihsiuh | perror("tcsetattr failed");
|
55 | exit(-1);
|
||
56 | } |
||
57 | 1224 | chihsiuh | } |
58 | |||
59 | int main()
|
||
60 | { |
||
61 | int serialFileIn = open("/dev/ttyUSB0", O_RDWR); |
||
62 | int serialFileOut = open("/dev/ttyUSB1", O_RDWR); |
||
63 | if(serialFileIn < 1 || serialFileOut < 1) |
||
64 | { |
||
65 | printf("Error opening serial\n");
|
||
66 | return -1; |
||
67 | } |
||
68 | |||
69 | setAttrib(serialFileOut); |
||
70 | setAttrib(serialFileIn); |
||
71 | 1216 | chihsiuh | |
72 | unsigned char encoders[4] = {1,1,1,1}; |
||
73 | 1224 | chihsiuh | |
74 | char senderNum = SENDER;
|
||
75 | char receiverNum = RECEIVER;
|
||
76 | |||
77 | write(serialFileIn, &receiverNum, 1);
|
||
78 | sleep(1);
|
||
79 | write(serialFileOut, &senderNum, 1);
|
||
80 | |||
81 | 1216 | chihsiuh | //Sending velocity as LS LD RS RD
|
82 | 1239 | chihsiuh | int tempCount = 190; |
83 | 1237 | ayeager | |
84 | int dx, left_v;
|
||
85 | 1301 | ayeager | double Ul, Ur;
|
86 | |||
87 | 1197 | ayeager | while(1) |
88 | { |
||
89 | 1301 | ayeager | |
90 | 1216 | chihsiuh | printf("SENDING DATA\n");
|
91 | 1301 | ayeager | |
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 | |||
106 | 1216 | chihsiuh | int temp = 0; |
107 | int count = 0; |
||
108 | do
|
||
109 | { |
||
110 | 1224 | chihsiuh | temp = write(serialFileOut, encoders + count, 1);
|
111 | 1216 | chihsiuh | if(temp < 0) |
112 | perror("Write Error");
|
||
113 | count += temp; |
||
114 | 1222 | chihsiuh | printf("sent: %d\n", count);
|
115 | 1239 | chihsiuh | usleep(200);
|
116 | 1216 | chihsiuh | }while(count < 4); |
117 | count = 0;
|
||
118 | printf("READING DATA\n");
|
||
119 | do
|
||
120 | { |
||
121 | 1224 | chihsiuh | count += read(serialFileIn, encoders, 4);
|
122 | 1216 | chihsiuh | }while(count < 4); |
123 | |||
124 | 1301 | ayeager | 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);
|
||
128 | 1237 | ayeager | |
129 | 1301 | ayeager | updatePosition(); |
130 | printf("Location is x: %g \t y: %g \t o: %g\n", robot.x,robot.y, robot.theta);
|
||
131 | 1237 | ayeager | |
132 | 1241 | ayeager | } |
133 | 1237 | ayeager | |
134 | 1241 | ayeager | } |
135 | 1237 | ayeager | |
136 | 1301 | ayeager | void scaleInput(double* input) |
137 | 1241 | ayeager | { |
138 | 1301 | ayeager | 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;
|
||
174 | 1241 | ayeager | double R;
|
175 | 1237 | ayeager | |
176 | 1301 | ayeager | if(robot.vr - robot.vl > 0) |
177 | R = (L/2)*(robot.vr + robot.vl)/(robot.vr - robot.vl);
|
||
178 | 1241 | ayeager | |
179 | if(wdt != 0) |
||
180 | { |
||
181 | 1301 | ayeager | 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; |
||
185 | 1197 | ayeager | } |
186 | 1241 | ayeager | else
|
187 | { |
||
188 | 1301 | ayeager | robot.x += cos(robot.theta)*robot.vr; |
189 | robot.y += sin(robot.theta)*robot.vr; |
||
190 | 1241 | ayeager | } |
191 | 1197 | ayeager | } |
192 | 1301 | ayeager | |
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 | } |