Project

General

Profile

Statistics
| Revision:

root / branches / encoders / code / behaviors / spline / server / server.c @ 1344

History | View | Annotate | Download (6.67 KB)

1
#include <termios.h>
2
#include <stdio.h>
3
#include <stdlib.h>
4
#include <sys/types.h>
5
#include <sys/stat.h>
6
#include <fcntl.h>
7
#include "internals.h"
8
#include <math.h>
9
#include "speed_map.h"
10

    
11
#include <time.h>
12

    
13
#define PG 8
14
#define DG .1
15
#define IG .1
16

    
17
int VEL_S = 1;
18

    
19

    
20
#define TICKS_PER_CM 48.03
21
#define TIME_INTERVAL 1520
22

    
23
#define LEN 30000
24

    
25

    
26
state robot;      
27
double vel_l_i;
28
double vel_r_i;
29
double vel_l_old;
30
double vel_r_old;
31

    
32

    
33
void init(double x, double y, double theta)
34
{
35
        robot.x = x;
36
        robot.y = y;
37
        robot.theta = theta;
38

    
39
        robot.vl = 0;
40
        robot.vr = 0;     
41
        robot.vl_ref = 0;
42
        robot.vr_ref = 0;
43
}
44

    
45
void setAttrib(int file)
46
{
47
        struct termios attributes;
48
            tcgetattr(file, &attributes);
49

    
50
        cfsetispeed(&attributes, B115200);
51
        cfsetospeed(&attributes, B115200);
52
        attributes.c_cflag &= ~PARENB;
53
        attributes.c_cflag &= ~CSTOPB;
54
        attributes.c_cflag &= ~CSIZE;
55
        attributes.c_cflag |=         CS8;
56
        
57
        attributes.c_cflag &= ~ICRNL;
58
        attributes.c_cflag &= ~OCRNL;
59
        attributes.c_cflag |= (CLOCAL | CREAD);
60
        attributes.c_lflag &= ~ICANON;
61

    
62

    
63
        if (tcsetattr(file, TCSANOW, &attributes) < 0){
64
                perror("tcsetattr failed");
65
                exit(-1);
66
        }
67
}
68

    
69
//#define FORWARD
70

    
71
#ifdef FORWARD
72
#undef LEFT
73
#undef RIGHT
74
#define LEFT 0
75
#define RIGHT 1
76
#endif
77

    
78
#define CIRCLE        
79
//#define SINE
80
//#define COSINE
81
//#define LINE45
82
//#define LINE
83

    
84
#define RADIUS          70
85
#define AMP                                70
86

    
87
int main(int argc, char** args)
88
{
89
#ifdef CIRCLE
90
    init(20, 30, 0);
91
#endif
92
#ifdef SINE
93
        init(0, 0, M_PI/4);
94
#endif
95
#ifdef COSINE
96
        init(0, AMP, 0);
97
#endif
98
#ifdef LINE45
99
        init(0,0,0);
100
#endif
101
#ifdef LINE
102
        init(0,0,0);
103
#endif
104
        double x_ref[LEN];
105
        double y_ref[LEN];
106
    int i;
107
        for (i = 0; i < LEN; i++)  {
108
#ifdef CIRCLE
109
              x_ref[i] = RADIUS * cos((double)(i) / LEN * 2.0 * M_PI);
110
        y_ref[i] = RADIUS * sin((double)(i) / LEN * 2.0 * M_PI);
111
#endif
112
#ifdef SINE
113
                x_ref[i] = ((double)(i+1) / LEN * 2.0 * M_PI) * 100;
114
            y_ref[i] = AMP *sin (x_ref[i] / 50);
115
#endif                 
116
#ifdef COSINE
117
                x_ref[i] = ((double)(i+1) / LEN * 2.0 * M_PI) * 100;
118
            y_ref[i] = AMP *cos (x_ref[i] / 50);
119
#endif                 
120
#ifdef LINE45
121
            x_ref[i] = (double)i / LEN * 1000;
122
            y_ref[i] = (double)i / LEN * 1000;
123
#endif
124
#ifdef LINE
125
            x_ref[i] = (double)i / LEN * 1000;
126
            y_ref[i] = 0;
127
#endif
128

    
129
        //printf("%g \t%g\n", x_ref[i], y_ref[i]);
130
                //getchar();
131
        }
132

    
133
        //printf("aaaaaaa\n");
134
        
135
        curv_t ref;                         
136
        ref.x = x_ref;
137
        ref.y = y_ref;
138
        
139
        int serialFileIn = open("/dev/ttyUSB0", O_RDWR);
140
        int serialFileOut = open("/dev/ttyUSB1", O_RDWR);
141
        if(serialFileIn < 1 || serialFileOut < 1)
142
        {
143
                printf("Error opening serial\n");
144
                return -1;
145
        }
146
            
147
        setAttrib(serialFileOut);
148
        setAttrib(serialFileIn);
149
                                           
150
        //unsigned char encoders[4] = {0,0,0,0};
151
        char ref_vels[2] = {0,0};
152

    
153
        char senderNum = SENDER;
154
        char receiverNum = RECEIVER;
155
        
156
        write(serialFileIn, &receiverNum, 1);
157
        sleep(1);
158
        write(serialFileOut, &senderNum, 1);
159
        
160
        //Sending velocity as LS LD RS RD
161
        int tempCount = 0;
162

    
163
        int dx;
164
        double Ul, Ur;
165

    
166
        while(1) {
167

    
168
        pos_control_curve(&ref, LEN);
169
            
170
  //          ref_vels[LEFT] = 20;
171
  //          ref_vels[RIGHT] = 20;
172
                
173

    
174
                ref_vels[LEFT] = -(char)robot_velocity(1, robot.vl_ref);
175
                ref_vels[RIGHT] = -(char)robot_velocity(1, robot.vr_ref);
176
        #ifdef FORWARD
177
                ref_vels[LEFT] = -ref_vels[LEFT];
178
                ref_vels[RIGHT] = -ref_vels[RIGHT];
179
        #endif
180

    
181
        //printf("Ref Vs: \t\t%d\t%d\n", ref_vels[LEFT], ref_vels[RIGHT]);
182
       // printf("Left Speed: %d \t Right Speed: %d\n", encoders[LS], encoders[RS]);
183

    
184
                //tempCount++;
185
                //printf("RECEIVED %d\n", tempCount);
186
                int temp = 0;
187
                int count = 0;
188
                do
189
                {
190
                        temp = write(serialFileOut, ref_vels + count, 1);        
191
                        if(temp < 0)
192
                                perror("Write Error");
193
                        count += temp;
194
                        //printf("sent: %d\n", count);
195
                        usleep(10000);
196
                   // sleep(1);
197
                } while(count < 2);
198
                
199
                count = 0;
200
                //printf("READING DATA\n");
201
                do
202
                {
203
                        count += read(serialFileIn, ref_vels, 2);
204
                }while(count < 2);                         
205
                
206
                
207
#ifndef FORWARD
208
                robot.vl = real_velocity(1, -ref_vels[LEFT]);
209
                robot.vr = real_velocity(1, -ref_vels[RIGHT]);
210
#else
211
        robot.vl = real_velocity(1, ref_vels[LEFT]);
212
                robot.vr = real_velocity(1, ref_vels[RIGHT]);            
213
#endif
214
        
215
            //printf("ACTUAL V \t\t%g\t%g\n", robot.vl, robot.vr);
216

    
217
                updatePosition();
218
                //printf("Raw Location is x: %g \t y: %g \t o: %g\n", robot.x,robot.y, robot.theta);
219
          printf("%g\t%g\n", robot.x, robot.y);
220
        }
221
}
222

    
223
//Todo: This makes no sense, 4am
224
void updatePosition()
225
{
226
           double wdt = (robot.vr - robot.vl)*DT/L;
227
           double R;           
228

    
229
           if(robot.vr - robot.vl != 0)
230
                   R = (L/2)*(robot.vr + robot.vl)/(robot.vr - robot.vl);
231
    
232
        if(wdt != 0)
233
        {
234
                double ICC[2] = {robot.x - R * sin(robot.theta), robot.y + R*cos(robot.theta)};
235
                robot.x = (cos(wdt)*(robot.x-ICC[0]) - sin(wdt)*(robot.y-ICC[1]) + ICC[0]);
236
                robot.y = (sin(wdt)*(robot.x-ICC[0]) + cos(wdt)*(robot.y-ICC[1]) + ICC[1]);
237
                robot.theta = robot.theta + wdt;  
238
        }
239
        else
240
        {
241
                   robot.x += cos(robot.theta)*robot.vr*DT;
242
                robot.y += sin(robot.theta)*robot.vr*DT;
243
        }
244
        if (robot.theta > 2*M_PI)
245
                robot.theta -= 2*M_PI;
246
        if (robot.theta < -2*M_PI)
247
                robot.theta += 2*M_PI;
248
}
249

    
250

    
251
static int find_goal(curv_t *ref, int len);
252
    
253
void pos_control_curve(curv_t *ref, int len) {
254
    int goal_idx = find_goal(ref, len);
255
    //printf("\tGOAL: x: %g,  y: %g\n", ref->x[goal_idx], ref->y[goal_idx]);
256
    //did a transformation here.
257
    double goal_y = cos(robot.theta)*(ref->y[goal_idx]-robot.y) 
258
        - sin(robot.theta)*(ref->x[goal_idx]-robot.x);
259
    double goal_rad = 2*goal_y / LOOK_AHEAD;
260
    
261

    
262
    robot.vl_ref = real_velocity(1, VELOCITY)*(1-0.5*goal_rad);
263
    robot.vr_ref = real_velocity(1, VELOCITY)*(1+0.5*goal_rad);
264
    /*
265
        if(robot.vl_ref > 127)
266
                robot.vl_ref = 127;
267
        else if(robot.vl_ref < -128)
268
                robot.vl_ref = -128;
269
        if(robot.vr_ref > 127)
270
                robot.vr_ref = 127;
271
        else if(robot.vr_ref < -128)
272
                robot.vr_ref = -128;
273
                */
274

    
275
}
276

    
277
static int find_goal(curv_t *ref, int len) {
278
    int i, goal_idx;
279
    double min_dist = 100000000;
280
    double temp, err;
281
    int min_idx;
282
        int ctr = 0;
283
    for (i = 0; i < len; i++) {
284
        temp = DIST(ref->x[i], ref->y[i], robot.x, robot.y);
285
        if (temp < min_dist) {
286
            min_dist = temp;
287
            min_idx = i;
288
        }
289
    }
290
        /*
291
    if (min_dist > LOOK_AHEAD) {
292
                printf("toooooo far away \n");
293
                return min_idx;
294
        }
295
    */
296
        //printf("%d min %f\n", min_idx, min_dist);
297
        //getchar();
298
    goal_idx = -1;
299
        err = LOOK_AHEAD_ERR_THRES;
300
        while(ctr < 3) {
301
            for (i = min_idx; i < len; i++) {
302
#ifdef CIRCLE
303
                        if (i == len-1)
304
                                i = 0;
305
#endif
306
                temp = DIST(ref->x[i], ref->y[i], robot.x, robot.y);
307
                if (abs(temp - LOOK_AHEAD) < err) {
308
                    goal_idx = i;
309
                        return goal_idx;
310
                        }
311
            }
312
                err += 5; 
313
                ctr++;
314
        }
315
        while(1) {
316
                printf("eol\n");
317
            exit(0);
318
        }
319
    return goal_idx;
320
} 
321