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

Servo Control Routies


Jan 13, 2004 dan created.

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

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


error Servo::SyncMode(bool synchronous) {

}

error Servo::Act() {
 #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) ;
 
}



error Servo::On(bool) { 

}
 
error Servo::Position(float) { 

 if (!synchronous) 
   act();
 
 return OK; 
}

float Servo::Position() { 

}    
 
int8  Servo::Current() {  

}

error Servo::Mode(bool) {  

} // Servo or Raw

error Servo::Gain(int3) {  

}

error Servo::PWM(int8) {  

}














//-------------------------------------------------------

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