Subversion Repositories svnkaklik

Compare Revisions

Ignore whitespace Rev 1 → Rev 2

/roboti/istrobot/3Orbis/bak/04 posledni RTOS/main/main.BAK
0,0 → 1,268
#include ".\main.h"
 
#use rtos (timer=2, minor_cycle=2ms)
 
#define STRED 128 // sredni poloha zadniho kolecka
#define BEAR1 12 // 3 stupne zataceni
#define BEAR2 34
#define BEAR3 70
#define SPEEDMAX 120 // maximalni rychlost
 
#define L 1
#define S 2
#define R 0
 
// servo
#define SERVO PIN_A2
 
// IR
#define IRTX PIN_B2
 
//motory
#define FR output_low(PIN_A7); output_high(PIN_A6) // Vpred
#define FL output_low(PIN_A1); output_high(PIN_A0)
#define BR output_low(PIN_A6); output_high(PIN_A7) // Vzad
#define BL output_low(PIN_A0); output_high(PIN_A1)
#define STOPR output_low(PIN_A6);output_low(PIN_A7)
#define STOPL output_low(PIN_A0);output_low(PIN_A1)
 
//HID
#define LED1 PIN_B1 //oranzova
#define LED2 PIN_B2 //zluta
 
#define STROBE PIN_B0
//#define SW1 PIN_A2 // Motory On/off
 
unsigned int8 sensors; // pomocna promenna pro cteni cidel na caru
unsigned int8 line; // na ktere strane byla detekovana cara
//unsigned int8 dira; // pocita dobu po kterou je ztracena cara
unsigned int8 uhel; // urcuje aktualni uhel zataceni
//unsigned int8 speed; // maximalni povolena rychlost
unsigned int8 rovinka; // pocitadlo na zjisteni rovinky
 
signed int16 Lmotor; // promene, ktere urcuji velikost vykonu na levem
signed int16 Rmotor; // a pravem motoru
 
// makro pro PWM
#define GO(motor, direction, power) if(get_timer0()<=power) \
{direction##motor;} else {stop##motor;}
 
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
/*
void diagnostika()
{
if(input(SW1))STOPR;STOPL;While(TRUE);
// if(LSENSOR==true) output_high(LED2); else output_low(LED2);
// if(RSENSOR==true) output_high(LED1); else output_low(LED1);
}
*/
////////////////////////////////////////////////////////////////////////////////
#task (rate=18ms,max=2ms)
void zatoc() // ridi servo ktere otaci kolem
{
unsigned int8 n;
 
output_high(SERVO);
delay_us(1100);
for(n=uhel; n>0; n--);
output_low(SERVO);
}
 
////////////////////////////////////////////////////////////////////////////////
void diag() // ridi servo ktere otaci kolem
{
unsigned int8 n;
 
output_high(SERVO);
delay_us(1100);
for(n=uhel; n>0; n--);
output_low(SERVO);
}
 
////////////////////////////////////////////////////////////////////////////////
short int IRcheck() // potvrdi detekci cihly
{
output_high(IRTX); // vypne vysilac IR
 
output_low(STROBE);
sensors = spi_read(0); // cteni senzoru
output_high(STROBE);
 
if(true==bit_test(sensors,7)) // otestuje, jestli je stale detekovan IR signal
{
output_low(IRTX); // zapne vysilac IR
 
output_low(STROBE);
sensors = spi_read(0); // cteni senzoru
output_high(STROBE);
 
if(false==bit_test(sensors,7)) // otestuje, jestli je detekovana cihla
{
output_high(IRTX); // vypne vysilac IR
 
output_low(STROBE);
sensors = spi_read(0); // cteni senzoru
output_high(STROBE);
 
if(bit_test(sensors,7)) return 1; //
 
else return 0; // vrat 0 kdyz je ruseni
}
else return 0; // vrat 0 kdyz to nebyla cihla
}
else return 0; // vrat 0 kdyz je detekovano ruseni
}
////////////////////////////////////////////////////////////////////////////////
/*
#task (rate=4ms, max=20us)
void rychlost()
{
if(speed<255) speed++;
}
*/
////////////////////////////////////////////////////////////////////////////////
#task (rate=2ms, max=1ms)
void rizeni()
{
unsigned int16 n;
unsigned int8 i;
 
GO(L,F,Lmotor);GO(R,F,Rmotor); // zapni motory
 
delay_us(500);
 
output_low(STROBE);
sensors = spi_read(0); // cteni senzoru
sensors=~sensors;
output_high(STROBE);
i=0; // havarijni kod
for (n=0; n<=6; n++)
{
if(bit_test(sensors,n)) i++;
}
if (i>3) rtos_terminate(); // zastavi, kdyz je cerno pod vice nez tremi cidly
 
if(bit_test(sensors,3)) //...|...//
{
uhel=STRED;
Lmotor=SPEEDMAX;
Rmotor=SPEEDMAX;
line=S;
if (rovinka<255) rovinka++;
return;
}
 
if(bit_test(sensors,0)) //|......// // z duvodu zkraceni doby reakce se cidla nevyhodnocuji poporade ale od krajnich k prostrednimu
{
uhel=STRED-BEAR3;
Lmotor=0;
Rmotor=SPEEDMAX;
line=L;
return;
}
 
if(bit_test(sensors,6)) //......|//
{
uhel=STRED+BEAR3;
Rmotor=0;
Lmotor=SPEEDMAX;
line=R;
return;
 
}
 
if(bit_test(sensors,1)) //.|.....//
{
uhel=STRED-BEAR2;
Lmotor=SPEEDMAX-50;
Rmotor=SPEEDMAX;
line=S;
return;
}
 
if(bit_test(sensors,5)) //.....|.//
{
uhel=STRED+BEAR2;
Rmotor=SPEEDMAX-50;
Lmotor=SPEEDMAX;
line=S;
return;
}
 
if (bit_test(sensors,2)) //..|....//
{
uhel=STRED-BEAR1;
Lmotor=SPEEDMAX;
Rmotor=SPEEDMAX;
line=S;
if (rovinka<255) rovinka++;
return;
}
 
if (bit_test(sensors,4)) //....|..//
{
uhel=STRED+BEAR1;
Rmotor=SPEEDMAX;
Lmotor=SPEEDMAX;
line=S;
if (rovinka<255) rovinka++;
return;
}
 
if ((L==line) || (R==line)) // Brzdeni pri vyjeti z trate
{
if (rovinka>250)
{
BL; BR;
for(n=1; n<(60); n++) {rtos_yield(); delay_us(500);};
};
rovinka=0;
}
if(bit_test(sensors,7)) // zjisti jestli neni cihla
{
rtos_terminate();
}
}
 
 
 
////////////////////////////////////////////////////////////////////////////////
void main()
{
unsigned int16 i;
unsigned int8 last;
 
setup_adc_ports(NO_ANALOGS);
setup_adc(ADC_CLOCK_INTERNAL);
setup_spi(SPI_MASTER|SPI_H_TO_L|SPI_XMIT_L_TO_H|SPI_CLK_DIV_4);
setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
setup_timer_1(T1_DISABLED);
setup_oscillator(OSC_8MHZ|OSC_INTRC);
 
STOPR; STOPL; // zastav motory
Lmotor=0;Rmotor=0;
 
uhel=STRED; // nastav zadni kolecko na stred
rovinka=0;
 
output_low(IRTX); // zapni IR vysilac
 
for(i=0;i<100;i++) // pockej, nez se zadni kolecko vystredi
{
diag();
delay_ms(16);
}
// diagnostika();
 
while(true)
{
rtos_run();
STOPR;
STOPL;
while(true);
}
}