Hallo und nen Guten.

Kleines Problem bittet um anregung zur Lösung.

Ich hab das Program Example_05_Move_05 um ein weiteres Verhalten erweitert. Zwischen Cruise und Avoid sollen nun die Daten (distance) eines SRF08 verarbeitet werden.

RP6 soll mit Speed 160 fahren und vor einem Hindernis das 1 m entfernt ist soll der Speed auf 100 gesetzt werden, um sich dann mit Avoid und Escape ran zutasten.

Die Distance greife ich vom ExampleI²C_Master_03 ab.

Das Problem ist nun das ich keine saubere distance rein bekomme.
Ich hab einen Stopwatch implementiert aber der scheint mir nix zu bringen.

Momentaner Stand:
RP6 fährt mit Speed 160 los und auf dem Weg zum Hindernis regelt er ab und an auf 100 und gibt dann wieder Gas. Ab einen Meter vorm Hindernis fährt er konstant 100 und Avoid und Escape springen dann mit ein.

Also der Weg ist mein Ziel

Hier ist mein code Segment

Code:
/*****************************************************************************/
// Slow Behaviour:

#define SLOW_SPEED_FWD   40 

#define SLOW 3
#define RANGE_MAX 100
behaviour_command_t slow = {0, 0, FWD, false, false, 0, IDLE};

void behaviour_slow(void)
{
  switch(slow.state)
  {
	case IDLE:
		if(distance>=0)
		{
			setStopwatch8(0);
			startStopwatch8();
			slow.state = SLOW;
		}
	break;
	case SLOW:

			if((getStopwatch8()>100) && (distance > RANGE_MAX))
				{
					slow.speed_left = CRUISE_SPEED_FWD;
					slow.speed_right = CRUISE_SPEED_FWD;
				}
	
			else
				{
					slow.speed_left = SLOW_SPEED_FWD;
					slow.speed_right = SLOW_SPEED_FWD;
				}	
	break;
  }	
}