Project

General

Profile

Revision 1342

sort of works, does the 45 line correctly.

View differences:

server.c
7 7
#include "encoders.h"
8 8
#include "internals.h"
9 9
#include <math.h>
10
#include "speed_map.h"
10 11

  
12
#include <time.h>
11 13

  
12
#define PG 150
13
#define DG 100
14
#define IG 10
14
#define PG 8
15
#define DG .1
16
#define IG .1
15 17

  
18
int VEL_S = 1;
19

  
20

  
21
#define TICKS_PER_CM 48.03
22
#define TIME_INTERVAL 1520
23

  
24
#define LEN 30000
25

  
26

  
16 27
state robot;      
17 28
double vel_l_i;
18 29
double vel_r_i;
......
27 38
	robot.theta = theta;
28 39

  
29 40
	robot.vl = 0;
30
	robot.vr = 0;
41
	robot.vr = 0;     
31 42
	robot.vl_ref = 0;
32 43
	robot.vr_ref = 0;
33 44
}
......
42 53
	attributes.c_cflag &= ~PARENB;
43 54
	attributes.c_cflag &= ~CSTOPB;
44 55
	attributes.c_cflag &= ~CSIZE;
45
	attributes.c_cflag |= CS8;
56
	attributes.c_cflag |= 	CS8;
46 57
	
47 58
	attributes.c_cflag &= ~ICRNL;
48 59
	attributes.c_cflag &= ~OCRNL;
......
56 67
	}
57 68
}
58 69

  
59
int main()
70

  
71
int main(int argc, char** args)
60 72
{
73
    init(50, 0, M_PI/2);
74
	//init(0,0,0);
75
	double x_ref[LEN];
76
	double y_ref[LEN];
77
    int i;
78
	for (i = 0; i < LEN; i++)  {
79
    	x_ref[i] = 50 * cos((double)(i+1) / LEN * M_PI);
80
    	y_ref[i] = 50 * sin((double)(i+1) / LEN * M_PI);
81
   // 	x_ref[i] = (double)i / LEN * 100;
82
   //` 	y_ref[i] = (double)i / LEN * 100;
83
		//printf("x_ref %g \t y_ref %g\n", x_ref[i], y_ref[i]);
84
		//getchar();
85
	}
86
	
87
	curv_t ref;                         
88
	ref.x = x_ref;
89
	ref.y = y_ref;
90
	
61 91
	int serialFileIn = open("/dev/ttyUSB0", O_RDWR);
62 92
	int serialFileOut = open("/dev/ttyUSB1", O_RDWR);
63 93
	if(serialFileIn < 1 || serialFileOut < 1)
......
68 98
    	
69 99
	setAttrib(serialFileOut);
70 100
	setAttrib(serialFileIn);
71
        
72
	unsigned char encoders[4] = {1,1,1,1};
73
	
101
                                           
102
	//unsigned char encoders[4] = {0,0,0,0};
103
	char ref_vels[2] = {0,0};
104

  
74 105
	char senderNum = SENDER;
75 106
	char receiverNum = RECEIVER;
76 107
	
......
79 110
	write(serialFileOut, &senderNum, 1);
80 111
	
81 112
	//Sending velocity as LS LD RS RD
82
	int tempCount = 190;
113
	int tempCount = 0;
83 114

  
84
	int dx, left_v;
115
	int dx;
85 116
	double Ul, Ur;
86 117

  
87
	while(1)
88
	{
118
	while(1) {
89 119

  
90
		printf("SENDING DATA\n");
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);
120
        pos_control_curve(&ref, LEN);
121
	    
122
//		ref_vels[LEFT] = 20;
123
//		ref_vels[RIGHT] = 20;
98 124
		
99
        if(Ur > 0)
100
			encoders[RD] = FORWARD;
101
		else
102
			encoders[RD] = BACKWARD;
103
		encoders[RS] = (unsigned char)abs(Ur);
125
		ref_vels[LEFT] = -(char)robot_velocity(1, robot.vl_ref);
126
		ref_vels[RIGHT] = -(char)robot_velocity(1, robot.vr_ref);
127
	
128
//        printf("Ref Vs: \t\t%d\t%d\n", ref_vels[LEFT], ref_vels[RIGHT]);
129
       // printf("Left Speed: %d \t Right Speed: %d\n", encoders[LS], encoders[RS]);
104 130

  
105

  
131
		//tempCount++;
132
		//printf("RECEIVED %d\n", tempCount);
106 133
		int temp = 0;
107 134
		int count = 0;
108 135
		do
109 136
		{
110
			temp = write(serialFileOut, encoders + count, 1);	
137
			temp = write(serialFileOut, ref_vels + count, 1);	
111 138
			if(temp < 0)
112 139
				perror("Write Error");
113 140
			count += temp;
114
			printf("sent: %d\n", count);
115
			usleep(200);
116
		}while(count < 4);
141
			//printf("sent: %d\n", count);
142
			usleep(10000);
143
		   // sleep(1);
144
		} while(count < 2);
145
		
117 146
		count = 0;
118
		printf("READING DATA\n");
147
		//printf("READING DATA\n");
119 148
		do
120 149
		{
121
			count += read(serialFileIn, encoders, 4);
122
		}while(count < 4);
150
			count += read(serialFileIn, ref_vels, 2);
151
		}while(count < 2);                         
123 152
		
124
		robot.vl = (encoders[0] << 8) | encoders[1];
125
		robot.vr = (encoders[2] << 8) | encoders[3];
126 153
		
127
		//printf("%d %d\n", robot.vl, robot.vr);
154
		robot.vl = real_velocity(1, -ref_vels[LEFT]);
155
		robot.vr = real_velocity(1, -ref_vels[RIGHT]);
156
		
157
  //  	printf("ACTUAL V \t\t%g\t%g\n", robot.vl, robot.vr);
128 158

  
129 159
		updatePosition();
130
		printf("Location is x: %g \t y: %g \t o: %g\n", robot.x,robot.y, robot.theta);
131

  
160
		//printf("Raw Location is x: %g \t y: %g \t o: %g\n", robot.x,robot.y, robot.theta);
161
          printf("%g\t%g\n", robot.x, robot.y);
132 162
	}
163
    
164
	return 0;
133 165

  
134 166
}
135 167

  
136 168
void scaleInput(double* input)
137 169
{
170
	printf("INPUT: %g\n", *input);
138 171
  	if(*input > 0)
139 172
	{
140
		(*input) += 170;
173
		//(*input) += 170;
141 174
		if(*input > 210)
142 175
			*input = 210;
143 176
	}
144
	else
177
	else if((*input) < 0)
145 178
	{
146
		(*input) -= 170;
179
		//(*input) -= 170;
147 180
		if(*input < -210)
148 181
			*input = -210;
149 182
	}
150 183
}
151 184

  
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

  
185
//Todo: This makes no sense, 4am
171 186
void updatePosition()
172 187
{
173 188
   	double wdt = (robot.vr - robot.vl)*DT/L;
174 189
   	double R;           
175 190

  
176
   	if(robot.vr - robot.vl > 0)
191
   	if(robot.vr - robot.vl != 0)
177 192
   		R = (L/2)*(robot.vr + robot.vl)/(robot.vr - robot.vl);
178 193
    
179 194
	if(wdt != 0)
180 195
	{
181 196
		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];
197
		robot.x = (cos(wdt)*(robot.x-ICC[0]) - sin(wdt)*(robot.y-ICC[1]) + ICC[0]);
198
		robot.y = (sin(wdt)*(robot.x-ICC[0]) + cos(wdt)*(robot.y-ICC[1]) + ICC[1]);
184 199
		robot.theta = robot.theta + wdt;  
185 200
	}
186 201
	else
187 202
	{
188
	   	robot.x += cos(robot.theta)*robot.vr;
189
		robot.y += sin(robot.theta)*robot.vr;
203
	   	robot.x += cos(robot.theta)*robot.vr*DT;
204
		robot.y += sin(robot.theta)*robot.vr*DT;
190 205
	}
206
	if (robot.theta > 2*M_PI)
207
		robot.theta -= 2*M_PI;
208
	if (robot.theta < -2*M_PI)
209
		robot.theta += 2*M_PI;
191 210
}
192 211

  
193 212

  
194 213
static int find_goal(curv_t *ref, int len);
195

  
196
void pos_control_curv(curv_t *ref, int len) {
214
    
215
void pos_control_curve(curv_t *ref, int len) {
197 216
    int goal_idx = find_goal(ref, len);
198 217
    //did a transformation here.
199 218
    double goal_y = cos(robot.theta)*(ref->y[goal_idx]-robot.y) 
200 219
        - sin(robot.theta)*(ref->x[goal_idx]-robot.x);
201 220
    double goal_rad = 2*goal_y / LOOK_AHEAD;
202 221
    
203
    robot.vl_ref = VELOCITY*(1-0.5*goal_rad);
204
    robot.vr_ref = VELOCITY*(1+0.5*goal_rad);
222
    //printf("GOAL: y: %g,  o: %g\n", goal_y, goal_rad);
223

  
224
    robot.vl_ref = real_velocity(1, VELOCITY)*(1-0.5*goal_rad);
225
    robot.vr_ref = real_velocity(1, VELOCITY)*(1+0.5*goal_rad);
226
    /*
227
	if(robot.vl_ref > 127)
228
		robot.vl_ref = 127;
229
	else if(robot.vl_ref < -128)
230
		robot.vl_ref = -128;
231
	if(robot.vr_ref > 127)
232
		robot.vr_ref = 127;
233
	else if(robot.vr_ref < -128)
234
		robot.vr_ref = -128;
235
		*/
236

  
205 237
}
206 238

  
207 239
static int find_goal(curv_t *ref, int len) {
......
216 248
            min_idx = i;
217 249
        }
218 250
    }
251
	//printf("%d min %f\n", min_idx, min_dist);
252
	//getchar();
219 253
    goal_idx = -1;
220 254
    for (i = min_idx; i < len; i++) {
221 255
        temp = DIST(ref->x[i], ref->y[i], robot.x, robot.y);
222 256
        if (abs(temp - LOOK_AHEAD) < LOOK_AHEAD_ERR_THRES) {
223 257
            goal_idx = i;
224
            break;
225
        }
258
        	return goal_idx;
259
		}
226 260
    }
261
	while(1) {
262
		printf("eol\n");
263
	    exit(0);
264
	}
227 265
    return goal_idx;
228
}             
266
} 
267

  

Also available in: Unified diff