/*--------------------------------------------------------------------
| Filename: omni_drive.c
|
| Authors:  Thomas Braunl, UWA, 04 April 2000
|
| Description:
| This is a demo program for OMNI robot using the omni driving library
| ------------------------------------------------------------------ */

#include "eyebot.h"
#include <math.h>
#include <stdlib.h>
#include "libomni/omni.h"

#define v_lin 7.0
#define t_lin 0.3
#define v_rot 10.0
#define t_rot 0.1

#define  speed 1.00          /*   1 m   per s */
#define rspeed (45*3.14/180) /* 45 deg. per s */
#define dist   0.20          /*   0.2 m       */

int main ()
{
  VWHandle vw;

  LCDPrintf("init\n");
  vw=OMNI_Init(VW_DRIVE,1);
  if (!vw) {LCDPrintf("init error\n"); exit(0); }

  LCDPrintf("control\n");
  /* OMNI_StartControl(vw, 0.09 , 0.3,  1.0, 0.1); */

                  /*vw, distance, angle, speed*/
  LCDPrintf("forward-back\n");
  OMNI_DriveStraight(vw, dist, 0.0, speed);
  OMNI_DriveWait(vw);
  OSWait(100);
  OMNI_DriveStraight(vw, -dist, 0.0, speed);
  OMNI_DriveWait(vw);
  OSWait(100);

  LCDPrintf("diamond 45\n");
  OMNI_DriveStraight(vw,  dist,  M_PI/4, speed);
  OMNI_DriveWait(vw);
  OSWait(100);
  OMNI_DriveStraight(vw,  dist, -M_PI/4, speed);
  OMNI_DriveWait(vw);
  OSWait(100);
  OMNI_DriveStraight(vw, -dist,  M_PI/4, speed);
  OMNI_DriveWait(vw);
  OSWait(100);
  OMNI_DriveStraight(vw, -dist, -M_PI/4, speed);
  OMNI_DriveWait(vw);
  OSWait(100);

  LCDPrintf("left-right\n");
  OMNI_DriveStraight(vw,  dist,  M_PI/2, speed);
  OMNI_DriveWait(vw);
  OSWait(100);
  OMNI_DriveStraight(vw, -dist,  M_PI/2, speed);
  OMNI_DriveWait(vw);
  OSWait(100);

              /*vw, angle, speed */
  LCDPrintf("spot left-right\n");
  OMNI_TurnSpot(vw,  M_PI/2 , rspeed);
  OMNI_DriveWait(vw);
  OSWait(100);
  OMNI_TurnSpot(vw, -M_PI/2 , rspeed);
  OMNI_DriveWait(vw);
  OSWait(100);

  LCDPutString("done\n");
  OMNI_StopControl(vw);
  OMNI_Release(vw);
  return 0;
}