Subversion Repositories svnkaklik

Rev

Rev 366 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log

Rev 366 Rev 367
Line 38... Line 38...
38 
#define CMPS03_SOFTWARE_REVISION 0x0
 38 
#define CMPS03_SOFTWARE_REVISION 0x0
39 
#define SRF02_SOFTWARE_REVISION 0x0
 39 
#define SRF02_SOFTWARE_REVISION 0x0
40 
 
 40 
 
41 
#define BC_Addr	    0x0B
 41 
#define BC_Addr	    0x0B
42 
#define US_Addr	    0x70 // 0xE0 in fact
 42 
#define US_Addr	    0x70 // 0xE0 in fact
43 
#define M1_Addr	0x50 // 0xA0 in fact
 43 
#define M1	0x50 // 0xA0 in fact
44 
#define M2_Addr	0x51 // 0xA2 in fact
 44 
#define M2	0x51 // 0xA2 in fact
45 
 
 45 
 
46 
char vystup[30];
 46 
char vystup[50];
47 
pthread_t thread_1, thread_2;
 47 
pthread_t thread_1, thread_2, thread_3;
48 
FILE *pRouraO,*pRouraI;
 48 
FILE *pRouraO,*pRouraI;
49 
unsigned int vzdalenost;
 49 
unsigned int vzdalenost;
50 
char command,ble;
 50 
char command,ble;
51 
int file;
 51 
int file;
- 
 
 52 
double n, e;
- 
 
 53 
 
- 
 
 54 
pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
52 
 
 55 
 
53 
void *print_tele(void *unused);
 56 
void *print_tele(void *unused);
- 
 
 57 
void *gps(void *unused);
54 
//void *sensors(void *unused);
 58 
//void *sensors(void *unused);
55 
 
 59 
 
56 
void I2C_addr (int Addr)
 60 
void I2C_addr (int Addr)
57 
{
 61 
{
58 
    if (ioctl(file, I2C_SLAVE, Addr) == -1)
 62 
    if (ioctl(file, I2C_SLAVE, Addr) == -1)
Line 60... Line 64...
60 
        fprintf(stderr, "Failed to set address to 0x%02x.\n", Addr);
 64 
        fprintf(stderr, "Failed to set address to 0x%02x.\n", Addr);
61 
        exit(-5);
 65 
        exit(-5);
62 
    }
 66 
    }
63 
}
 67 
}
64 
 
 68 
 
- 
 
 69 
void go (int Addr, int speed)
- 
 
 70 
{
- 
 
 71 
    char Buf[1];
- 
 
 72 
 
- 
 
 73 
    I2C_addr (Addr);
- 
 
 74 
    Buf[0]=speed;
- 
 
 75 
    write(file, Buf, 1);
- 
 
 76 
}
- 
 
 77 
 
65 
int main(int argc, char *argv[], char *envp[])
 78 
int main(int argc, char *argv[], char *envp[])
66 
{
 79 
{
67 
	fprintf(stdout, "**** Vector Control Programm ****\n");
 80 
	fprintf(stdout, "**** Vector Control Programm ****\n");
68 
 
 81 
 
69 
	file = open("/dev/i2c-0", O_RDWR);
 82 
	file = open("/dev/i2c-0", O_RDWR);
Line 75... Line 88...
75 
 
 88 
 
76 
    pthread_create(&thread_1, NULL, print_tele, NULL);
 89 
    pthread_create(&thread_1, NULL, print_tele, NULL);
77 
//    pthread_create(&thread_2, NULL, sensors, NULL);
 90 
//    pthread_create(&thread_2, NULL, sensors, NULL);
78 
 
 91 
 
79 
    char Buf[64];
 92 
    char Buf[64];
80 
    command=127;
 93 
    command=0;
81 
 
 94 
 
82 
    while(true)
 95 
    while(true)
83 
    {
 96 
    {
84 
        I2C_addr(US_Addr);
 - 
 
85 
        Buf[0]=0x0;
 - 
 
86 
        Buf[1]=0x51;
 - 
 
87 
        write(file, Buf, 2);
 - 
 
88 
        usleep(80000);
 - 
 
89 
        read(file, Buf, 3);
 - 
 
90 
        vzdalenost=(Buf[1]*256+Buf[2]);
 - 
 
91 
        usleep(300000);
 - 
 
92 
 
 - 
 
93 
        if (command!=127)
 97 
        switch (command)
94 
        {
 98 
        {
- 
 
 99 
            case 'f':   // forward
95 
            I2C_addr(M1_Addr);
 100 
                go(M1, 20);
- 
 
 101 
                go(M2, 20);
96 
            Buf[0]=command;
 102 
                command=0;
- 
 
 103 
                break;
- 
 
 104 
            case 'b':   // backward
- 
 
 105 
                go(M1, -20);
97 
            write(file, Buf, 1);
 106 
                go(M2, -20);
- 
 
 107 
                command=0;
- 
 
 108 
                break;
- 
 
 109 
            case 's':   // stop
- 
 
 110 
                go(M1, 0);
- 
 
 111 
                go(M2, 0);
- 
 
 112 
                command=0;
- 
 
 113 
                break;
- 
 
 114 
            case 'g':
- 
 
 115 
usleep(180000);
98 
            I2C_addr(M2_Addr);
 116 
                I2C_addr(US_Addr);
99 
            Buf[0]=command;
 117 
                Buf[0]=0x0;
- 
 
 118 
                Buf[1]=0x51;
100 
            write(file, Buf, 1);
 119 
                write(file, Buf, 2);
101 
 
 - 
 
- 
 
 120 
                usleep(80000);
- 
 
 121 
                read(file, Buf, 3);
- 
 
 122 
                vzdalenost=(Buf[1]*256+Buf[2]);
- 
 
 123 
                if ((vzdalenost>50)&&(vzdalenost<80))
- 
 
 124 
                {
- 
 
 125 
                    go(M1, 20);
- 
 
 126 
                    go(M2, 20);
- 
 
 127 
                    break;
- 
 
 128 
                };
- 
 
 129 
                if ((vzdalenost>30)&&(vzdalenost<120))
- 
 
 130 
                {
- 
 
 131 
                    if (vzdalenost<50)
- 
 
 132 
                    {
- 
 
 133 
                        go(M1, 20);
- 
 
 134 
                        go(M2, 40);
- 
 
 135 
                    }
- 
 
 136 
                    else
- 
 
 137 
                    {
- 
 
 138 
                        go(M1, 40);
- 
 
 139 
                        go(M2, 20);
- 
 
 140 
                    }
- 
 
 141 
                }
- 
 
 142 
                else
- 
 
 143 
                {
- 
 
 144 
                    go(M1, 0);  // zastav, neni videt doprovod
- 
 
 145 
                    go(M2, 0);
- 
 
 146 
                };
102 
            command=127;
 147 
                break;
103 
        }
 148 
        }
104 
    };
 149 
    };
105 
 
 150 
 
106 
	close(file);
 151 
	close(file);
107 
    pthread_join(thread_1, NULL);
 152 
    pthread_join(thread_1, NULL);
Line 111... Line 156...
111 
}
 156 
}
112 
 
 157 
 
113 
 
 158 
 
114 
void *print_tele(void *unused)
 159 
void *print_tele(void *unused)
115 
{
 160 
{
116 
    char i;
 161 
    char string[2];
117 
 
 162 
 
118 
    while(true)
 163 
    while(true)
119 
    {
 164 
    {
120 
        pRouraI = fopen("/home/ble/pipe","r");
 165 
        pRouraI = fopen("/home/ble/pipe","r");
121 
        i=fgetc(pRouraI);
 166 
        command=fgetc(pRouraI);
122 
        switch (i)
 - 
 
123 
        {
 - 
 
124 
            case 'a':
 - 
 
125 
                command=0;
 167 
        string[0]=command;
126 
                break;
 - 
 
127 
            case 'b':
 - 
 
128 
                command=20;
 - 
 
129 
                break;
 - 
 
130 
            case 'c':
 - 
 
131 
                command=-20;
 - 
 
132 
                break;
 168 
        string[1]=0;
133 
        }
 - 
 
134 
 
 - 
 
135 
        fclose(pRouraI);
 169 
        fclose(pRouraI);
136 
        pRouraO = fopen("/home/ble/pipe","w");
 170 
        pRouraO = fopen("/home/ble/pipe","w");
137 
        fprintf(pRouraO,"Vzdalenost: %u cm Command: 0x%X\n",vzdalenost,command);
 171 
        fprintf(pRouraO,"Vzdalenost: %u cm Command: %s\n",vzdalenost,string);
- 
 
 172 
 
- 
 
 173 
    pthread_mutex_lock(&mutex);
- 
 
 174 
   	fprintf(pRouraO,"%f N %f E\n", n, e);
138 
   	fprintf(pRouraO,"Vzdalenost: %.1f m\n", GeoCalc::EllipsoidDistance(49.266667, 14.7, 49.266667, 14.716667)*1000);
 175 
   	fprintf(pRouraO,"Vzdalenost: %.1f m\n", GeoCalc::EllipsoidDistance(n, e, 49.266667, 14.716667));
139 
	fprintf(pRouraO,"Azimut: %.2f Deg\n", GeoCalc::GCAzimuth(49.266667, 14.7, 49.266667, 14.716667));
 176 
	fprintf(pRouraO,"Azimut: %.2f Deg\n", GeoCalc::GCAzimuth(n, e, 49.266667, 14.716667));
- 
 
 177 
    pthread_mutex_unlock(&mutex);
- 
 
 178 
 
140 
        fclose(pRouraO);
 179 
        fclose(pRouraO);
141 
    }
 180 
    }
142 
}
 181 
}
143 
 
 182 
 
- 
 
 183 
void *gps(void *unused)
- 
 
 184 
{
- 
 
 185 
    FILE *pRS232;
- 
 
 186 
    double pomN, pomE;
- 
 
 187 
 
- 
 
 188 
    pRS232 = fopen("/dev/ttyS1","r");
- 
 
 189 
 
- 
 
 190 
    while(true)
- 
 
 191 
    {
- 
 
 192 
        fscanf(pRS232,"$GPGGA,*,%lf,N,%lf,E", &pomN, &pomE);
- 
 
 193 
        pthread_mutex_lock(&mutex);
- 
 
 194 
        n=pomN; e=pomE;
- 
 
 195 
        pthread_mutex_unlock(&mutex);
- 
 
 196 
        usleep(500000);
- 
 
 197 
    }
- 
 
 198 
}
- 
 
 199 
 
144 
/*
 200 
/*
145 
void *sensors(void *unused)
 201 
void *sensors(void *unused)
146 
{
 202 
{
147 
	char Buf[64];
 203 
	char Buf[64];
148 
 
 204