Subversion Repositories svnkaklik

Compare Revisions

Ignore whitespace Rev 371 → Rev 372

/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=0xA2) // Motor 2
 
#define H1 PIN_A1
#define L1 PIN_A2
46,17 → 46,14
enable_interrupts(GLOBAL);
enable_interrupts(INT_SSP);
 
command=0; // zastaveni po resetu
command=-128; // zastaveni po resetu
 
while(true)
{
 
if ((0==command) || (command>127) || (command<-127)) // prikaz na odpojeni mustky nebo chybna hodnota
if (command==-128) // prikaz na odpojeni mustku nebo chybna hodnota
{
output_low(H1); // stop
output_low(H2);
output_low(L1);
output_low(L2);
output_a(0); // volnobeh
continue;
};
 
/roboti/Robotour/SW/motor/motor.hex
0,0 → 1,35
:1000000000308A007B280000FF00030E8301A1005E
:100010007F08A0000A08A8008A01A00E0408A20018
:100020007708A3007808A4007908A5007A08A6003C
:100030007B08A700831383128C308400801D222844
:100040008C193528220884002308F7002408F800BA
:100050002508F9002608FA002708FB0028088A006E
:10006000210E8300FF0E7F0E09008A114528130818
:100070003008930014168C118316141C4128831227
:100080003C28F80183128A1168288316850183129F
:1000900085018316941A51288312AA011318AA17EE
:1000A000831683122A08AA0AAF002F087F3C031C7C
:1000B0006128831614185E288312592883121308A6
:1000C000A9002F08803C031D68282908B0003728A4
:1000D0008C118A1122280830AD02031C7A282D3099
:1000E00084000310800C000803197A287828000087
:1000F000800B7728003484011F308305703083160D
:100100008F0006168614A2309300363083129400B6
:1001100083161F129F121B0880399B0007309C001A
:1001200003308312AA0083161F129F121B08803906
:100130009B0083121F1083160108C03981000B3009
:10014000F700073083128101813084000008F03904
:100150000738800064000008F739F719F039770490
:10016000800090010030F800920000308316920069
:1001700007309C0005080630F700F70BBD2800008B
:100180001C0883120D1383169D0172308F00C0303E
:1001900083128B0483168C1580308312A9002908E2
:1001A000803C031DD8288316850183128501CF2842
:1001B0007F302907AB00831685011230831285003A
:1001C0002B08AD006B20831685018312850100008A
:1001D00000006400831685010C30831285002B0813
:1001E000FE3CAC00AD006B208316850183128501B7
:0801F00000000000CF286300AD
:04400E00343FFC3F00
:00000001FF
;PIC16F88
/roboti/Robotour/SW/vector/vector.cpp
36,14 → 36,15
 
using namespace std;
 
#define CMPS03_SOFTWARE_REVISION 0x0
#define SRF02_SOFTWARE_REVISION 0x0
 
#define BC_Addr 0x0B
#define US3_Addr 0x70 // 0xE0 in fact; Sonar na doprovod
#define US3_Addr 0x70 // 0xE0 in fact; Sonar na doprovod
#define CMPS_Addr 0x60 // 0xC0
#define M1 0x50 // 0xA0 in fact
#define M2 0x51 // 0xA2 in fact
 
#define SEVER 0
 
char vystup[50];
pthread_t thread_1, thread_2, thread_3;
FILE *pRouraO,*pRouraI;
51,7 → 52,9
char command,ble;
int file;
double n, e;
unsigned char azimut_mag;
 
 
pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
 
void *print_tele(void *unused);
67,7 → 70,7
}
}
 
void go (int Addr, int speed)
void go (int Addr, int speed) // nastavi rychlost motoru
{
char Buf[1];
 
76,7 → 79,7
write(file, Buf, 1);
}
 
unsigned int echo(int Addr)
unsigned int echo(int Addr) // precte vzdalenost z US cidla
{
char Buf[3];
 
105,12 → 108,8
} while(++n<POINTS);
}
 
int main(int argc, char *argv[], char *envp[])
int i2c_init() // zinicializuje i2c
{
int filtr;
 
fprintf(stdout, "**** Vector Control Programm ****\n");
 
file = open("/dev/i2c-0", O_RDWR);
if (file < 0)
{
117,7 → 116,45
cerr << "Could not open /dev/i2c-0." << endl;
return -1;
}
return 0;
}
 
int read_azimut_mag() // precte azimut z kompasu
{
char Buf[3]; // promena pro manipulaci s i2c
I2C_addr(CMPS_Addr);
Buf[0]=1;
write(file,Buf,1);
read(file, Buf,1);
return Buf[0];
}
 
void turnL(unsigned char angle, signed char speed) // otoci robota o zadany uhel
{
int azimut;
 
go(M1, speed);
go(M2, -speed);
azimut=read_azimut_mag();
while(read_azimut_mag() >= (azimut + angle)) usleep(10000);
go(M1, 0);
go(M2, 0);
command=0;
}
 
void turnR() // otoci robota o zadany uhel
{
}
 
int main(int argc, char *argv[], char *envp[])
{
int filtr;
signed char test;
 
fprintf(stdout, "\n **** Starting Vector Control Programm **** \n \r");
 
i2c_init();
 
pthread_create(&thread_1, NULL, print_tele, NULL);
pthread_create(&thread_3, NULL, gps, NULL);
// pthread_create(&thread_2, NULL, sensors, NULL);
139,18 → 176,52
go(M2, -70);
command=0;
break;
case 'v': // stop
case 'v': // volnobeh
go(M1, -128);
go(M2, -128);
command=0;
break;
case 's': // stop
go(M1, 0);
go(M2, 0);
command=0;
break;
case 's': // stop
go(M1, 1);
go(M2, 1);
case 'a': // test otaceni
go(M1, 100);
go(M2, -100);
azimut_mag=read_azimut_mag();
while(read_azimut_mag() >= (azimut_mag + 45)) usleep(10000);
go(M1, 0);
go(M2, 0);
command=0;
break;
 
case 't': // test
for(test=0;test<127;test++)
{
go(M1, test);
go(M2, test);
usleep(10000);
};
go(M1, 127);
go(M2, 127);
for(test=127;test>-128;test--)
{
go(M1, test);
go(M2, test);
usleep(10000);
};
go(M1, -128);
go(M2, -128);
command=0;
break;
 
case 'g':
usleep(180000);
usleep(180000); // simulace ostatnich cidel (zdrzeni)
azimut_mag=read_azimut_mag();
 
FindNearestCrossing();
 
vzdalenost=echo(US3_Addr);
if ((vzdalenost>60)&&(vzdalenost<80))
{
183,7 → 254,6
filtr=6;
}
};
FindNearestCrossing();
break;
}
};
215,6 → 285,7
fprintf(pRouraO,"%f N %f E\n", n, e);
fprintf(pRouraO,"Vzdalenost: %.1f m\n", GeoCalc::EllipsoidDistance(n, e, cros[1].n, cros[1].e));
fprintf(pRouraO,"Azimut: %.2f Deg\n", GeoCalc::GCAzimuth(n, e, cros[1].n, cros[1].e));
fprintf(pRouraO,"AzimutMag: %d (0-255)\n", azimut_mag);
pthread_mutex_unlock(&mutex);
 
fclose(pRouraO);
/roboti/Robotour/SW/wdt/wdt.c
0,0 → 1,17
#include "C:\dokumenty\svn\Kaklik\roboti\Robotour\SW\wdt\wdt.h"
 
 
void main()
{
 
setup_adc_ports(NO_ANALOGS|VSS_VDD);
setup_adc(ADC_OFF);
setup_spi(FALSE);
setup_timer_0(RTCC_INTERNAL);setup_wdt(WDT_144MS);
setup_timer_1(T1_DISABLED);
setup_timer_2(T2_DISABLED,0,1);
setup_comparator(NC_NC_NC_NC);
setup_vref(VREF_LOW|-2);
setup_oscillator(False);
 
}
/roboti/Robotour/SW/wdt/wdt.h
0,0 → 1,18
#include <16F88.h>
#device adc=8
 
#FUSES WDT //Watch Dog Timer
#FUSES INTRC //Internal RC Osc
#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=8000000,RESTART_WDT)
 
/roboti/Robotour/SW/wdt/wdt.hex
0,0 → 1,13
:1000000000308A000428000084011F30830570300E
:1000100083168F001F129F121B0880399B00073028
:100020009C001F129F121B0880399B0083121F1017
:10003000941283160611861406120030831294005F
:10004000831694000108C03981000B30F700073097
:1000500083128101813084000008F0390738800064
:1000600064000008F739F719F0397704800090012F
:100070000030F800920000308316920007309C0098
:10008000050864000630F700F70B44281C088312AB
:100090000D13FE3083169D0005158F010F086300B8
:04400E003D3FFC3FF7
:00000001FF
;PIC16F88