Bei linienverfolgung umdreungen der räder zählen mit odometr
Hi wieder einmal
hab da mal einen code geschrieben wo mir der asuro die anzahl von schwarz weis feldern erkennt und dan den durchschnitt der beiden rädern am schluss ausgiebt nach dem ich stop sage. Das programm funktioniert ohne die odometrie einwandfrei doch nun läuft gar nichts mehr mit diesem code. ähm ist es überhaupt möglich die odometrie auszuwerten während dem linienverfolgen??
Code:
#include "asuro.h"
#define TRIGGERLEVEL 600
#define HYSTERESIS 10
#define HIGH 1
#define LOW 0
int main(void)
{
unsigned char data[] = "0123";
unsigned int i;
unsigned int linien_verfolgen ( int );
int Odometrie_zaehler( int *a , int *b );
Init ();
while (1)
{
for (i = 0; i < 100; i++)
Sleep (72);
SerRead (data, 3, 0);
for (i = 0; i < 100; i++)
Sleep (72);
if (data[0] == 'A' && data[1] == 'S' && data[2] == 'U')
linien_verfolgen ( 120 );
else if (data[0] == 'G' && data[1] == 'O' && data[2] == 'A')
linien_verfolgen ( 155 );
else if (data[0] == 'G' && data[1] == 'O' && data[2] == 'B')
linien_verfolgen ( 180 );
else if (data[0] == 'G' && data[1] == 'O' && data[2] == 'C')
linien_verfolgen ( 200 );
else
SerWrite ("falsch", 6);
}
return 0;
}
int linien_verfolgen ( int speed )
{
unsigned int data2[2];
unsigned char data3[] = "0123";
int count1 = 0;
int count2 = 0;
int count = 0;
Init();
FrontLED(ON);
MotorDir(FWD,FWD);
while(1) {
SerRead (data3, 3, 0);
if (data3[0] == 'S' && data3[1] == 'T' && data3[2] == 'O'){
count = count1 + count2;
count = count/2;
SerWrite (" count ", 7);
PrintInt(count);
count1 = 0;
count2 = 0;
break;
}
else
{
SerWrite ("falsch", 6);
LineData(data2);
if(data2[0]>data2[1])
{
MotorSpeed(speed,0);
BackLED(ON,OFF);
Odometrie_zaehler ( count1, count2);
}
else if (data2 [0] < data2 [1] )
{
MotorSpeed(0,speed);
BackLED(OFF,ON);
Odometrie_zaehler ( count1, count2);
}
else
{
MotorSpeed(speed,speed);
BackLED(ON,ON);
Odometrie_zaehler ( count1, count2 );
}
}
}
return 0;
}
int Odometrie_zaehler ( int *a , int *b )
{
unsigned int data [2];
signed int status [2] = {0, 0};
Init();
OdometrieData(data);
if ((status [ 0 ] == LOW) && (data [0] > TRIGGERLEVEL + HYSTERESIS)) {
status [0] = HIGH;
*a ++;
}
if ((status [ 0 ] == HIGH) && (data [0] < TRIGGERLEVEL + HYSTERESIS)) {
status [0] = LOW;
*a ++;
}
if ((status [ 1 ] == LOW) && (data [1] > TRIGGERLEVEL + HYSTERESIS)) {
status [1] = HIGH;
*b ++;
}
if ((status [ 1 ] == HIGH) && (data [1] < TRIGGERLEVEL + HYSTERESIS)) {
status [1] = LOW;
*b ++;
}
return 0 ;
}
