Motoransteuerung

Zum Einstellen der Stillstandspunkte der Servos haben wir eine Prozedur entwickelt, bei der über den vorderen und den hinteren Taster die Stillstandspunkte der Servos einzeln eingestellt werden können. Ist ein Werte gut eingestellt muss der Rechte Taster gedrückt werden um fortzufahren. Die Variablen _rcal und _lcal sind global und speichern die Stillstandswerte.

#define RIGHT_INIT_CAL 22
#define LEFT_INIT_CAL -22

/*** Einstellung der Stillstandswerte fuer die beiden Motoren ***/
void kalibrieren()
{
  /* Rechter Servo */
  _rcal=RIGHT_INIT_CAL;
  /* Rechten Taster drücken, wenn Wert eingestellt ist */
  while(!RIGHT_BUMPER) {
    if(FRONT_BUMPER) { while(FRONT_BUMPER); _rcal++; }
    if(REAR_BUMPER) { while(REAR_BUMPER); _rcal--; }

    if(_rcal<-100) _rcal=-100;
    if(_rcal>100) _rcal=100;

    motorp(RIGHT_MOTOR, _rcal);
  }
  motorp(RIGHT_MOTOR, 0); /* Servo Abschalten */
  printf("\n\n_rcal = %d\n", _rcal);

  while(RIGHT_BUMPER);

  /* Linker Servo */
  _lcal=LEFT_INIT_CAL;
  /* Rechten Taster drücken, wenn Wert eingestellt ist */
  while(!RIGHT_BUMPER) {
    if(FRONT_BUMPER) { while(FRONT_BUMPER); _lcal++; }
    if(REAR_BUMPER) { while(REAR_BUMPER); _lcal--; }

    if(_lcal<-100) _lcal=-100;
    if(_lcal>100) _lcal=100;

    motorp(LEFT_MOTOR, _lcal);
  }
  motorp(LEFT_MOTOR, 0); /* Servo Abschalten */
  printf("_lcal = %d\n", _lcal);

  while(RIGHT_BUMPER);
}

Über eine neue Motoransteuerprozedur führen nun gleiche Werte zu gleichen Geschwindigkeiten. Dabei muss der Wert 0 (Servo abgeschaltet) für die motorp()-Funktion umgangen werden. (Bsp.: Stillstandswerte sind 20 und -20. Geschwindigkeit 20 soll einsestellt werden $\rightarrow$ Werte für motorp() sind dann 40 und 0...das ist suboptimal.)

/*** Motoreinstellung mit beruecksichtigung der Kalibrierung ***/
void motorv(int m, int p)
{
  if(m==LEFT_MOTOR) p+=_lcal;
  if(m==RIGHT_MOTOR) p+=_rcal;

  if(p==0) p=1;
  if(p<-100) p=-100;
  if(p>100) p=100;

  motorp(m, p);
}