/**********************************************************

Servo Control Routies


Jan 13, 2004 dan created.

**********************************************************/

#include "retvals.h"
#include "legdefs.h"

int Joints[2][3][3];


int setLeg( int side, int legNumber, int joint, float angle);


int setLeg( int Side, int LegNumber, int Joint, float Angle) {

 #declare MaxAngle   300  
 #declare MaxControl 255
 
 if (Angle > (MaxAngle/2)) {
   Angle = (MaxAngle/2);
 } else if (Angle < (MaxAngle / -2)) {
   Angle = (MaxAnlge / -2);
 }
 
 Joints[Side][LegNumber][Joint] = int(Angle * (MaxControl/MaxAngle)) + (MaxControl/2); 
 
 return OK;
};

Get