/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 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 |