root / branches / encoders / code / behaviors / spline / server / server.c @ 1237
History | View | Annotate | Download (2.17 KB)
1 | 1197 | ayeager | #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 | 1237 | ayeager | #include "encoders.h" |
8 | 1197 | ayeager | |
9 | 1216 | chihsiuh | #define LS 0 |
10 | #define LD 1 |
||
11 | #define RS 2 |
||
12 | #define RD 3 |
||
13 | #define FORWARD 1 |
||
14 | #define BACKWARD 0 |
||
15 | 1224 | chihsiuh | #define SENDER 1 |
16 | 1237 | ayeager | #define RECEIVER 0 |
17 | 1216 | chihsiuh | |
18 | 1237 | ayeager | int left_dx = 0; |
19 | int right_dx = 0; |
||
20 | |||
21 | 1224 | chihsiuh | void setAttrib(int file) |
22 | 1197 | ayeager | { |
23 | 1216 | chihsiuh | struct termios attributes;
|
24 | 1224 | chihsiuh | tcgetattr(file, &attributes); |
25 | 1197 | ayeager | |
26 | 1216 | chihsiuh | cfsetispeed(&attributes, B115200); |
27 | cfsetospeed(&attributes, B115200); |
||
28 | attributes.c_cflag &= ~PARENB; |
||
29 | attributes.c_cflag &= ~CSTOPB; |
||
30 | attributes.c_cflag &= ~CSIZE; |
||
31 | attributes.c_cflag |= CS8; |
||
32 | |||
33 | 1237 | ayeager | attributes.c_cflag &= ~ICRNL; |
34 | attributes.c_cflag &= ~OCRNL; |
||
35 | attributes.c_cflag |= (CLOCAL | CREAD); |
||
36 | attributes.c_lflag &= ~ICANON; |
||
37 | 1216 | chihsiuh | |
38 | |||
39 | 1224 | chihsiuh | if (tcsetattr(file, TCSANOW, &attributes) < 0){ |
40 | 1216 | chihsiuh | perror("tcsetattr failed");
|
41 | exit(-1);
|
||
42 | } |
||
43 | 1224 | chihsiuh | } |
44 | |||
45 | int main()
|
||
46 | { |
||
47 | int serialFileIn = open("/dev/ttyUSB0", O_RDWR); |
||
48 | int serialFileOut = open("/dev/ttyUSB1", O_RDWR); |
||
49 | if(serialFileIn < 1 || serialFileOut < 1) |
||
50 | { |
||
51 | printf("Error opening serial\n");
|
||
52 | return -1; |
||
53 | } |
||
54 | |||
55 | setAttrib(serialFileOut); |
||
56 | setAttrib(serialFileIn); |
||
57 | 1216 | chihsiuh | |
58 | unsigned char encoders[4] = {1,1,1,1}; |
||
59 | 1224 | chihsiuh | |
60 | char senderNum = SENDER;
|
||
61 | char receiverNum = RECEIVER;
|
||
62 | |||
63 | write(serialFileIn, &receiverNum, 1);
|
||
64 | sleep(1);
|
||
65 | write(serialFileOut, &senderNum, 1);
|
||
66 | |||
67 | 1216 | chihsiuh | //Sending velocity as LS LD RS RD
|
68 | 1222 | chihsiuh | int tempCount = 0; |
69 | 1237 | ayeager | |
70 | int dx, left_v;
|
||
71 | 1222 | chihsiuh | |
72 | 1197 | ayeager | while(1) |
73 | { |
||
74 | 1216 | chihsiuh | printf("SENDING DATA\n");
|
75 | 1222 | chihsiuh | encoders[LS] = tempCount; |
76 | 1216 | chihsiuh | encoders[LD] = 1;
|
77 | 1222 | chihsiuh | encoders[RS] = tempCount; |
78 | encoders[RD] = 0;
|
||
79 | tempCount += 50;
|
||
80 | 1216 | chihsiuh | int temp = 0; |
81 | int count = 0; |
||
82 | do
|
||
83 | { |
||
84 | 1224 | chihsiuh | // tcflush(serialFile, TCIOFLUSH);
|
85 | temp = write(serialFileOut, encoders + count, 1);
|
||
86 | 1216 | chihsiuh | if(temp < 0) |
87 | perror("Write Error");
|
||
88 | count += temp; |
||
89 | 1222 | chihsiuh | printf("sent: %d\n", count);
|
90 | 1224 | chihsiuh | usleep(100);
|
91 | 1216 | chihsiuh | }while(count < 4); |
92 | count = 0;
|
||
93 | printf("READING DATA\n");
|
||
94 | do
|
||
95 | { |
||
96 | 1224 | chihsiuh | count += read(serialFileIn, encoders, 4);
|
97 | 1216 | chihsiuh | }while(count < 4); |
98 | |||
99 | int leftEncoder = (encoders[0] << 8) | encoders[1]; |
||
100 | int rightEncoder = (encoders[2] << 8) | encoders[3]; |
||
101 | 1237 | ayeager | // printf("%d %d\n", leftEncoder, rightEncoder);
|
102 | |||
103 | encoder_recv(leftEncoder, rightEncoder); |
||
104 | printf("LEFT V: %d \t RIGHT V: %d\n", encoder_get_v(LEFT), encoder_get_v(RIGHT));
|
||
105 | |||
106 | |||
107 | |||
108 | |||
109 | 1197 | ayeager | } |
110 | |||
111 | } |