Subversion Repositories svnkaklik

Compare Revisions

Ignore whitespace Rev 364 → Rev 365

/roboti/Robotour/SW/board/main.cpp
51,14 → 51,15
/* This changes the address, even if it */
/* is already taken! */
 
#define BC_Addr 0x0B
#define US_Addr 0x70 // 0xE0 in fact
#define BC_Addr 0x0B
#define US_Addr 0x70 // 0xE0 in fact
#define PIC_Addr 0x50 // 0xA0 in fact
 
char vystup[30];
char flag;
pthread_t thread_id;
FILE *pRouraO,*pRouraI;
unsigned int vzdalenost;
char command,ble;
 
void *print_tele(void *unused);
 
121,33 → 122,32
 
//!------ US ---------------------------------------------------------------------------
 
if (ioctl(file, I2C_SLAVE, US_Addr) == -1)
{
fprintf(stderr, "Failed to set address to 0x%02x.\n", US_Addr);
DoExit(-6);
}
 
pthread_create(&thread_id, NULL, print_tele, NULL);
flag = 0;
 
while(true)
{
if (ioctl(file, I2C_SLAVE, US_Addr) == -1)
{
fprintf(stderr, "Failed to set address to 0x%02x.\n", US_Addr);
DoExit(-6);
}
Buf[0]=0x0;
Buf[1]=0x51;
write(file, Buf, 2);
usleep(80000);
read(file, Buf, 3);
if (flag==0)
vzdalenost=(Buf[1]*256+Buf[2]);
// printf("Vzdalenost: %u cm\n",ble);
vzdalenost=(Buf[1]*256+Buf[2]);
usleep(300000);
 
/*
if (0==flag)
if (ioctl(file, I2C_SLAVE, PIC_Addr) == -1)
{
sprintf(vystup,"Vzdalenost: %u cm\n",vzdalenost);
};
*/
usleep(300000);
fprintf(stderr, "Failed to set address to 0x%02x.\n", US_Addr);
DoExit(-6);
}
Buf[0]=command;
write(file, Buf, 1);
read(file, Buf, 1);
ble=Buf[0];
};
close(file);
pthread_join(thread_id, NULL);
158,19 → 158,13
 
void *print_tele(void *unused)
{
flag=0;
 
while(1)
{
char pom;
 
pRouraI = fopen("/home/ble/pipe","r");
pom=fgetc(pRouraI);
command=fgetc(pRouraI);
fclose(pRouraI);
flag=1;
pRouraO = fopen("/home/ble/pipe","w");
fprintf(pRouraO,"Vzdalenost: %u cm\n",vzdalenost);
fprintf(pRouraO,"Vzdalenost: %u cm Command: %x\n",vzdalenost,ble);
fclose(pRouraO);
flag=0;
}
}
/roboti/Robotour/SW/motor/motor.PJT
0,0 → 1,40
[PROJECT]
Target=motor.HEX
Development_Mode=
Processor=0x688F
ToolSuite=CCS
 
[Directories]
Include=C:\Program Files\PICC\devices\;C:\Program Files\PICC\Dr
Library=
LinkerScript=
 
[Target Data]
FileList=C:\dokumenty\svn\Kaklik\roboti\Robotour\SW\motor\motor.c
BuildTool=C-COMPILER
OptionString=+FM
AdditionalOptionString=
BuildRequired=1
 
[motor.c]
Type=4
Path=
FileList=
BuildTool=
OptionString=
AdditionalOptionString=
 
[mru-list]
1=motor.c
 
[Windows]
0=0000 motor.c 0 0 796 451 3 0
 
[Opened Files]
1=C:\dokumenty\svn\Kaklik\roboti\Robotour\SW\motor\motor.c
2=C:\dokumenty\svn\Kaklik\roboti\Robotour\SW\motor\motor.h
3=C:\Program Files\PICC\devices\16F88.h
4=
[Units]
Count=1
1=C:\dokumenty\svn\Kaklik\roboti\Robotour\SW\motor\motor.c (main)
/roboti/Robotour/SW/motor/motor.c
0,0 → 1,86
#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
 
#define H1 PIN_A1
#define L1 PIN_A2
#define H2 PIN_A3
#define L2 PIN_A4
 
signed int8 command;
 
#INT_SSP
void ssp_interupt ()
{
BYTE incoming, state;
output_low(H1);
output_low(L1);
output_low(H2);
output_low(L2);
 
state = i2c_isr_state();
 
if(state < 0x80) //Master is sending data
{
output_toggle(PIN_A0);
command = i2c_read();
}
if(state == 0x80) //Master is requesting data
{
i2c_write(command);
}
}
 
 
void main()
{
 
setup_adc_ports(NO_ANALOGS|VSS_VDD);
setup_adc(ADC_OFF);
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
setup_timer_1(T1_DISABLED);
setup_timer_2(T2_DISABLED,0,1);
setup_comparator(NC_NC_NC_NC);
setup_vref(FALSE);
setup_oscillator(OSC_8MHZ|OSC_INTRC);
enable_interrupts(GLOBAL);
enable_interrupts(INT_SSP);
while(true)
{
 
if (0==command)
{
output_low(H1); // stop
output_low(H2);
output_low(L1);
output_low(L2);
continue;
};
if (command>0)
{
output_high(H1); // vpred
output_low(H2);
output_low(L1);
output_high(L2);
}
else
{
output_low(H1); // vzad
output_high(H2);
output_high(L1);
output_low(L2);
};
delay_us(127-abs(command));
output_low(H1);
output_low(H2);
output_low(L1);
output_low(L2);
}
}
 
 
 
/roboti/Robotour/SW/motor/motor.h
0,0 → 1,18
#include <16F88.h>
#device adc=8
 
#FUSES NOWDT //No Watch Dog Timer
#FUSES INTRC_IO //Internal RC Osc, no CLKOUT
#FUSES PUT //Power Up Timer
#FUSES MCLR //Master Clear pin enabled
#FUSES NOBROWNOUT //No brownout reset
#FUSES NOLVP //No low voltage prgming, B3(PIC16) or B5(PIC18) used for I/O
#FUSES NOCPD //No EE protection
#FUSES NOWRT //Program memory not write protected
#FUSES NODEBUG //No Debug mode for ICD
#FUSES NOPROTECT //Code not protected from reading
#FUSES NOFCMEN //Fail-safe clock monitor disabled
#FUSES NOIESO //Internal External Switch Over mode disabled
 
#use delay(clock=8000000)
 
/roboti/Robotour/SW/pic_i2c_slave/test.PJT
0,0 → 1,40
[PROJECT]
Target=test.HEX
Development_Mode=
Processor=0x688F
ToolSuite=CCS
 
[Directories]
Include=C:\Program Files\PICC\devices\;C:\Program Files\PICC\Dr
Library=
LinkerScript=
 
[Target Data]
FileList=C:\Dokumenty\svn\Kaklik\roboti\Robotour\SW\pic\test.c
BuildTool=C-COMPILER
OptionString=+FM
AdditionalOptionString=
BuildRequired=1
 
[test.c]
Type=4
Path=
FileList=
BuildTool=
OptionString=
AdditionalOptionString=
 
[mru-list]
1=test.c
 
[Windows]
0=0000 test.c 0 0 796 451 3 0
 
[Opened Files]
1=C:\Dokumenty\svn\Kaklik\roboti\Robotour\SW\pic\test.c
2=C:\Dokumenty\svn\Kaklik\roboti\Robotour\SW\pic\test.h
3=C:\Program Files\PICC\devices\16F88.h
4=
[Units]
Count=1
1=C:\Dokumenty\svn\Kaklik\roboti\Robotour\SW\pic\test.c (main)
/roboti/Robotour/SW/pic_i2c_slave/test.c
0,0 → 1,73
#include "test.h"
 
#define H1 PIN_A1
#define L1 PIN_A2
#define H2 PIN_A3
#define L2 PIN_A4
 
char command;
 
int n;
 
#INT_SSP
void ssp_interupt ()
{
BYTE incoming, state;
output_low(H1);
output_low(L1);
output_low(H2);
output_low(L2);
 
state = i2c_isr_state();
 
if(state < 0x80) //Master is sending data
{
output_toggle(PIN_A0);
command = i2c_read();
}
if(state == 0x80) //Master is requesting data
{
i2c_write(command);
}
}
 
void main()
{
 
setup_adc_ports(NO_ANALOGS|VSS_VDD);
setup_adc(ADC_OFF);
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
setup_timer_1(T1_DISABLED);
setup_timer_2(T2_DISABLED,0,1);
setup_comparator(NC_NC_NC_NC);
setup_vref(FALSE);
setup_oscillator(False);
 
enable_interrupts(GLOBAL);
enable_interrupts(INT_SSP);
while(true)
{
if(command=='a')
{
output_high(H1);
output_low(H2);
output_low(L1);
output_high(L2);
}
else
{
output_low(H1);
output_high(H2);
output_high(L1);
output_low(L2);
};
delay_us(60);
output_low(H1);
output_low(H2);
output_low(L1);
output_low(L2);
}
}
/roboti/Robotour/SW/pic_i2c_slave/test.h
0,0 → 1,19
#include <16F88.h>
#device adc=8
 
#FUSES NOWDT //No Watch Dog Timer
#FUSES HS //High speed Osc (> 4mhz)
#FUSES NOPUT //No Power Up Timer
#FUSES MCLR //Master Clear pin enabled
#FUSES NOBROWNOUT //No brownout reset
#FUSES NOLVP //No low voltage prgming, B3(PIC16) or B5(PIC18) used for I/O
#FUSES NOCPD //No EE protection
#FUSES NOWRT //Program memory not write protected
#FUSES NODEBUG //No Debug mode for ICD
#FUSES NOPROTECT //Code not protected from reading
#FUSES NOFCMEN //Fail-safe clock monitor disabled
#FUSES NOIESO //Internal External Switch Over mode disabled
 
#use delay(clock=4000000)
#use i2c(Slave,Fast,sda=PIN_B1,scl=PIN_B4,force_hw,address=0xA0)
 
/roboti/Robotour/SW/vector/geocalc.cpp
0,0 → 1,118
/* Navigation code */
 
#include "geocalc.h"
 
double GeoCalc::EllipsoidDistance(double lat1, double lon1, double lat2, double lon2)
{
double distance = 0.0;
double faz, baz;
double r = 1.0 - GEO::FLATTENING;
double tu1, tu2, cu1, su1, cu2, x, sx, cx, sy, cy, y, sa, c2a, cz, e, c, d;
double cosy1, cosy2;
distance = 0.0;
 
if((lon1 == lon2) && (lat1 == lat2)) return distance;
lon1 *= GEO::DE2RA;
lon2 *= GEO::DE2RA;
lat1 *= GEO::DE2RA;
lat2 *= GEO::DE2RA;
 
cosy1 = cos(lat1);
cosy2 = cos(lat2);
 
if(cosy1 == 0.0) cosy1 = 0.0000000001;
if(cosy2 == 0.0) cosy2 = 0.0000000001;
 
tu1 = r * sin(lat1) / cosy1;
tu2 = r * sin(lat2) / cosy2;
cu1 = 1.0 / sqrt(tu1 * tu1 + 1.0);
su1 = cu1 * tu1;
cu2 = 1.0 / sqrt(tu2 * tu2 + 1.0);
x = lon2 - lon1;
 
distance = cu1 * cu2;
baz = distance * tu2;
faz = baz * tu1;
 
do {
sx = sin(x);
cx = cos(x);
tu1 = cu2 * sx;
tu2 = baz - su1 * cu2 * cx;
sy = sqrt(tu1 * tu1 + tu2 * tu2);
cy = distance * cx + faz;
y = atan2(sy, cy);
sa = distance * sx / sy;
c2a = -sa * sa + 1.0;
cz = faz + faz;
if(c2a > 0.0) cz = -cz / c2a + cy;
e = cz * cz * 2. - 1.0;
c = ((-3.0 * c2a + 4.0) * GEO::FLATTENING + 4.0) * c2a * GEO::FLATTENING / 16.0;
d = x;
x = ((e * cy * c + cz) * sy * c + y) * sa;
x = (1.0 - c) * x * GEO::FLATTENING + lon2 - lon1;
} while(fabs(d - x) > GEO::EPS);
 
x = sqrt((1.0 / r / r - 1.0) * c2a + 1.0) + 1.0;
x = (x - 2.0) / x;
c = 1.0 - x;
c = (x * x / 4.0 + 1.0) / c;
d = (0.375 * x * x - 1.0) * x;
x = e * cy;
distance = 1.0 - e - e;
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;
}
 
 
double GeoCalc::GCAzimuth(double lat1, double lon1, double lat2, double lon2)
{
double result = 0.0;
 
INT32 ilat1 = (INT32)(0.50 + lat1 * 360000.0);
INT32 ilat2 = (INT32)(0.50 + lat2 * 360000.0);
INT32 ilon1 = (INT32)(0.50 + lon1 * 360000.0);
INT32 ilon2 = (INT32)(0.50 + lon2 * 360000.0);
 
lat1 *= GEO::DE2RA;
lon1 *= GEO::DE2RA;
lat2 *= GEO::DE2RA;
lon2 *= GEO::DE2RA;
 
if ((ilat1 == ilat2) && (ilon1 == ilon2))
{
return result;
}
else if (ilon1 == ilon2)
{
if (ilat1 > ilat2)
result = 180.0;
}
else
{
double c = acos(sin(lat2)*sin(lat1) + cos(lat2)*cos(lat1)*cos((lon2-lon1)));
double A = asin(cos(lat2)*sin((lon2-lon1))/sin(c));
result = (A * GEO::RA2DE);
 
if ((ilat2 > ilat1) && (ilon2 > ilon1))
{
}
else if ((ilat2 < ilat1) && (ilon2 < ilon1))
{
result = 180.0 - result;
}
else if ((ilat2 < ilat1) && (ilon2 > ilon1))
{
result = 180.0 - result;
}
else if ((ilat2 > ilat1) && (ilon2 < ilon1))
{
result += 360.0;
}
}
 
return result;
}
 
/roboti/Robotour/SW/vector/geocalc.h
0,0 → 1,27
#include <iostream>
#include "math.h"
 
typedef signed int INT32;
 
namespace GEO {
const double PI = 3.14159265359;
const double PIOVER2 = PI/2.0;
const double TWOPI = 6.28318530718;
const double DE2RA = 0.01745329252;
const double RA2DE = 57.2957795129;
const double ERAD = 6378.137;
const double FLATTENING = 1.000000/298.257223563;// Earth flattening (WGS84)
const double EPS = 0.000000000005;
}
 
using namespace std;
 
class GeoCalc {
public:
// great circle method
static double GCAzimuth(double lat1, double lon1, double lat2, double lon2);
 
// ellipsoid methods
static double EllipsoidDistance(double lat1, double lon1, double lat2, double lon2);
};
 
/roboti/Robotour/SW/vector/main.cpp
0,0 → 1,9
#include <iostream>
 
using namespace std;
 
int main()
{
cout << "Hello world!" << endl;
return 0;
}
/roboti/Robotour/SW/vector/vector.cbp
0,0 → 1,42
<?xml version="1.0" encoding="UTF-8" standalone="yes" ?>
<CodeBlocks_project_file>
<FileVersion major="1" minor="6" />
<Project>
<Option title="vector" />
<Option pch_mode="2" />
<Option compiler="atmel_gnu_gcc_compiler" />
<Build>
<Target title="Debug">
<Option output="bin\Debug\vector" prefix_auto="1" extension_auto="1" />
<Option object_output="obj\Debug\" />
<Option type="1" />
<Option compiler="atmel_gnu_gcc_compiler" />
<Compiler>
<Add option="-g" />
</Compiler>
</Target>
<Target title="Release">
<Option output="bin\Release\vector" prefix_auto="1" extension_auto="1" />
<Option object_output="obj\Release\" />
<Option type="1" />
<Option compiler="atmel_gnu_gcc_compiler" />
<Compiler>
<Add option="-O2" />
</Compiler>
<Linker>
<Add option="-s" />
</Linker>
</Target>
</Build>
<Compiler>
<Add option="-Wall" />
<Add option="-fexceptions" />
</Compiler>
<Unit filename="main.cpp" />
<Extensions>
<code_completion />
<envvars />
<debugger />
</Extensions>
</Project>
</CodeBlocks_project_file>
/roboti/Robotour/SW/vector/vector.cpp
0,0 → 1,159
/*****************************************************************************/
/*
* vector.cpp - Control program for Vector robot
*
* Copyright (C) 2007 KAKL
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
*/
/*****************************************************************************/
 
#include <iostream>
#include <getopt.h>
#include <errno.h>
#include <string.h>
#include <pthread.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include "linux/i2c-dev.h"
#include "linux/i2c.h"
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include "geocalc.h"
 
using namespace std;
 
#define CMPS03_SOFTWARE_REVISION 0x0
#define SRF02_SOFTWARE_REVISION 0x0
 
#define BC_Addr 0x0B
#define US_Addr 0x70 // 0xE0 in fact
#define PIC_Addr 0x50 // 0xA0 in fact
 
char vystup[30];
pthread_t thread_1, thread_2;
FILE *pRouraO,*pRouraI;
unsigned int vzdalenost;
char command,ble;
int file;
 
void *print_tele(void *unused);
void *sensors(void *unused);
 
void I2C_addr (int Addr)
{
if (ioctl(file, I2C_SLAVE, Addr) == -1)
{
fprintf(stderr, "Failed to set address to 0x%02x.\n", Addr);
exit(-5);
}
}
 
int main(int argc, char *argv[], char *envp[])
{
fprintf(stdout, "**** Vector Control Programm ****\n");
 
file = open("/dev/i2c-0", O_RDWR);
if (file < 0)
{
cerr << "Could not open /dev/i2c-0." << endl;
return -1;
}
 
pthread_create(&thread_1, NULL, print_tele, NULL);
// pthread_create(&thread_2, NULL, sensors, NULL);
 
while(true)
{
int n;
char Buf[64];
 
for(n=-127; n<=127;n++)
{
 
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]);
printf("+%X",Buf[2]);
usleep(300000);
/*
Buf[0]=(char)n;
I2C_addr(PIC_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);
Buf[0]=command;
write(file, Buf, 1);
usleep(300000);
};
*/
};
close(file);
pthread_join(thread_1, NULL);
pthread_join(thread_2, NULL);
 
return 0;
}
 
void *print_tele(void *unused)
{
while(true)
{
pRouraI = fopen("/home/ble/pipe","r");
command=fgetc(pRouraI);
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));
fclose(pRouraO);
}
}
 
void *sensors(void *unused)
{
char Buf[64];
 
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);
 
I2C_addr(PIC_Addr);
Buf[0]=command;
write(file, Buf, 1);
read(file, Buf, 1);
ble=Buf[0];
}
}