Subversion Repositories svnkaklik

Compare Revisions

Ignore whitespace Rev 366 → Rev 367

/roboti/Robotour/SW/vector/geocalc.cpp
63,7 → 63,7
distance = ((((sy * sy * 4.0 - 3.0) *
distance * cz * d / 6.0 - x) * d / 4.0 + cz) * sy * d + y) * c * GEO::ERAD * r;
 
return distance;
return distance*1000;
}
 
 
/roboti/Robotour/SW/vector/vector.cbp
7,7 → 7,7
<Option compiler="atmel_gnu_gcc_compiler" />
<Build>
<Target title="Debug">
<Option output="bin\Debug\vector" prefix_auto="1" extension_auto="1" />
<Option output="bin\Debug\vector" prefix_auto="0" extension_auto="0" />
<Option object_output="obj\Debug\" />
<Option type="1" />
<Option compiler="atmel_gnu_gcc_compiler" />
14,6 → 14,9
<Compiler>
<Add option="-g" />
</Compiler>
<Linker>
<Add option="-lpthread" />
</Linker>
</Target>
<Target title="Release">
<Option output="bin\Release\vector" prefix_auto="1" extension_auto="1" />
32,7 → 35,8
<Add option="-Wall" />
<Add option="-fexceptions" />
</Compiler>
<Unit filename="main.cpp" />
<Unit filename="geocalc.cpp" />
<Unit filename="vector.cpp" />
<Extensions>
<code_completion />
<envvars />
/roboti/Robotour/SW/vector/vector.cpp
40,17 → 40,21
 
#define BC_Addr 0x0B
#define US_Addr 0x70 // 0xE0 in fact
#define M1_Addr 0x50 // 0xA0 in fact
#define M2_Addr 0x51 // 0xA2 in fact
#define M1 0x50 // 0xA0 in fact
#define M2 0x51 // 0xA2 in fact
 
char vystup[30];
pthread_t thread_1, thread_2;
char vystup[50];
pthread_t thread_1, thread_2, thread_3;
FILE *pRouraO,*pRouraI;
unsigned int vzdalenost;
char command,ble;
int file;
double n, e;
 
pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
 
void *print_tele(void *unused);
void *gps(void *unused);
//void *sensors(void *unused);
 
void I2C_addr (int Addr)
62,6 → 66,15
}
}
 
void go (int Addr, int speed)
{
char Buf[1];
 
I2C_addr (Addr);
Buf[0]=speed;
write(file, Buf, 1);
}
 
int main(int argc, char *argv[], char *envp[])
{
fprintf(stdout, "**** Vector Control Programm ****\n");
77,29 → 90,61
// pthread_create(&thread_2, NULL, sensors, NULL);
 
char Buf[64];
command=127;
command=0;
 
while(true)
{
I2C_addr(US_Addr);
Buf[0]=0x0;
Buf[1]=0x51;
write(file, Buf, 2);
usleep(80000);
read(file, Buf, 3);
vzdalenost=(Buf[1]*256+Buf[2]);
usleep(300000);
 
if (command!=127)
switch (command)
{
I2C_addr(M1_Addr);
Buf[0]=command;
write(file, Buf, 1);
I2C_addr(M2_Addr);
Buf[0]=command;
write(file, Buf, 1);
 
command=127;
case 'f': // forward
go(M1, 20);
go(M2, 20);
command=0;
break;
case 'b': // backward
go(M1, -20);
go(M2, -20);
command=0;
break;
case 's': // stop
go(M1, 0);
go(M2, 0);
command=0;
break;
case 'g':
usleep(180000);
I2C_addr(US_Addr);
Buf[0]=0x0;
Buf[1]=0x51;
write(file, Buf, 2);
usleep(80000);
read(file, Buf, 3);
vzdalenost=(Buf[1]*256+Buf[2]);
if ((vzdalenost>50)&&(vzdalenost<80))
{
go(M1, 20);
go(M2, 20);
break;
};
if ((vzdalenost>30)&&(vzdalenost<120))
{
if (vzdalenost<50)
{
go(M1, 20);
go(M2, 40);
}
else
{
go(M1, 40);
go(M2, 20);
}
}
else
{
go(M1, 0); // zastav, neni videt doprovod
go(M2, 0);
};
break;
}
};
 
113,34 → 158,45
 
void *print_tele(void *unused)
{
char i;
char string[2];
 
while(true)
{
pRouraI = fopen("/home/ble/pipe","r");
i=fgetc(pRouraI);
switch (i)
{
case 'a':
command=0;
break;
case 'b':
command=20;
break;
case 'c':
command=-20;
break;
}
 
command=fgetc(pRouraI);
string[0]=command;
string[1]=0;
fclose(pRouraI);
pRouraO = fopen("/home/ble/pipe","w");
fprintf(pRouraO,"Vzdalenost: %u cm Command: 0x%X\n",vzdalenost,command);
fprintf(pRouraO,"Vzdalenost: %.1f m\n", GeoCalc::EllipsoidDistance(49.266667, 14.7, 49.266667, 14.716667)*1000);
fprintf(pRouraO,"Azimut: %.2f Deg\n", GeoCalc::GCAzimuth(49.266667, 14.7, 49.266667, 14.716667));
fprintf(pRouraO,"Vzdalenost: %u cm Command: %s\n",vzdalenost,string);
 
pthread_mutex_lock(&mutex);
fprintf(pRouraO,"%f N %f E\n", n, e);
fprintf(pRouraO,"Vzdalenost: %.1f m\n", GeoCalc::EllipsoidDistance(n, e, 49.266667, 14.716667));
fprintf(pRouraO,"Azimut: %.2f Deg\n", GeoCalc::GCAzimuth(n, e, 49.266667, 14.716667));
pthread_mutex_unlock(&mutex);
 
fclose(pRouraO);
}
}
 
void *gps(void *unused)
{
FILE *pRS232;
double pomN, pomE;
 
pRS232 = fopen("/dev/ttyS1","r");
 
while(true)
{
fscanf(pRS232,"$GPGGA,*,%lf,N,%lf,E", &pomN, &pomE);
pthread_mutex_lock(&mutex);
n=pomN; e=pomE;
pthread_mutex_unlock(&mutex);
usleep(500000);
}
}
 
/*
void *sensors(void *unused)
{