/* **********************************************
| Omnidirectional Drive - Mecanum Wheels
| Example for using MOTOR and QUAD functions for
| driving a vehicle with OMNI_DRIVE steering
|
| Thomas Braunl, 2004, 2005
| ---------------------------------------------------------------------------
*/ 

#include "eyebot.h"
MotorHandle motor_fl, motor_fr, motor_bl, motor_br;
QuadHandle  quad_fl, quad_fr, quad_bl, quad_br;
  
void Minit()
/* Initialize motors and encoders */
{ motor_fl = MOTORInit(MOTOR_FL);
  motor_fr = MOTORInit(MOTOR_FR);
  motor_bl = MOTORInit(MOTOR_BL);
  motor_br = MOTORInit(MOTOR_BR);
  quad_fl  = QUADInit(QUAD_FL);
  quad_fr  = QUADInit(QUAD_FR);
  quad_bl  = QUADInit(QUAD_BL);
  quad_br  = QUADInit(QUAD_BR);
  
  if(!motor_fl || !motor_fr || !motor_bl || !motor_br ||
     !quad_fl || !quad_fr || !quad_bl || !quad_br) 
    LCDPrintf("Init Error!\n");
}
 
void Mterm()
/* Release motors and encoders */
{ MOTORRelease(motor_fl|motor_fr|motor_bl|motor_br);
  QUADRelease (quad_fl|quad_fr|quad_bl|quad_br);
}

void Mdrive(char* txt, int Fleft, int Fright, int Bleft, int Bright)
/* Print txt and drive motors and encoders for 1.5s */
{ LCDPrintf("%s\n", txt);
  MOTORDrive(motor_fl,Fleft);
  MOTORDrive(motor_fr,Fright);
  MOTORDrive(motor_bl,Bleft);
  MOTORDrive(motor_br,Bright);
  OSWait(150);
  LCDPrintf("Enc.%5d %5d\n    %5d %5d\n",
   QUADRead(quad_fl), QUADRead(quad_fr), QUADRead(quad_bl), QUADRead(quad_br));
}


int main ()
{ LCDPutString("Mecanum Wheels\n");
  Minit();
	
  Mdrive("Forward",     60, 60, 60, 60);
  Mdrive("Backward",   -60,-60,-60,-60);
  Mdrive("Left",       -60, 60, 60,-60);
  Mdrive("Right",       60,-60,-60, 60);
  Mdrive("Left45",       0, 60, 60,  0);
  Mdrive("Right45",     60,  0,  0, 60);
  Mdrive("Turn Spot L",-60, 60,-60, 60);
  Mdrive("Turn Spot R", 60,-60, 60,-60);
  Mdrive("Stop",         0,  0,  0,  0);
  
  Mterm();
  return 0;
}