//----------  Motor, Quad functions for Rock steady, A.Pickel------------
//----------------last update  13.02.03-------------------
#include "RSinit.h"

void QuIni()
{	// QUAD init
	if(!(qH[0] = QUADInit(QUAD_LE)))
		LCDPrintf("\nQuadinit ER");
	if(!(qH[1] = QUADInit(QUAD_RI)))
		LCDPrintf("\nQuadinit ER");
	if(!(qH[2] = QUADInit(QUAD_LR)))
		LCDPrintf("\nQuadinit ER");
	
}


void MoIni()
{
	// motor init
	if(!(mH[0] = MOTORInit(LEG_LEFT)))
		LCDPrintf("\nMotorini1 ER");
	if(!(mH[1] = MOTORInit(LEG_RIGHT)))
		LCDPrintf("\nMotorini2 ER");
	if(!(mH[2] = MOTORInit(MOTOR_LR)))
		LCDPrintf("\nMotorini3 ER");
	
}

void MOQuRelease()
{	
	for (int c=0; c<3; c++)
	{
		if (QUADRelease(qH[c]))
			LCDPrintf("\nQUADrel ERR!");
		if (MOTORRelease(mH[c]))
			LCDPrintf("\nMOTORrel ERR!");
	}
}