Subversion Repositories svnkaklik

Compare Revisions

Ignore whitespace Rev 366 → Rev 365

/roboti/Robotour/SW/board/main.cpp
41,6 → 41,16
 
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
79,7 → 89,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);
109,7 → 119,7
 
Buf[Len] = 0x00;
fprintf(stdout, "Board ID is %s.\n", Buf);
*/
 
//!------ US ---------------------------------------------------------------------------
 
pthread_create(&thread_id, NULL, print_tele, NULL);
138,10 → 148,6
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(abs(command));
delay_us(127-abs(command));
output_low(H1);
output_low(H2);
output_low(L1);
/roboti/Robotour/SW/vector/vector.cpp
40,8 → 40,7
 
#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 PIC_Addr 0x50 // 0xA0 in fact
 
char vystup[30];
pthread_t thread_1, thread_2;
51,7 → 50,7
int file;
 
void *print_tele(void *unused);
//void *sensors(void *unused);
void *sensors(void *unused);
 
void I2C_addr (int Addr)
{
76,11 → 75,14
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;
88,21 → 90,29
usleep(80000);
read(file, Buf, 3);
vzdalenost=(Buf[1]*256+Buf[2]);
printf("+%X",Buf[2]);
usleep(300000);
 
if (command!=127)
{
I2C_addr(M1_Addr);
/*
Buf[0]=(char)n;
I2C_addr(PIC_Addr);
Buf[0]=command;
write(file, Buf, 1);
I2C_addr(M2_Addr);
read(file, Buf, 1);
printf("*%X",Buf[0]);
usleep(300000);
*/
};
/*
for(n=127; n>=-127;n--)
{
Buf[0]=(char)n;
I2C_addr(PIC_Addr);
Buf[0]=command;
write(file, Buf, 1);
 
command=127;
}
usleep(300000);
};
*/
};
 
close(file);
pthread_join(thread_1, NULL);
pthread_join(thread_2, NULL);
110,38 → 120,21
return 0;
}
 
 
void *print_tele(void *unused)
{
char i;
 
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);
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: %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));
fclose(pRouraO);
}
}
 
/*
void *sensors(void *unused)
{
char Buf[64];
164,4 → 157,3
ble=Buf[0];
}
}
*/