/* **********************************************
| Ackermann Drive
| Example for using MOTOR and QUAD functions for
| driving a vehicle with ACKERMANN_DRIVE steering
|
| Thomas Braunl, 2004, 2017
| -----------------------------------------------*/ 
#include "eyebot.h"

void Mdrive(char* txt, int drive, int steer)
/* Print txt and drive motors and encoders for 1.5s */
{ LCDPrintf("%s\n", txt);
  MOTORDrive(1, drive);
  SERVOSet  (1, steer);
}

 
int main ()
{ QVGAcol img;

  CAMInit(QVGA);
  LCDPrintf("Ackermann Steering\n");
  CAMGet(img); LCDImage(img);
  Mdrive("Forward",     20, 128);  OSWait(1000);
  CAMGet(img); LCDImage(img);
  Mdrive("Backward",   -20, 128);  OSWait(1000);
  CAMGet(img); LCDImage(img);
  Mdrive("Left Curve",  10,   0);  OSWait(2000);
  CAMGet(img); LCDImage(img);
  Mdrive("Right Curve", 10, 255);  OSWait(2000);
  CAMGet(img); LCDImage(img);
  Mdrive("Stop",         0,  0);
  CAMGet(img); LCDImage(img);
  return 0;
}

