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

#include "eyebot.h"
MotorHandle motor_l, motor_r;
QuadHandle  quad_l, quad_r;
  
void Minit()
/* Initialize motors and encoders */
{ motor_l = MOTORInit(MOTOR_LEFT);
  motor_r = MOTORInit(MOTOR_RIGHT);
  quad_l  = QUADInit(QUAD_LEFT);
  quad_r  = QUADInit(QUAD_RIGHT);
  
  if(!motor_l || !motor_r || !quad_l || !quad_r) 
    LCDPrintf("Init Error!\n");
}
 
void Mterm()
/* Release motors and encoders */
{ MOTORRelease(motor_l|motor_r);
  QUADRelease (quad_l|quad_r);
}

void Mdrive(char* txt, int left, int right)
/* Print txt and drive motors and encoders for 1.5s */
{ LCDPrintf("%s\n", txt);
  MOTORDrive(motor_l,left);
  MOTORDrive(motor_r,right);
  OSWait(150);
  LCDPrintf("Enc. %5d %5d\n", QUADRead(quad_l), QUADRead(quad_r));
}

 
int main ()
{ LCDPutString("Diff.Steering\n");
  Minit();
	
  Mdrive("Forward",     60, 60);
  Mdrive("Backward",   -60,-60);
  Mdrive("Left Curve",  20, 60);
  Mdrive("Right Curve", 60, 20);
  Mdrive("Turn Spot L",-20, 20);
  Mdrive("Turn Spot R", 20,-20);
  Mdrive("Stop",         0,  0);

  Mterm();
  return 0;
}