main

 
global [cmd count newbase oldbase stepbase newshoulder oldshoulder stepshoulder newelbow oldelbow stepelbow newwrist oldwrist stepwrist newgrip]

to main
  setoldbase -1 setnewbase 0
  setoldshoulder -1 setnewshoulder 0
  setoldelbow -1 setnewelbow 0
  setoldwrist -1 setnewwrist 0
   setnewgrip 0
  main-loop
end

to main-loop
  waituntil [newir?] setcmd ir
  if cmd and $E0 = $40 [
    fastsend cmd
    do-move-cmd
    fastsend $FF
  ]
  if cmd and $E0 = $60 [
    fastsend cmd
    do-grip-cmd
    fastsend $FF
  ]
  if cmd and $E0 = $20 [
    fastsend cmd
    do-off-cmd
    fastsend $FF
  ]
  main-loop
end

to do-move-cmd
  
  waituntil [newir?]
  setnewbase ir
  if (cmd and 1) = 1 [setnewbase newbase or $100]
  fastsend low-byte newbase
  
  waituntil [newir?]
  setnewshoulder ir
  if (cmd and 2) = 2 [setnewshoulder newshoulder or $100]
  fastsend low-byte newshoulder
  
  waituntil [newir?]
  setnewelbow ir
  if (cmd and 4) = 4 [setnewelbow newelbow or $100]
  fastsend low-byte newelbow
  
  waituntil [newir?]
  setnewwrist ir
  if (cmd and 8) = 8 [setnewwrist newwrist or $100]
  fastsend low-byte newwrist

  ; Scale the value from the PC to avoid integer truncation.
    setnewbase newbase * 100

    ; If this is the first move, go ahead and move to the new position.
    if oldbase = -1 [
      servo-update 0 (newbase / 100) -1
      setoldbase newbase
    ]
  ; Scale the value from the PC to avoid integer truncation.
    setnewshoulder newshoulder * 100

    ; If this is the first move, go ahead and move to the new position.
    if oldshoulder = -1 [
      servo-update 1 (newshoulder / 100) -1
      setoldshoulder newshoulder
    ]
  ; Scale the value from the PC to avoid integer truncation.
    setnewelbow newelbow * 100

    ; If this is the first move, go ahead and move to the new position.
    if oldelbow = -1 [
      servo-update 2 (newelbow / 100) -1
      setoldelbow newelbow
    ]
  ; Scale the value from the PC to avoid integer truncation.
    setnewwrist newwrist * 100

    ; If this is the first move, go ahead and move to the new position.
    if oldwrist = -1 [
      servo-update 3 (newwrist / 100) -1
      setoldwrist newwrist
    ]
  

  ; Find the step increments and the number of steps
  setcount 0
  ; Calculate the total offset and store it in STEP.
    setstepbase newbase - oldbase

    ; Store the largest absolute offset in count.
    ifelse stepbase < 0 [
      if (0 - stepbase) > count [setcount (0 - stepbase)]
    ] [
      if stepbase > count [setcount stepbase]
    ]
  ; Calculate the total offset and store it in STEP.
    setstepshoulder newshoulder - oldshoulder

    ; Store the largest absolute offset in count.
    ifelse stepshoulder < 0 [
      if (0 - stepshoulder) > count [setcount (0 - stepshoulder)]
    ] [
      if stepshoulder > count [setcount stepshoulder]
    ]
  ; Calculate the total offset and store it in STEP.
    setstepelbow newelbow - oldelbow

    ; Store the largest absolute offset in count.
    ifelse stepelbow < 0 [
      if (0 - stepelbow) > count [setcount (0 - stepelbow)]
    ] [
      if stepelbow > count [setcount stepelbow]
    ]
  ; Calculate the total offset and store it in STEP.
    setstepwrist newwrist - oldwrist

    ; Store the largest absolute offset in count.
    ifelse stepwrist < 0 [
      if (0 - stepwrist) > count [setcount (0 - stepwrist)]
    ] [
      if stepwrist > count [setcount stepwrist]
    ]
  

  ; Scale the count down
  setcount count / (100 * 2)

  ; Update STEP to contain the step size, rather than total offset.
    setstepbase stepbase / count
  ; Update STEP to contain the step size, rather than total offset.
    setstepshoulder stepshoulder / count
  ; Update STEP to contain the step size, rather than total offset.
    setstepelbow stepelbow / count
  ; Update STEP to contain the step size, rather than total offset.
    setstepwrist stepwrist / count
  

  ; Move the arm using the steps...
  repeat count [
    servo-update 0 ((oldbase + stepbase) / 100) (oldbase / 100)
      setoldbase oldbase + stepbase
    servo-update 1 ((oldshoulder + stepshoulder) / 100) (oldshoulder / 100)
      setoldshoulder oldshoulder + stepshoulder
    servo-update 2 ((oldelbow + stepelbow) / 100) (oldelbow / 100)
      setoldelbow oldelbow + stepelbow
    servo-update 3 ((oldwrist + stepwrist) / 100) (oldwrist / 100)
      setoldwrist oldwrist + stepwrist
    
  ]

  ; Finish by moving to exactly the requested position.
    servo-update 0 (newbase / 100) -1
    setoldbase newbase
  ; Finish by moving to exactly the requested position.
    servo-update 1 (newshoulder / 100) -1
    setoldshoulder newshoulder
  ; Finish by moving to exactly the requested position.
    servo-update 2 (newelbow / 100) -1
    setoldelbow newelbow
  ; Finish by moving to exactly the requested position.
    servo-update 3 (newwrist / 100) -1
    setoldwrist newwrist
  
end

to do-grip-cmd
  
  waituntil [newir?]
  setnewgrip ir
  if (cmd and 1) = 1 [setnewgrip newgrip or $100]
  fastsend low-byte newgrip
  servo-update 4 newgrip -1
end

to do-off-cmd
  setoldbase -1 setnewbase 0 bsend $115 bsend 0 * 2 bsend low-byte $00
  setoldshoulder -1 setnewshoulder 0 bsend $115 bsend 1 * 2 bsend low-byte $00
  setoldelbow -1 setnewelbow 0 bsend $115 bsend 2 * 2 bsend low-byte $00
  setoldwrist -1 setnewwrist 0 bsend $115 bsend 3 * 2 bsend low-byte $00
   setnewgrip 0 bsend $115 bsend 4 * 2 bsend low-byte $00
end

to servo-update :n :new :old
  if not (:new = :old) [
    ifelse :new > $ff [
      if (not (:old > $ff)) or (:old = -1) [bsend $115 bsend :n * 2 bsend low-byte $bf]
      bsend $115 bsend :n * 2 + 1 bsend low-byte :new - $3f
    ] [
      if (:old > $ff) or (:old = -1) [bsend $115 bsend :n * 2 bsend low-byte $80]
      bsend $115 bsend :n * 2 + 1 bsend low-byte :new
    ]
  ]
end