Kreis fahren

[ehenkes]

Ich habe versucht, die Funktion Go(...) auf eine Funktion Circle(...) umzuwandeln, funktioniert bisher allerdings nicht. ASURO dreht nur nach rechts am Punkt. Wo liegt der Fehler?

#include "myasuro.h" // #define MY_GO_CORRECTIVE_ACTION  15 // siehe unten
#include "asuro.h" // dort sind LEFT und RIGHT festgelegt

void Circle (int distance, int speed, int diameter, int direction)
{
  const float HALFWidth_ASURO = 51.5; // Halber Radabstand des ASURO
  float outer_diameter = (float) diameter + HALFWidth_ASURO;
  float factor = outer_diameter / (float) diameter;
    
  uint32_t enc_count;
  int tot_count = 0;
  float diff = 0;
  int l_speed = speed, r_speed = speed;

  // calculation tics/mm
  enc_count=abs(distance)*10000L;
  enc_count/=MY_GO_ENC_COUNT_VALUE;
  EncoderSet(0,0); // reset encoder

  MotorSpeed(l_speed,r_speed);
  if (distance<0) MotorDir(RWD,RWD);
  else MotorDir(FWD,FWD);

  while (tot_count<enc_count)
  {
    if(direction==RIGHT) // Rechtskreis
	{
	  tot_count += encoder[LEFT]/factor; 
	  diff = (float) encoder[LEFT]/factor - (float) encoder[RIGHT]*factor;
	}
    if(direction==LEFT) // Linkskreis
	{
	  tot_count += encoder[RIGHT]/factor; 
	  diff = (float) encoder[RIGHT]/factor - (float) encoder[LEFT]*factor;
	}	
	
 	if (diff > 0)
    { //Left faster than right
      if ((l_speed > speed) || (r_speed > 244)) l_speed -= MY_GO_CORRECTIVE_ACTION;
      else r_speed += MY_GO_CORRECTIVE_ACTION;
    }
    if (diff < 0)
    { //Right faster than left
      if ((r_speed > speed) || (l_speed > 244)) r_speed -= MY_GO_CORRECTIVE_ACTION;
      else l_speed += MY_GO_CORRECTIVE_ACTION;
    }
    EncoderSet(0,0); // reset encoder
    MotorSpeed(l_speed,r_speed);
    Msleep(1);
  }
  MotorDir(BREAK,BREAK);
  Msleep(200);
}

int main()
{
  Init();
  EncoderInit(); 
  StatusLED (YELLOW);
 
  Circle(5000,200,700,LEFT);
   
  StatusLED (GREEN); 
 
  while (1);
  return 0;
}