Servo (Pseudo code)
Set global variables as max/min for each servo dependent on physical limitations
Servo_SetAngle
Takes servo number and integer angle (angle from 0-180 for all but 360 degree servos)
For all servo numbers but BallSpinServo,
Check limits on angle for specific servo number
Set pulse width of servo number to 600+10*angle (Taken from HS322-HD datasheet)
For BallSpinServo,
Check limits on angle
Set pulse width of servo number to 10*angle (due to 360 degree servo having different characteristics)
ServoPulseWidthCh1
Takes int PortNum and int NewWidth
For Portnum, set corresponding channel’s TC to 0xFFFF – NewWidth*TicksPeruS (which is 3)
Servo_SetAngle
Takes servo number and integer angle (angle from 0-180 for all but 360 degree servos)
For all servo numbers but BallSpinServo,
Check limits on angle for specific servo number
Set pulse width of servo number to 600+10*angle (Taken from HS322-HD datasheet)
For BallSpinServo,
Check limits on angle
Set pulse width of servo number to 10*angle (due to 360 degree servo having different characteristics)
ServoPulseWidthCh1
Takes int PortNum and int NewWidth
For Portnum, set corresponding channel’s TC to 0xFFFF – NewWidth*TicksPeruS (which is 3)