'simple PC to Servo Interface

   const lastservo 5

   dim tSERVO0
   dim tSERVO1
   dim tSERVO2
   dim tSERVO3
   dim tSERVO4
   dim tSERVO5
   dim tSERVO6
   dim tSERVO7
   dim tSERVO8
   
   dim cmdidx,servonum,servodata

   dim x,tmp,tdelay
   configio 0,1,2,3,4,5,6,7,8
   clearall
   
loop:
   gosub procservos

   getpacket loop,cmdidx,servonum,servodata

   if servonum > lastservo then
      goto loop
   endif

   arrayset tSERVO1,servonum,servodata

   goto loop


'-----------------------------
procservos:
   for x = 0 to lastservo
    arrayget tSERVO1,x,tmp
    
    tdelay = 255 - tmp 'Calculate wait value
    servo x,tmp 'Servo Control command
    servo 10,tdelay 'Compensate for servo delays (dummy call)
   next
   return