Project

General

Profile

Statistics
| Revision:

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

History | View | Annotate | Download (5.71 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 "encoders.h"
8
#include "internals.h"
9
#include <math.h>
10
#include "speed_map.h"
11

    
12
#include <time.h>
13

    
14
#define PG 8
15
#define DG .1
16
#define IG .1
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

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

    
33

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

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

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

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

    
63

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

    
70

    
71
int main(int argc, char** args)
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
        
91
        int serialFileIn = open("/dev/ttyUSB0", O_RDWR);
92
        int serialFileOut = open("/dev/ttyUSB1", O_RDWR);
93
        if(serialFileIn < 1 || serialFileOut < 1)
94
        {
95
                printf("Error opening serial\n");
96
                return -1;
97
        }
98
            
99
        setAttrib(serialFileOut);
100
        setAttrib(serialFileIn);
101
                                           
102
        //unsigned char encoders[4] = {0,0,0,0};
103
        char ref_vels[2] = {0,0};
104

    
105
        char senderNum = SENDER;
106
        char receiverNum = RECEIVER;
107
        
108
        write(serialFileIn, &receiverNum, 1);
109
        sleep(1);
110
        write(serialFileOut, &senderNum, 1);
111
        
112
        //Sending velocity as LS LD RS RD
113
        int tempCount = 0;
114

    
115
        int dx;
116
        double Ul, Ur;
117

    
118
        while(1) {
119

    
120
        pos_control_curve(&ref, LEN);
121
            
122
//                ref_vels[LEFT] = 20;
123
//                ref_vels[RIGHT] = 20;
124
                
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]);
130

    
131
                //tempCount++;
132
                //printf("RECEIVED %d\n", tempCount);
133
                int temp = 0;
134
                int count = 0;
135
                do
136
                {
137
                        temp = write(serialFileOut, ref_vels + count, 1);        
138
                        if(temp < 0)
139
                                perror("Write Error");
140
                        count += temp;
141
                        //printf("sent: %d\n", count);
142
                        usleep(10000);
143
                   // sleep(1);
144
                } while(count < 2);
145
                
146
                count = 0;
147
                //printf("READING DATA\n");
148
                do
149
                {
150
                        count += read(serialFileIn, ref_vels, 2);
151
                }while(count < 2);                         
152
                
153
                
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);
158

    
159
                updatePosition();
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);
162
        }
163
    
164
        return 0;
165

    
166
}
167

    
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
//Todo: This makes no sense, 4am
186
void updatePosition()
187
{
188
           double wdt = (robot.vr - robot.vl)*DT/L;
189
           double R;           
190

    
191
           if(robot.vr - robot.vl != 0)
192
                   R = (L/2)*(robot.vr + robot.vl)/(robot.vr - robot.vl);
193
    
194
        if(wdt != 0)
195
        {
196
                double ICC[2] = {robot.x - R * sin(robot.theta), robot.y + R*cos(robot.theta)};
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]);
199
                robot.theta = robot.theta + wdt;  
200
        }
201
        else
202
        {
203
                   robot.x += cos(robot.theta)*robot.vr*DT;
204
                robot.y += sin(robot.theta)*robot.vr*DT;
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;
210
}
211

    
212

    
213
static int find_goal(curv_t *ref, int len);
214
    
215
void pos_control_curve(curv_t *ref, int len) {
216
    int goal_idx = find_goal(ref, len);
217
    //did a transformation here.
218
    double goal_y = cos(robot.theta)*(ref->y[goal_idx]-robot.y) 
219
        - sin(robot.theta)*(ref->x[goal_idx]-robot.x);
220
    double goal_rad = 2*goal_y / LOOK_AHEAD;
221
    
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

    
237
}
238

    
239
static int find_goal(curv_t *ref, int len) {
240
    int i, goal_idx;
241
    double min_dist = 100000000;
242
    double temp;
243
    int min_idx;
244
    for (i = 0; i < len; i++) {
245
        temp = DIST(ref->x[i], ref->y[i], robot.x, robot.y);
246
        if (temp < min_dist) {
247
            min_dist = temp;
248
            min_idx = i;
249
        }
250
    }
251
        //printf("%d min %f\n", min_idx, min_dist);
252
        //getchar();
253
    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
    }
261
        while(1) {
262
                printf("eol\n");
263
            exit(0);
264
        }
265
    return goal_idx;
266
} 
267