Subversion Repositories svnkaklik

Compare Revisions

Ignore whitespace Rev 370 → Rev 371

/roboti/Robotour/SW/motor/motor.c
14,16 → 14,12
{
BYTE incoming, state;
 
output_low(H1);
output_low(L1);
output_low(H2);
output_low(L2);
output_a(0); // vypnuti vsech budicu
 
state = i2c_isr_state();
 
if(state < 0x80) //Master is sending data
{
// output_toggle(PIN_A0);
command = i2c_read();
}
 
40,7 → 36,6
 
setup_adc_ports(NO_ANALOGS|VSS_VDD);
setup_adc(ADC_OFF);
// setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
setup_timer_0(RTCC_INTERNAL);setup_wdt(WDT_144MS);
setup_timer_1(T1_DISABLED);
setup_timer_2(T2_DISABLED,0,1);
67,35 → 62,15
 
speed=command+127; // posunuti 0 pro zaporna cisla
 
output_a(0b10010);
output_a(0b10010); // vpred
delay_us(speed);
output_a(0);
output_a(0); // vypnuti vsech budicu
delay_us(1);
restart_wdt();
output_a(0b01100);
output_a(0b01100); // vzad
delay_us(254-speed);
output_a(0);
output_a(0); // vypnuti vsech budicu
delay_us(1);
 
/*
output_high(H1); // vpred
output_high(L2);
delay_us(speed);
 
output_low(H1); // stop, aby se neseply tranzistory proti sobe!
output_low(H2);
output_low(L1);
output_low(L2);
 
output_high(H2); // vzad
output_high(L1);
delay_us(32-speed);
 
output_low(H1); // stop, aby se neseply tranzistory proti sobe!
output_low(H2);
output_low(L1);
output_low(L2);
*/
}
}
 
/roboti/Robotour/SW/vector/vector.cbp
36,6 → 36,8
<Add option="-fexceptions" />
</Compiler>
<Unit filename="geocalc.cpp" />
<Unit filename="nav.h" />
<Unit filename="track.h" />
<Unit filename="vector.cpp" />
<Extensions>
<code_completion />
/roboti/Robotour/SW/vector/vector.cpp
40,7 → 40,7
#define SRF02_SOFTWARE_REVISION 0x0
 
#define BC_Addr 0x0B
#define US_Addr 0x70 // 0xE0 in fact
#define US3_Addr 0x70 // 0xE0 in fact; Sonar na doprovod
#define M1 0x50 // 0xA0 in fact
#define M2 0x51 // 0xA2 in fact
 
76,8 → 76,39
write(file, Buf, 1);
}
 
unsigned int echo(int Addr)
{
char Buf[3];
 
I2C_addr(Addr);
Buf[0]=0x0;
Buf[1]=0x51;
write(file, Buf, 2);
usleep(80000);
read(file, Buf, 3);
return (Buf[1]*256+Buf[2]);
}
 
void FindNearestCrossing(void)
{
int n;
 
n=0;
do
{
if (0==cros[n].id) break;
if (GeoCalc::EllipsoidDistance(n, e, cros[n].n, cros[n].e) <= cros[n].dia)
{
#include "nav.h"
break;
}
} while(++n<POINTS);
}
 
int main(int argc, char *argv[], char *envp[])
{
int filtr;
 
fprintf(stdout, "**** Vector Control Programm ****\n");
 
file = open("/dev/i2c-0", O_RDWR);
91,8 → 122,8
pthread_create(&thread_3, NULL, gps, NULL);
// pthread_create(&thread_2, NULL, sensors, NULL);
 
char Buf[64];
command=0;
filtr=0;
 
while(true)
{
120,37 → 151,39
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))
vzdalenost=echo(US3_Addr);
if ((vzdalenost>60)&&(vzdalenost<80))
{
go(M1, 20);
go(M2, 20);
go(M1, 60);
go(M2, 60);
filtr=0;
break;
};
if ((vzdalenost>30)&&(vzdalenost<120))
if ((vzdalenost>30)&&(vzdalenost<130))
{
if (vzdalenost<50)
filtr=0;
if (vzdalenost<55)
{
go(M1, 20);
go(M2, 40);
go(M1, 30);
go(M2, 60);
}
else
{
go(M1, 40);
go(M2, 20);
go(M1, 60);
go(M2, 30);
}
}
else
{
go(M1, 0); // zastav, neni videt doprovod
go(M2, 0);
filtr++;
if (filtr>5)
{
go(M1, 1); // zastav, neni videt doprovod
go(M2, 1);
filtr=6;
}
};
FindNearestCrossing();
break;
}
};
193,11 → 226,11
FILE *pRS232;
double N, E, pomN, pomE, nn, ee;
 
pRS232 = fopen("/dev/ttyS1","r");
 
while(true)
{
pRS232 = fopen("/dev/ttyS1","r");
fscanf(pRS232,"$GPGGA,%*f,%lf,N,%lf,E,*", &N, &E); // parser NMEA
fclose(pRS232);
nn=ldiv((long)N,100).quot; // prepocet DDMM.MM na DD.DD
pomN=(N-nn*100)/60+nn;
ee=ldiv((long)E,100).quot;
205,7 → 238,7
pthread_mutex_lock(&mutex); // prepis souradnic do sdilenych promennych
n=pomN; e=pomE;
pthread_mutex_unlock(&mutex);
usleep(500000); // NMEA nechodi castejc nez 1x za 1s
usleep(800000); // NMEA nechodi castejc nez 1x za 1s
}
}