Subversion Repositories svnkaklik

Compare Revisions

Problem with comparison.

Ignore whitespace Rev HEAD → Rev 1

/roboti/merkur/PIC16F88/verze/1 tank.c
0,0 → 1,313
#include "tank.h"
 
#define TXo PIN_B1 // To the transmitter modulator
#include "AX25.c" // podprogram pro prenos telemetrie
 
//motory //Napred vypnout potom zapnout!
#define FR output_low(PIN_B5); output_high(PIN_B4) // Vpred
#define FL output_low(PIN_B7); output_high(PIN_B6)
#define BR output_low(PIN_B4); output_high(PIN_B5) // Vzad
#define BL output_low(PIN_B6); output_high(PIN_B7)
#define STOPR output_low(PIN_B4);output_low(PIN_B5)
#define STOPL output_low(PIN_B6);output_low(PIN_B7)
 
#define L 0b10 // left
#define R 0b01 // right
#define S 0b11 // straight
 
#define COUVANI 1600 // couvnuti po zjisteni diry
#define MEZERA 5400 // za jak dlouho bude ztracena cara
#define PRES_DIRU 1000 // velikost mezery v care
#define BRZDENI 5000 // doba ptrebna k zastaveni jednoho motoru
 
//cidla
#define RSENSOR C2OUT // Senzory na caru
#define LSENSOR C1OUT
#define BUMPER PIN_A4 // sensor na cihlu
 
#define DIAG_SERVO PIN_B2 // Propojka pro diagnosticky mod
#define DIAG_SENSORS PIN_B3 // Propojka pro diagnosticky mod
 
#DEFINE SOUND_HI PIN_A7
#DEFINE SOUND_LO PIN_A6
 
char AXstring[40]; // Buffer pro prenos telemetrie
 
#define GO(motor, direction, power) if(get_timer0()<=power) \
{direction##motor;} \
else \
{stop##motor;}
 
int movement; // smer minuleho pohybu
int line; // na ktere strane byla detekovana cara
unsigned int16 dira; // pocitadlo pro nalezeni preruseni cary
int speed,speedL,speedR;
 
// Primitivni Pipani
void beep(unsigned int16 period, unsigned int16 length)
{
unsigned int16 nn;
 
for(nn=length; nn>0; nn--)
{
output_high(SOUND_HI);output_low(SOUND_LO);
delay_us(period);
output_high(SOUND_LO);output_low(SOUND_HI);
delay_us(period);
}
}
#int_TIMER2
void TIMER2_isr()
{
switch(line) // upravime smer
{
case S: //obe cidla na care
if(speedL<200)speedL++;
if(speedR<200)speedR++;
break; // vrat se zpet na cteni cidel
case L: // cara je pod levym cidlem, trochu zatocime
if (speedL>100)speedL -- ;
if (speedR<200)speedR ++ ;
break;
case R: // cara pod pravym cidlem
if (speedR>100)speedR -- ;
if (speedL<200)speedL ++ ;
break;
default:
}
}
// Diagnostika pohonu, hejbne vsema motorama ve vsech smerech
void diagnostika()
{
unsigned int16 n;
 
while (input(DIAG_SERVO)) // Propojka, ktera spousti diagnostiku
{
for (n=500; n<800; n+=100)
{
beep(n,n); //beep UP
};
Delay_ms(1000);
//zastav vse
STOPL; STOPR;
//pravy pas
FR; Delay_ms(1000); STOPR; Delay_ms(1000);
BR; Delay_ms(1000); STOPR; Delay_ms(1000);
Beep(880,100); Delay_ms(1000);
//levy pas
FL; Delay_ms(1000); STOPL; Delay_ms(1000);
BL; Delay_ms(1000); STOPL; Delay_ms(1000);
Beep(880,100); Delay_ms(1000);
//oba pasy
FL; FR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000);
BL; BR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000);
};
 
while (input(DIAG_SENSORS))
{
int ls, rs;
while(!input(BUMPER)){beep(1100,100); Delay_ms(50);}
set_adc_channel(RSENSOR);
Delay_us(20);
rs=read_adc();
set_adc_channel(LSENSOR);
Delay_us(20);
ls=read_adc();
sprintf(AXstring,"L: %U R: %U\0", ls, rs); // Convert DATA to String.
SendPacket(&AXstring[0]);
delay_ms(1000);
};
}
 
void cikcak()
{
int n;
switch(movement) // podivej se na jednu stranu
{
case L:
FL;BR;
movement=R;
break;
case R:
FR;BL;
movement=L;
break;
case S:
FR;BL;
movement=L;
break;
}
while (0==(RSENSOR|LSENSOR))
{
if (n==50) // asi bude na druhe strane
{
STOPR;STOPL;
n=0;
switch(movement)
{
case L:
FL;BR;
movement=R;
break;
case R:
FR;BL;
movement=L;
break;
}
}
Delay_ms(5);
n++;
}
STOPL;STOPR; // nasli jsme caru
line=S;
}
void objizdka()
{
BL;BR;Delay_ms(300);
STOPR;STOPL;
beep(1000,1000);
Delay_ms(500);
beep(1000,1000);
Delay_ms(1000);
 
 
 
}
 
void pozordira()
{
beep(800,500);
Delay_ms(50);
beep(800,500);
switch (movement) //vrat se zpet na caru
{
case L:
STOPL;STOPR;
BR;Delay_ms(COUVANI);STOPR;
break;
case R:
STOPL;STOPR;
BL;Delay_ms(COUVANI);STOPL;
break;
case S:
BL; BR; Delay_ms(COUVANI);
STOPL; STOPR;
break;
}
 
FR;FL; Delay_ms(PRES_DIRU); // popojedem dopredu mozna tam bude cara
STOPL; STOPR; movement=S;
cikcak(); // najdi caru
dira=0;
}
 
void main()
{
unsigned int16 rovinka;
int last;
 
STOPL; STOPR;
 
setup_oscillator(OSC_4MHZ|OSC_INTRC); // 4 MHz interni RC oscilator
port_b_pullups(true);
setup_adc_ports(NO_ANALOGS|VSS_VDD);
setup_adc(ADC_OFF);
setup_spi(FALSE);
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); // Casovac pro PWM
setup_timer_1(T1_DISABLED); // Casovac pro regulaci
setup_timer_2(T2_DIV_BY_16,50,16);
setup_ccp1(CCP_OFF);
setup_comparator(A0_VR_A1_VR);
setup_vref(VREF_HIGH|15);
enable_interrupts(INT_TIMER2);
enable_interrupts(GLOBAL);
 
diagnostika();
 
Beep(1000,200); //double beep
Delay_ms(50);
Beep(1000,200);
Delay_ms(1000); // 1s
 
// FL; FR;
movement=S;
line=S;
dira=0;
last=0;
rovinka=0;
 
speed=speedL=speedR=200;
 
while(true)
{
line = RSENSOR; // precteni cidel
line |= LSENSOR << 1; // sestav informaci o care
if(line!=0)
{
GO(L, F, speedL); GO(R,F, speedR);
}
else{STOPR; STOPL;}
//sem:
/* switch(line) // upravime smer
{
case S: //obe cidla na care
// if (speedL<speedR) speedL=speedR;
// else speedR=speedL;
GO(L,F,speedL); GO(R,F,speedR) // jedeme rovne
// if(rovinka<BRZDENI) rovinka++; //cara je rovne
// dira=0; // videli jsme caru, proto neni dira
continue; // vrat se zpet na cteni cidel
case L: // cara je pod levym cidlem, trochu zatocime
GO(L, F, speedL); GO(R,F, speedR);
// if(rovinka<BRZDENI) rovinka++; //cara je celkem rovne
// dira=0;
continue;
case R: // cara pod pravym cidlem
GO(R, F, speedR); GO(L, F, speedL);
// if(rovinka<BRZDENI) rovinka++;
// dira=0;
continue;
default: // cara neni pod zadnym cidlem
}*/
 
 
/*switch (last) // zatacka
{
case L: // do leva
BL;STOPR; //zabrzdeni leveho motoru
for(;rovinka>0;rovinka--) //chvili pockej
{
line = RSENSOR; // precteni cidel
line |= LSENSOR << 1; // sestav informaci o care
if(line!=0) goto sem; //kdyz najdes caru, zastav
}
STOPL; FR; // pokracuj v zataceni
while(line==0)
{
line = RSENSOR; // precteni cidel
line |= LSENSOR << 1; // sestav informaci o care
}
movement=L;
rovinka=0; //zataceli jsme, uz neni rovna cara
break;
case R:
BR; STOPL; // zabrzdeni praveho motoru
for(;rovinka>0;rovinka--)
{
line = RSENSOR; // precteni cidel
line |= LSENSOR << 1; // sestav informaci o care
if(line!=0) goto sem;
}
STOPR; FL;
while(line==0)
{
line = RSENSOR; // precteni cidel
line |= LSENSOR << 1; // sestav informaci o care
}
movement=R;
rovinka=0; //zataceli jsme, uz neni rovna cara
break;
}*/
} // while(true)
}
/roboti/merkur/PIC16F88/verze/2 tank.c
0,0 → 1,331
#include "tank.h"
 
#define DEBUG
 
#define TXo PIN_A3 // To the transmitter modulator
#include "AX25.c" // podprogram pro prenos telemetrie
 
unsigned int8 sensors; // pomocna promenna pro cteni cidel na caru
unsigned int8 line; // na ktere strane byla detekovana cara
unsigned int8 speed; // rychlost zataceni
unsigned int8 rovinka; // pocitadlo pro zjisteni rovneho useku
unsigned int8 last; // kde byla cara, kdyz byly minule cidla mimo
unsigned int8 movement; // obsahuje aktualni smer zataceni
unsigned int8 dira; // pocita dobu po kterou je ztracena cara
 
// Konstanty pro dynamiku pohybu
#define T_DIRA 100 // po jakem case zataceni se detekuje dira
#define INC_SPEED 1 // prirustek rychlosti v jednom kroku
#define FW_POMALU 100 // trochu mimo caru vnitrni pas
#define FW_ZATACKA 200 // rychlost vnejsiho kola pri zataceni
#define FW_STREDNE 110 // trochu mimo caru vnejsi pas
#define COUVANI 600 // couvnuti zpet na caru, po detekci diry
#define PRES_DIRU 300
#define MAX_ROVINKA (255-FW_STREDNE)
#define TRESHOLD 6 // rozhodovaci uroven komparatoru, 0xF = 0.75*Vdd
 
//motory //Napred vypnout potom zapnout!
#define FR output_low(PIN_B5); output_high(PIN_B4) // Vpred
#define FL output_low(PIN_B7); output_high(PIN_B6)
#define BR output_low(PIN_B4); output_high(PIN_B5) // Vzad
#define BL output_low(PIN_B6); output_high(PIN_B7)
#define STOPR output_low(PIN_B4);output_low(PIN_B5) // Zastav
#define STOPL output_low(PIN_B6);output_low(PIN_B7)
 
#define L 0b10 // left
#define R 0b01 // right
#define S 0b11 // straight
 
//cidla
#define RSENSOR C2OUT // Senzory na caru
#define LSENSOR C1OUT
#define BUMPER PIN_A4 // Senzor na cihlu
 
#define DIAG_SERVO PIN_B3 // Propojka pro diagnosticky mod
#define DIAG_SENSORS PIN_B2 // Propojka pro diagnosticky mod
 
#DEFINE SOUND_HI PIN_A6 // komplementarni vystupy pro piezo pipak
#DEFINE SOUND_LO PIN_A7
 
char AXstring[40]; // Buffer pro prenos telemetrie
 
// makro pro PWM
#define GO(motor, direction, power) if(get_timer0()<=power) \
{direction##motor;} \
else \
{stop##motor;}
 
#int_TIMER2
void TIMER2_isr()
{
if (speed<255) speed+=INC_SPEED;
if (rovinka<MAX_ROVINKA) rovinka++;
if (dira<T_DIRA) dira++;
}
 
// Primitivni Pipani
void beep(unsigned int16 period, unsigned int16 length)
{
unsigned int16 nn;
 
for(nn=length; nn>0; nn--)
{
output_high(SOUND_HI);output_low(SOUND_LO);
delay_us(period);
output_high(SOUND_LO);output_low(SOUND_HI);
delay_us(period);
}
}
/******************************************************************************/
void diagnostika()
{
unsigned int16 n;
 
while (input(DIAG_SERVO)) // Propojka, ktera spousti diagnostiku
{
for (n=500; n<800; n+=100)
{
beep(n,n); //beep UP
};
Delay_ms(1000);
//zastav vse
STOPL; STOPR;
//pravy pas
FR; Delay_ms(1000); STOPR; Delay_ms(1000);
BR; Delay_ms(1000); STOPR; Delay_ms(1000);
Beep(880,100); Delay_ms(1000);
//levy pas
FL; Delay_ms(1000); STOPL; Delay_ms(1000);
BL; Delay_ms(1000); STOPL; Delay_ms(1000);
Beep(880,100); Delay_ms(1000);
//oba pasy
FL; FR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000);
BL; BR; Delay_ms(1000); STOPL; STOPR; Delay_ms(1000);
};
 
while (input(DIAG_SENSORS))
{
int ls, rs;
while(!input(BUMPER)){beep(1100,100); Delay_ms(50);}
set_adc_channel(RSENSOR);
Delay_us(20);
rs=read_adc();
set_adc_channel(LSENSOR);
Delay_us(20);
ls=read_adc();
sprintf(AXstring,"L: %U R: %U\0", ls, rs); // Convert DATA to String.
SendPacket(&AXstring[0]);
delay_ms(1000);
};
}
///////////////////////////////////////////////////////////////////////////////
void cikcak()
{
int n;
switch(movement) // podivej se na jednu stranu
{
case L:
FL;BR;
movement=R;
break;
case R:
FR;BL;
movement=L;
break;
case S:
FR;BL;
movement=L;
break;
}
while (0==(RSENSOR|LSENSOR))
{
if (n==50) //cara asi bude na druhe strane
{
STOPR;STOPL;
n=0;
switch(movement)
{
case L:
FL;BR;
movement=R;
break;
case R:
FR;BL;
movement=L;
break;
}
}
Delay_ms(5);
n++;
}
STOPL;STOPR; // nasli jsme caru
line=S;
}
///////////////////////////////////////////////////////////////////////////////
void objizdka()
{
BL;BR;Delay_ms(300);
STOPR;STOPL;
beep(1000,1000);
Delay_ms(500);
beep(1000,1000);
Delay_ms(1000);
 
 
 
}
///////////////////////////////////////////////////////////////////////////////
void prejeddiru()
{
STOPL;STOPR;
beep(800,500);
Delay_ms(50);
beep(800,500);
switch (movement) //vrat se zpet na caru
{
case L:
BR;Delay_ms(COUVANI);STOPR;
STOPL;STOPR;
break;
case R:
 
BL;Delay_ms(COUVANI);STOPL;
STOPL;STOPR;
break;
case S:
BL; BR; Delay_ms(COUVANI);
STOPL; STOPR;
break;
}
 
FR;FL; Delay_ms(PRES_DIRU); // popojedem dopredu mozna tam bude cara
STOPL; STOPR;
cikcak(); // najdi caru
dira=0;
}
///////////////////////////////////////////////////////////////////////////////
void main()
{
unsigned int16 n; // pro FOR
 
STOPL; STOPR; // prepne vystupy na ovladani motoru na output a zastavi
 
setup_oscillator(OSC_4MHZ|OSC_INTRC); // 4 MHz interni RC oscilator
 
port_b_pullups(TRUE); // pullups pro piano na diagnostiku
setup_spi(FALSE);
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); // Casovac pro PWM
 
setup_timer_2(T2_DIV_BY_16,255,10); // Casovac pro regulaci
// preruseni kazdych 10ms
setup_adc_ports(sAN2|VSS_VDD); // nastaveni A/D prevodniku pro naraznik
setup_adc(ADC_CLOCK_INTERNAL);
set_adc_channel(2);
setup_timer_1(T1_INTERNAL|T1_DIV_BY_1); // Casovac pro naraznik
setup_ccp1(CCP_COMPARE_RESET_TIMER);
CCP_1=(2^10)-1; // prevod kazdou 1ms
 
setup_comparator(A0_VR_A1_VR); // inicializace komparatoru pro cidla cary
setup_vref(VREF_HIGH|TRESHOLD); // 32 kroku od 0.25 do 0.75 Vdd
 
Beep(1000,200); //double beep
Delay_ms(50);
Beep(1000,200);
Delay_ms(1000); // 1s
 
// povoleni rizeni rychlosti zataceni pres preruseni
enable_interrupts(INT_TIMER2);
enable_interrupts(GLOBAL);
 
/*---------------------------------------------------------------------------*/
sensors=S;
line=S;
last=S;
movement=S;
speed=FW_POMALU;
 
diagnostika();
cikcak(); // toc se, abys nasel caru
Delay_ms(500);
Beep(1000,200);
Delay_ms(500);
 
while(true) // hlavni smycka (jizda podle cary)
{
 
sensors = RSENSOR; // cteni senzoru na caru
sensors |= LSENSOR << 1;
 
switch (sensors) // zatacej podle toho, kde vidis caru
{
case S: // rovne
GO(L, F, speed); GO(R, F, speed);
// FL; FR; // pokud se jede dlouho rovne, tak pridej
dira=0;
continue;
 
case L: // trochu vlevo
GO(L, F, FW_POMALU+rovinka); GO(R, F, FW_STREDNE+rovinka);
line=L;
dira=0;
continue;
 
case R: // trochu vpravo
GO(R, F, FW_POMALU+rovinka); GO(L, F, FW_STREDNE+rovinka);
line=R;
dira=0;
continue;
 
default: // kdyz jsou obe cidla mimo caru, tak pokracuj dal
}
rovinka=0;
// if (dira>=T_DIRA) prejeddiru();
if (last!=line) // pokud si prejel caru z jedne strany na druhou stranu, tak zabrzdi
{
last=line;
speed=FW_ZATACKA;
}
else speed=255;
/* if (dira==0)
{
if (L==line) // kdyz jsou obe cidla mimo caru, zabrzdi vnitrni kolo
{
BL;
for(n=4000;n>0;n--) // Delay
{
line = RSENSOR; // precteni cidel
line |= LSENSOR << 1; // sestav informaci o care
if(line!=0) break;
}
STOPL;
}
else
{
BR;
for(n=4000;n>0;n--) // Delay
{
line = RSENSOR; // precteni cidel
line |= LSENSOR << 1; // sestav informaci o care
if(line!=0) break;
}
STOPR;
}
}*/
if (L==line) // kdyz jsou obe cidla mimo caru, zatoc na caru
{
STOPL;
GO(R, F, speed);
movement=L;
}
else
{
STOPR;
GO(L, F, speed);
movement=R;
}
} // while(true)
}