Revision 1241
Server code does forward kinematic calculations
server.c | ||
---|---|---|
5 | 5 |
#include <sys/stat.h> |
6 | 6 |
#include <fcntl.h> |
7 | 7 |
#include "encoders.h" |
8 |
#include <math.h> |
|
8 | 9 |
|
9 | 10 |
#define LS 0 |
10 | 11 |
#define LD 1 |
... | ... | |
15 | 16 |
#define SENDER 1 |
16 | 17 |
#define RECEIVER 0 |
17 | 18 |
|
18 |
int left_dx = 0;
|
|
19 |
int right_dx = 0;
|
|
19 |
#define INFINITE -1
|
|
20 |
#define L 10.0
|
|
20 | 21 |
|
22 |
double position[3] = {0,0,0}; |
|
23 |
|
|
24 |
void updatePosition(int vl, int vr, double dt); |
|
25 |
|
|
21 | 26 |
void setAttrib(int file) |
22 | 27 |
{ |
23 | 28 |
struct termios attributes; |
... | ... | |
73 | 78 |
{ |
74 | 79 |
printf("SENDING DATA\n"); |
75 | 80 |
encoders[LS] = tempCount; |
76 |
encoders[LD] = 0;
|
|
81 |
encoders[LD] = 1;
|
|
77 | 82 |
encoders[RS] = tempCount; |
78 | 83 |
encoders[RD] = 0; |
79 | 84 |
//tempCount += 50; |
... | ... | |
100 | 105 |
short rightEncoder = (encoders[2] << 8) | encoders[3]; |
101 | 106 |
printf("%d %d\n", leftEncoder, rightEncoder); |
102 | 107 |
|
103 |
// encoder_recv(leftEncoder, rightEncoder);
|
|
104 |
// printf("LEFT V: %d \t RIGHT V: %d\n", encoder_get_v(LEFT), encoder_get_v(RIGHT));
|
|
108 |
updatePosition(leftEncoder, rightEncoder, .1);
|
|
109 |
printf("Location is x: %g \t y: %g \t o: %g\n", position[0], position[1], position[2]);
|
|
105 | 110 |
|
111 |
} |
|
106 | 112 |
|
113 |
} |
|
107 | 114 |
|
115 |
void updatePosition(int vl, int vr, double dt) |
|
116 |
{ |
|
117 |
double wdt = (vr - vl)*dt/L; |
|
118 |
double R; |
|
108 | 119 |
|
120 |
if(vr - vl > 0) |
|
121 |
R = (L/2)*(vr + vl)/(vr - vl); |
|
122 |
|
|
123 |
if(wdt != 0) |
|
124 |
{ |
|
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; |
|
109 | 129 |
} |
110 |
|
|
130 |
else |
|
131 |
{ |
|
132 |
position[0] += cos(position[2])*vr; |
|
133 |
position[1] += sin(position[2])*vr; |
|
134 |
} |
|
111 | 135 |
} |
Also available in: Unified diff