Project

General

Profile

Revision 1343

WORKING

View differences:

branches/encoders/code/behaviors/spline/server/internals.h
6 6
#define RS 2
7 7
#define RD 3
8 8

  
9
#define LEFT 0
10
#define RIGHT 1
9
#define LEFT 1
10
#define RIGHT 0
11 11

  
12 12
#define FORWARD 1
13 13
#define BACKWARD 0
......
17 17
#define INFINITE -1
18 18
#define L 13.5
19 19

  
20
#define DT .102
20
//#define DT .102
21
#define DT .088
21 22

  
22 23
#define DIST(x1, y1, x2, y2)    (sqrt(((x1)-(x2))*((x1)-(x2)) + ((y1)-(y2))*((y1)-(y2))))
23
#define LOOK_AHEAD              30
24
#define LOOK_AHEAD              40
24 25
#define LOOK_AHEAD_ERR_THRES    5
25
#define VELOCITY                20
26
#define VELOCITY                10
26 27

  
27 28
typedef struct state
28 29
{
branches/encoders/code/behaviors/spline/server/server.c
4 4
#include <sys/types.h>
5 5
#include <sys/stat.h>
6 6
#include <fcntl.h>
7
#include "encoders.h"
8 7
#include "internals.h"
9 8
#include <math.h>
10 9
#include "speed_map.h"
......
67 66
	}
68 67
}
69 68

  
70

  
69
#define RADIUS          100
71 70
int main(int argc, char** args)
72 71
{
73
    init(50, 0, M_PI/2);
72
    //init(RADIUS, 0, M_PI/2);
73
	init(0, 50, 0);
74 74
	//init(0,0,0);
75 75
	double x_ref[LEN];
76 76
	double y_ref[LEN];
77 77
    int i;
78 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);
79
//        x_ref[i] = RADIUS * cos((double)(i+1) / LEN * 5.0 / 2.0 * M_PI);
80
//        y_ref[i] = RADIUS * sin((double)(i+1) / LEN * 5.0 / 2.0 * M_PI);
81
 
82
		x_ref[i] = ((double)(i+1) / LEN * 2.0 * M_PI) * 100;
83
    	y_ref[i] = 50 *cos (x_ref[i] / 16.6667);//(double)(i+1) / LEN * 3.0 / 2.0 * M_PI);
81 84
   // 	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]);
85
   // 	y_ref[i] = (double)i / LEN * 100;
86
        //printf("%g \t%g\n", x_ref[i], y_ref[i]);
84 87
		//getchar();
85 88
	}
89

  
90
	//printf("aaaaaaa\n");
86 91
	
87 92
	curv_t ref;                         
88 93
	ref.x = x_ref;
......
119 124

  
120 125
        pos_control_curve(&ref, LEN);
121 126
	    
122
//		ref_vels[LEFT] = 20;
123
//		ref_vels[RIGHT] = 20;
127
  //  	ref_vels[LEFT] = 20;
128
  //  	ref_vels[RIGHT] = 20;
124 129
		
125 130
		ref_vels[LEFT] = -(char)robot_velocity(1, robot.vl_ref);
126 131
		ref_vels[RIGHT] = -(char)robot_velocity(1, robot.vr_ref);
127 132
	
128
//        printf("Ref Vs: \t\t%d\t%d\n", ref_vels[LEFT], ref_vels[RIGHT]);
133
        //printf("Ref Vs: \t\t%d\t%d\n", ref_vels[LEFT], ref_vels[RIGHT]);
129 134
       // printf("Left Speed: %d \t Right Speed: %d\n", encoders[LS], encoders[RS]);
130 135

  
131 136
		//tempCount++;
......
154 159
		robot.vl = real_velocity(1, -ref_vels[LEFT]);
155 160
		robot.vr = real_velocity(1, -ref_vels[RIGHT]);
156 161
		
157
  //  	printf("ACTUAL V \t\t%g\t%g\n", robot.vl, robot.vr);
162
   // 	printf("ACTUAL V \t\t%g\t%g\n", robot.vl, robot.vr);
158 163

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

  
166 168
}
167 169

  
168
void scaleInput(double* input)
169
{
170
	printf("INPUT: %g\n", *input);
171
  	if(*input > 0)
172
	{
173
		//(*input) += 170;
174
		if(*input > 210)
175
			*input = 210;
176
	}
177
	else if((*input) < 0)
178
	{
179
		//(*input) -= 170;
180
		if(*input < -210)
181
			*input = -210;
182
	}
183
}
184

  
185 170
//Todo: This makes no sense, 4am
186 171
void updatePosition()
187 172
{
......
239 224
static int find_goal(curv_t *ref, int len) {
240 225
    int i, goal_idx;
241 226
    double min_dist = 100000000;
242
    double temp;
227
    double temp, err;
243 228
    int min_idx;
229
	int ctr = 0;
244 230
    for (i = 0; i < len; i++) {
245 231
        temp = DIST(ref->x[i], ref->y[i], robot.x, robot.y);
246 232
        if (temp < min_dist) {
......
251 237
	//printf("%d min %f\n", min_idx, min_dist);
252 238
	//getchar();
253 239
    goal_idx = -1;
254
    for (i = min_idx; i < len; i++) {
255
        temp = DIST(ref->x[i], ref->y[i], robot.x, robot.y);
256
        if (abs(temp - LOOK_AHEAD) < LOOK_AHEAD_ERR_THRES) {
257
            goal_idx = i;
258
        	return goal_idx;
259
		}
260
    }
240
	err = LOOK_AHEAD_ERR_THRES;
241
	while(goal_idx == -1 && ctr < 3) {
242
	    for (i = min_idx; i < len; i++) {
243
    	    temp = DIST(ref->x[i], ref->y[i], robot.x, robot.y);
244
        	if (abs(temp - LOOK_AHEAD) < err) {
245
            	goal_idx = i;
246
	        	return goal_idx;
247
			}
248
	    }
249
		err += 5; 
250
		ctr++;
251
	}
261 252
	while(1) {
262
		printf("eol\n");
253
		//printf("eol\n");
263 254
	    exit(0);
264 255
	}
265 256
    return goal_idx;

Also available in: Unified diff