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