Subversion Repositories svnkaklik

Compare Revisions

Ignore whitespace Rev 365 → Rev 366

/roboti/Robotour/SW/board/main.cpp
41,16 → 41,6
 
using namespace std;
 
#define CMPS03_SOFTWARE_REVISION 0x0
#define SRF02_SOFTWARE_REVISION 0x0
 
#define I2C_SLAVE 0x0703 /* Change slave address */
#define I2C_RDWR 0x0707 /* Combined R/W transfer (one stop only)*/
#define I2C_SLAVE_FORCE 0x0706 /* Change slave address */
/* Attn.: Slave address is 7 or 10 bits */
/* This changes the address, even if it */
/* is already taken! */
 
#define BC_Addr 0x0B
#define US_Addr 0x70 // 0xE0 in fact
#define PIC_Addr 0x50 // 0xA0 in fact
89,7 → 79,7
cerr << "Could not open /dev/i2c-0." << endl;
return -1;
}
 
/*
if (ioctl(file, I2C_SLAVE, BC_Addr) == -1)
{
fprintf(stderr, "Failed to set address to 0x%02x.\n", BC_Addr);
119,7 → 109,7
 
Buf[Len] = 0x00;
fprintf(stdout, "Board ID is %s.\n", Buf);
 
*/
//!------ US ---------------------------------------------------------------------------
 
pthread_create(&thread_id, NULL, print_tele, NULL);
148,6 → 138,10
write(file, Buf, 1);
read(file, Buf, 1);
ble=Buf[0];
 
//!--------
printf("Vzdalenost: %u cm Command: %x\n",vzdalenost,ble);
 
};
close(file);
pthread_join(thread_id, NULL);
/roboti/Robotour/SW/motor/motor.c
1,6 → 1,6
#include "motor.h"
#use i2c(Slave,Fast,sda=PIN_B1,scl=PIN_B4,force_hw,address=0xA0) // Motor 1
//#use i2c(Slave,Fast,sda=PIN_B1,scl=PIN_B4,force_hw,address=0xA2) // Motor 2
//#use i2c(Slave,Fast,sda=PIN_B1,scl=PIN_B4,force_hw,address=0xA0) // Motor 1
#use i2c(Slave,Fast,sda=PIN_B1,scl=PIN_B4,force_hw,address=0xA2) // Motor 2
 
#define H1 PIN_A1
#define L1 PIN_A2
74,7 → 74,7
output_high(L1);
output_low(L2);
};
delay_us(127-abs(command));
delay_us(abs(command));
output_low(H1);
output_low(H2);
output_low(L1);
/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];
}
}
*/