Subversion Repositories svnkaklik

Compare Revisions

Ignore whitespace Rev 1 → Rev 2

/roboti/istrobot/merkur/PIC16F873/Kopie (2) - main.c
0,0 → 1,236
#include "main.h"
 
int movement; // smer minuleho pohybu
int line; // na ktere strane byla detekovana cara
unsigned int8 dira; // pocitadlo pro nalezeni preruseni cary
unsigned int8 speed; // rychlost zataceni
unsigned int8 straight; // pocitadlo pro zjisteni rovneho useku
 
// Konstanty pro dynamiku pohybu
#define T_DIRA 16 // po jakem case zataceni se detekuje dira
#define INC_SPEED 5 // prirustek rychlosti v jednom kroku
#define RIGHT_ANGLE 1000 // 90 stupnu
#define CIKCAK 20000 // 45 stupnu
#define BW_PO_DIRE 200 // zpetny chod po dire
#define FW_RYCHLE 200 // cara primo rovne
#define FW_POMALU 100 // trochu mimo caru vnitrni pas
#define FW_STREDNE 150 // trochu mimo caru vnejsi pas
#define TURN_MIN 90 // minimalni rychlost pri zataceni
#define TURN_MAX 150 // miximalni rychlost pri zataceni
#define BRZDENI 40 // doba zpetneho chodu v ms, aby pas stal
#define ROVINKA 8 // doba po kterou se musi jet rovne, aby se brzdilo
 
//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 1 // Senzory na caru
#define LSENSOR 0
#define BUMPER PIN_C4 // Senzor na cihlu
 
#define DIAG_SERVO PIN_B0 // Propojka pro diagnosticky mod
#define DIAG_SENSORS PIN_B1 // Propojka pro diagnosticky mod
 
#DEFINE SOUND_HI PIN_B3 // komplementarni vystupy pro piezo pipak
#DEFINE SOUND_LO PIN_B2
 
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_TIMER1 // This function is called every time
void TIMER1_isr() { // the RTCC (timer0) overflows (255->0).
// For this program this is apx 76 times
// per second.
if (speed<TURN_MAX) speed+=INC_SPEED;
if (dira<255) dira++;
if (straight<255) straight++;
}
 
// 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();
};
}
void main()
{
STOPL; STOPR; // prepne vystupy na ovladani motoru na output a zastavi
setup_adc_ports(RA0_RA1_RA3_ANALOG);
setup_adc(ADC_CLOCK_DIV_2);
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1); // Casovac pro PWM
setup_timer_1(T1_INTERNAL|T1_DIV_BY_1); // Casovac pro regulaci
setup_timer_2(T2_DISABLED,0,1);
setup_ccp1(CCP_OFF);
diagnostika();
 
Beep(1000,200); //double beep
Delay_ms(50);
Beep(1000,200);
Delay_ms(1000); // 1s
 
 
speed=TURN_MIN; // povoleni rizeni rychlosti zataceni pres preruseni
enable_interrupts(INT_TIMER1);
enable_interrupts(GLOBAL);
 
// Cik-Cak -------------------------------------------------------------
line=S;
movement=R;
// cik_cak(); // toc se, abys nasel caru
Delay_ms(500);
Beep(1000,200);
Delay_ms(500);
 
dira=0; // inicializace globalnich promennych
straight=255;
 
while(true) // hlavni smycka (jizda podle cary)
{
set_adc_channel(LSENSOR); // kdyz cara nebyla pod pravym cidlem, mozna bude pod levym
Delay_us(10);
if(tresholdL > read_adc())
{
movement = L;
if (straight>ROVINKA)
{GO(R, F, FW_STREDNE+40); GO(L, F, FW_POMALU+40)} // pridej
else
{GO(R, F, FW_STREDNE); GO(L, F, FW_POMALU)};
speed=TURN_MIN;
dira=0;
line=L;
continue;
}
set_adc_channel(RSENSOR); // podivej se jestli neni cara pod pravym cidlem
Delay_us(10);
if(tresholdR > read_adc())
{
movement = R;
if (straight>ROVINKA)
{GO(L, F, FW_STREDNE+40); GO(R, F, FW_POMALU+40)} // pridej
else
{GO(L, F, FW_STREDNE); GO(R, F, FW_POMALU)};
speed=TURN_MIN;
dira=0;
line=R;
continue;
}
if(line==S)
movement = S;
if (straight>ROVINKA)
{FL; FR;} // pokud se jede dlouho rovne, tak pridej
else
{GO(R, F, FW_RYCHLE); GO(L, F, FW_RYCHLE)};
speed=TURN_MIN; // nastav minimalni rychlost pro zataceni
dira=0; // protoze byla cara, tak nuluj pocitadlo diry
continue;
 
 
if (straight>ROVINKA) // pokud byla dlouha rovinka, tak zabrzdi
{
BL; BR;
Delay_ms(BRZDENI);
STOPL;
STOPR;
dira=0;
};
straight=0; // nuluj pocitadlo rovinky
 
if (L==movement) // kdyz jsou obe cidla mimo caru, zatoc na caru
{
GO(R, F, speed);
STOPL;
}
else
{
GO(L, F, speed);
STOPR;
}
 
if (dira>T_DIRA) // pokud se moc dlouho zataci bez detekce cary, vrat se
{
STOPL;
STOPR;
Beep(1000,200); //double beep
Delay_ms(30);
Beep(2000,200);
Delay_ms(30);
if (L==movement) // zpet, podle toho kam se jelo
{
STOPL;
BR;
}
else
{
STOPR;
BL;
};
Delay_ms(BW_PO_DIRE);
STOPL;
STOPR;
cik_cak(); // najdi caru
dira=0;
}
 
} // while(true)
}