Project

General

Profile

Revision 1301

Control code added, not tested

View differences:

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