Subversion Repositories svnkaklik

Compare Revisions

Ignore whitespace Rev 365 → Rev 366

/roboti/Robotour/SW/vector/vector.cpp
40,7 → 40,8
 
#define BC_Addr 0x0B
#define US_Addr 0x70 // 0xE0 in fact
#define PIC_Addr 0x50 // 0xA0 in fact
#define M1_Addr 0x50 // 0xA0 in fact
#define M2_Addr 0x51 // 0xA2 in fact
 
char vystup[30];
pthread_t thread_1, thread_2;
50,7 → 51,7
int file;
 
void *print_tele(void *unused);
void *sensors(void *unused);
//void *sensors(void *unused);
 
void I2C_addr (int Addr)
{
75,14 → 76,11
pthread_create(&thread_1, NULL, print_tele, NULL);
// pthread_create(&thread_2, NULL, sensors, NULL);
 
char Buf[64];
command=127;
 
while(true)
{
int n;
char Buf[64];
 
for(n=-127; n<=127;n++)
{
 
I2C_addr(US_Addr);
Buf[0]=0x0;
Buf[1]=0x51;
90,29 → 88,21
usleep(80000);
read(file, Buf, 3);
vzdalenost=(Buf[1]*256+Buf[2]);
printf("+%X",Buf[2]);
usleep(300000);
/*
Buf[0]=(char)n;
I2C_addr(PIC_Addr);
 
if (command!=127)
{
I2C_addr(M1_Addr);
Buf[0]=command;
write(file, Buf, 1);
read(file, Buf, 1);
printf("*%X",Buf[0]);
usleep(300000);
*/
};
/*
for(n=127; n>=-127;n--)
{
Buf[0]=(char)n;
I2C_addr(PIC_Addr);
I2C_addr(M2_Addr);
Buf[0]=command;
write(file, Buf, 1);
usleep(300000);
};
*/
 
command=127;
}
};
 
close(file);
pthread_join(thread_1, NULL);
pthread_join(thread_2, NULL);
120,21 → 110,38
return 0;
}
 
 
void *print_tele(void *unused)
{
char i;
 
while(true)
{
pRouraI = fopen("/home/ble/pipe","r");
command=fgetc(pRouraI);
i=fgetc(pRouraI);
switch (i)
{
case 'a':
command=0;
break;
case 'b':
command=20;
break;
case 'c':
command=-20;
break;
}
 
fclose(pRouraI);
pRouraO = fopen("/home/ble/pipe","w");
fprintf(pRouraO,"Vzdalenost: %u cm Command: %x\n",vzdalenost,ble);
printf("Vzdalenost: %.1f m\n", GeoCalc::EllipsoidDistance(49.266667, 14.7, 49.266667, 14.716667)*1000);
printf("Azimut: %.2f Deg\n", GeoCalc::GCAzimuth(49.266667, 14.7, 49.266667, 14.716667));
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));
fclose(pRouraO);
}
}
 
/*
void *sensors(void *unused)
{
char Buf[64];
157,3 → 164,4
ble=Buf[0];
}
}
*/