/* **********************************************
| Ackermann Drive
| Example for using MOTOR and QUAD functions for
| driving a vehicle with ACKERMANN_DRIVE steering
|
| Thomas Braunl, 2004
| ---------------------------------------------------------------------------
*/ 

#include "eyebot.h"
MotorHandle motor;
QuadHandle  quad;
ServoHandle serv;

void Minit()
/* Initialize motors and encoders */
{ motor = MOTORInit(MOTOR_DRIVE);
  quad  = QUADInit (QUAD_DRIVE);
  serv  = SERVOInit(SERVO2);
  if(!motor || !quad || !serv) 
    LCDPrintf("Init Error!\n");
}
 
void Mterm()
/* Release motors and encoders */
{ MOTORRelease(motor);
  QUADRelease (quad);
  SERVORelease(serv);
}

void Mdrive(char* txt, int drive, int steer)
/* Print txt and drive motors and encoders for 1.5s */
{ LCDPrintf("%s\n", txt);
  MOTORDrive(motor, drive);
  SERVOSet  (serv,  steer);
  OSWait(150);
  LCDPrintf("Enc. %5d\n", QUADRead(quad));
}

 
int main ()
{ LCDPutString("Ackermann Steer\n");
  Minit();
	
  Mdrive("Forward",     60, 127);
  Mdrive("Backward",   -60, 127);
  Mdrive("Left Curve",  60,   0);
  Mdrive("Right Curve", 60, 255);
  Mdrive("Stop",         0,  0);

  Mterm();
  return 0;
}