10.3 Beispielprogramm 2

Das folgende Programm läßt den Khepera im Sekundentakt hin- und herdrehen. Dabei wird die Geschwindigkeit der Motoren vor dem eigentlichen Setzen mit Send_IR() an den anderen Khepera übermittelt. Folglich laufen beide Roboter synchron. Bemerkenswert daran ist, daß zum Synchronfahren wirklich nur die Zeile

mot_new_speed_2m(speed[1],speed[0]);

durch die Zeilen

data[0]='D';
data[1]=(char)(speed[1]+127);
data[2]=(char)(speed[0]+127);
data[3]='\0';

send_IR (slave, data);
mot_new_speed_2m(speed[1],speed[0]);

ersetzt werden muß. Es ist also gar kein Problem, ganz andere Fahr-Algorithmen synchron ausführen zu lassen, die Implementierung ist völlig unproblematisch.

/*****************************************************************************/
/* File:        usercr.c (Cross Compiler)                                    */
/* Author:      Christopher Odenbach & Juergen Klahold                       */
/* Date:        Tue Apr 26 1999                                              */
/* Description: Master fuer Synchron-Tanz                                    */
/*****************************************************************************/

#include "bios.h"
#include "usercr.h"
#include 
#include 
#include 

#include "can-ir.h"
#include "k-bus.h"

char slave;
int32 ID_process[15];

void StopRun()
{
  char tmp_code;
  boolean RUN=TRUE;
  char StopMsg[4];

StopMsg[0]='D';
StopMsg[1]=(char)127;
StopMsg[2]=(char)127;
StopMsg[3]='\0';

while(TRUE)
{
  while(((int)(tmp_code=ser_receive_byte()))<0) /* reception buffer empty */
    tim_switch_fast();

  if((tmp_code == 13) || (tmp_code == 10))
  {
    if(RUN)
    {
      tim_lock();
      send_IR (slave, StopMsg);
      mot_stop();
      RUN = FALSE;
    }
    else
    {
      tim_unlock();
      RUN = TRUE;
    }
  }
}
}

void StopAndGo()
{
char data[8];
int8 speed[2];

speed[0]=3;
speed[1]=-3;
data[0]='D';
data[3]='\0';

  while(1) {
    var_change_led(0);
    data[1]=(char)(speed[1]+127);
    data[2]=(char)(speed[0]+127);
    send_IR ('R', data);
    mot_new_speed_2m(speed[1],speed[0]);
    tim_suspend_task(1000);

    var_change_led(0);
    data[1]=(char)(speed[0]+127);
    data[2]=(char)(speed[1]+127);
    send_IR ('R', data);
    mot_new_speed_2m(speed[0],speed[1]);

    tim_suspend_task(1000);
  }
}


void main()
{
  bios_reset();
  tim_reset();
  mot_reset();
  sens_reset();
  msg_reset();
  var_reset();
  ser_reset();


  mot_config_speed_1m(0,3800,800,100);
  mot_config_speed_1m(1,3800,800,100);
  mot_config_position_1m(0,3000,20,4000);
  mot_config_position_1m(1,3000,20,4000);
  mot_config_profil_1m(0,20,((1<<8)/4));
  mot_config_profil_1m(1,20,((1<<8)/4));
  mot_put_sensors_2m(0,0);
  ser_config(4);

  tim_lock();
    tim_suspend_task(500);
    var_on_led(1);
    var_on_led(0);
    tim_suspend_task(500);
    var_off_led(1);
    var_off_led(0);
  tim_unlock();

  printf("Khepera OK\n");

  init_modules();
  printf("Modules initialized\n");

  ID_process[10]=tim_new_task(process_IR_data);
  var_set_irq_vector(irq_routine);
  var_enable_irq();

  send_IR ('A', "IR OK");

  tim_suspend_task(2000);
  if (robnr < 1) {
    printf("Kein anderer Roboter verfuegbar!\nSende an Master...\n");
    slave='R';
  } else {
    slave=robot_ID[0];
    printf("Roboter %c wird gesteuert.\n",slave);
  }

  ID_process[14] = tim_new_task(StopRun);
  ID_process[9] = tim_new_task(StopAndGo);
  while (1);
}