Project

General

Profile

Statistics
| Revision:

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

History | View | Annotate | Download (1.7 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

    
8
#define LS  0
9
#define LD  1
10
#define RS  2
11
#define RD  3
12
#define FORWARD 1
13
#define BACKWARD 0
14

    
15
int main()
16
{
17
        int serialFile = open("/dev/ttyUSB0", O_RDWR);
18
        if(serialFile < 1)
19
        {
20
                printf("Error opening serial\n");
21
                return -1;
22
        }
23
            
24
        struct termios attributes;
25
            tcgetattr(serialFile, &attributes);
26

    
27
        cfsetispeed(&attributes, B115200);
28
        cfsetospeed(&attributes, B115200);
29
        attributes.c_cflag &= ~PARENB;
30
        attributes.c_cflag &= ~CSTOPB;
31
        attributes.c_cflag &= ~CSIZE;
32
        attributes.c_cflag |= CS8;
33
        
34
//        attributes.c_cflag &= ~ICRNL;
35
//        attributes.c_cflag &= ~OCRNL;
36
//        attributes.c_cflag |= (CLOCAL | CREAD);
37
//        attributes.c_lflag &= ~ICANON;
38
//        attributes.c_cc[VMIN] = 1;
39
//        attributes.c_cc[VTIME] = 50;
40

    
41

    
42
        if (tcsetattr(serialFile, TCSANOW, &attributes) < 0){
43
                perror("tcsetattr failed");
44
                exit(-1);
45
        }
46
        
47
        unsigned char encoders[4] = {1,1,1,1};
48
///        while(1)
49
//                write(serialFile, encoders, 1);
50
        //Sending velocity as LS LD RS RD
51
        while(1)
52
        {
53
                printf("SENDING DATA\n");
54
                encoders[LS] = 170;
55
                encoders[LD] = 1;
56
                encoders[RS] = 170;
57
                encoders[RD] = 1;
58
                int temp = 0;
59
                int count = 0;
60
                do
61
                {
62
//                        tcflush(serialFile, TCIOFLUSH
63
                        temp = write(serialFile, encoders + count, 1);        
64
                        if(temp < 0)
65
                                perror("Write Error");
66
                        count += temp;
67
//                        tcflush(serialFile, TCIOFLUSH);
68
                        usleep(10000);
69
                }while(count < 4);
70
                count = 0;
71
                printf("READING DATA\n");
72
                do
73
                {
74
                        count += read(serialFile, encoders, 4);
75
                }while(count < 4);
76
                
77
                int leftEncoder = (encoders[0] << 8) | encoders[1];
78
                int rightEncoder = (encoders[2] << 8) | encoders[3];
79
                printf("%d %d\n", leftEncoder, rightEncoder);
80
        
81
        }
82

    
83
}