/********************************************************************************/
*DIO_PJDDR=0xff; /*output gate*/
*DIO_PEDDR=0xff; /*output gate*/
- *DIO_PEDR=0x60; /*0x0-LED - light all; 0x6 -ENA,ENB=1, LE33CD=0*/
- *DIO_PJDR=0x00; //rozsviceni vsech diod na */
+ *DIO_PEDR=0x6f; /*0x0-LED - light all; 0x6 -ENA,ENB=1, LE33CD=0*/
+ *DIO_PJDR=0x00; //zhasnuti vsech diod na */
/*priority preruseni - SCI > TPU*/
if (init_timer3((long long)100/*ms*/*1000*1000) < 0) {
/* ERROR */
- DEB_LED_OFF(0);
+ DEB_LED_ON(0);
}
}
//printf("speed=%5d, r=%5d, sl=%5d, sr=%5d\n", speed, r, sl, sr);
}
+#define DIST_TRESHOLD 280
+
void autonomni_pohyb(void)
{
long int now = get_timer();
long int next = now;
- long int otoc_next = now;
- long int time_next = now;
+ long int straight_timout = 0;
int speed = 3000;
- int radius = 100;
- int stav = 1; // podle priznaku stavu se rozhoduje,zda pojede rovne, nebo zatoci
- int otoc = 0; // priznak pro otoceni
- int smer = 1; // smer = 1 robot jede dopredu, smer = -1 robot jede dozadu
- uint16_t vzdalenost, vzdalenost_zpet, cas=0;
+ int radius = 70;
+ enum { TURN, STRAIGHT } stav = TURN; // podle priznaku stavu se rozhoduje,zda pojede rovne, nebo zatoci
+ uint16_t voltage1, voltage2;
while (1) {
long int now = get_timer();
- vzdalenost = read_GP1();
- vzdalenost_zpet = read_GP2();
-
- if (now >= time_next){ // podminka vypisu, jednou za vterinu
- time_next = now + 10;
- cas++;
- /*Vypsani vzdalenosti na terminal*/
- //printf("Predni: %d\tZadni: %d\tSpeed: %d\tSmer: %d\tOtoc: %d\n", vzdalenost, vzdalenost_zpet, speed, smer, otoc);
- } // konec if podminky na vypis za 1s
-
- if (stav == 1 && now >= next){ // podminka pro jizdu
- stav = 0;
- move(-speed,0);
- otoc++;
- }
-
- if (stav == 0 && vzdalenost >= 280 && smer == 1){ // podminka pro zatoceni 260 odpovida (mensi nez 20cm)
-
- if (rand() % 100 < 70) // 50 % predpoklad pro zatoceni doprava
- move(-speed,radius); // zatoc rychlosti speed a polomerem radius(doprava)
- else
- move(-speed,-radius); // zatoc rychlosti speed a polomerem -radius (doleva)
- stav = 1;
-
- next = now + ((rand() % 8)+5); // zataceni po dobu 5 - 12 (nahodne cislo)
- } // konec if
-
+ voltage1 = read_GP2();
+ voltage2 = read_GP1();
- if (stav == 0 && vzdalenost_zpet >= 280 && smer == -1){ // jizda zpet!!! podminka pro zatoceni 260 odpovida (mensi nez 20cm)
+ if (voltage1 >= DIST_TRESHOLD) DEB_LED_ON(1);
+ else DEB_LED_OFF(1);
- if (rand() % 100 < 70) // 50 % predpoklad pro zatoceni doprava
- move(-speed,radius); // zatoc rychlosti speed a radiusem,
- else
- move(-speed,-radius);
- stav = 1;
-
- next = now + ((rand() % 8)+5); // zataceni po dobu 5 - 12 (nahodne cislo)
- } //konec if
-
-// NEVIM JAK UDELAT OPRENI SE O PREKAZKU A OTOCENI // osetreni, kdyz se robot opre o prekazku, chceme,aby se rozjel opacnym smerem
- /*if (vzdalenost <=30 && now >= otoc_next) { // pokud se sensor dotyka prekazky, tak dava hodnotu 110 az 140, kontroluji to 10x 0,1s
- otoc++;
- otoc_next = now + 1;
- }
-
- if (vzdalenost_zpet >= 110 && vzdalenost_zpet <=140 && now >= otoc_next) { // pokud se sensor dotyka prekazky, tak dava hodnotu 110 az 140, kontroluji to 10x 0,1s
- otoc++;
- otoc_next = now + 1;
+ if (voltage2 >= DIST_TRESHOLD) DEB_LED_ON(2);
+ else DEB_LED_OFF(2);
+
+ if (stav == TURN && now >= next){ // podminka pro jizdu
+ stav = STRAIGHT;
+ straight_timout = 10/*sec*/*10 + now;
+ move(speed,0);
}
- if (otoc >= 10){
- speed = -speed; // zmena smeru
- otoc = 0;
- } */
-
- //zmena_smeru
- if (otoc >= 6){
- speed = -speed;
- otoc = 0;
- smer = -smer;
- move(-speed,0); //otoc normalne je pouzit o if cyklus drive, ted si ho jen pujcuji
+ if (stav == STRAIGHT) {
+ if (now >= straight_timout) {
+ //Go backward
+ speed = -speed;
+ move(speed,0); //otoc normalne je pouzit o if cyklus drive, ted si ho jen pujcuji
+ }
+ if ((voltage1 >= DIST_TRESHOLD && speed > 0) ||
+ (voltage2 >= DIST_TRESHOLD && speed < 0)) { // podminka pro zatoceni 260 odpovida (mensi nez 20cm)
+ //Obstacle detected
+ if (rand() % 100 < 50) // 50 % predpoklad pro zatoceni doprava
+ move(speed,radius); // zatoc rychlosti speed a polomerem radius(doprava)
+ else
+ move(speed,-radius); // zatoc rychlosti speed a polomerem -radius (doleva)
+ stav = TURN;
+
+ next = now + ((rand() % 5)+8); // zataceni po dobu 5 - 12 (nahodne cislo)
+ } // konec if
}
-
} //konec while
} //konec funkce
projects / mirosot.git / commitdiff