CollaborativeRobotHubNet

extensions [ gpio ]

to do-hubnet-reset
  hubnet-reset  
  set reset-hubnet false
end 

to setup 
  ca
  if (reset-hubnet) [ do-hubnet-reset ] 
  gpio:set-mode "gpio7" "WRITE"
  let i 2
  while [ i <= 12 ]
  [
  gpio:set-mode (word "gpio" i) "WRITE"
  set i i + 2
  ]
  ask patches with [ pxcor < 0 ] [ set pcolor red - 3 ]
  ask patches with [ pxcor > 0 ] [ set pcolor green - 3 ]
  reset-ticks
end 

to execute-votes [vote-left vote-right]
  let vlist get-votelist vote-left vote-right
  ;show vlist
  
  let i 2
  while [ i <= 12 ]
  [
    let towrite "LOW"
    if  item (-1 + i / 2) vlist = 1 [ set towrite "HIGH" ]
    
    gpio:digital-write (word "gpio" i) towrite
    ;show (word "gpio:digital-write " (word "gpio" i) towrite)
    set i i + 2
  ]
end 

to-report get-votelist [ vleft vright ]
  let returnlist n-values 6 [0]
  if vleft > 0 [ set returnlist replace-item 0 returnlist 1 ]
  if vright > 0 [set returnlist replace-item 3 returnlist 1 ]
  let t floor ((abs vleft) / 2)
  let r (abs vleft) mod 2
  set returnlist replace-item 1 returnlist r
  set returnlist replace-item 2 returnlist t
  
  let tr floor ((abs vright) / 2)
  let rr (abs vright) mod 2
  set returnlist replace-item 4 returnlist rr
  set returnlist replace-item 5 returnlist tr
  
  report returnlist
end 

to display-votes [vleft vright]
  ask patch -5 7 [ set plabel "left" ]
  ask patch (max-pxcor - 2) 7 [ set plabel "right" ]
  ask patch -5 -3 [ set plabel vleft ]
  ask patch (max-pxcor - 2) -3 [ set plabel vright ]
end 

to kill
  gpio:digital-write "gpio7" "LOW" 
end 

to unkill
  gpio:digital-write "gpio7" "HIGH"   
end 

to hubnet-control
  if (reset-hubnet) [ do-hubnet-reset ]
  let lvotes 0
  let rvotes 0
  while [ hubnet-message-waiting? ]
  [
    hubnet-fetch-message
    ifelse (hubnet-enter-message? or hubnet-exit-message?) 
    [ show "enter or exit" ]
    [
      if hubnet-message-tag = "left-forward" [ set lvotes lvotes + 1 ]
      if hubnet-message-tag = "left-backward" [ set lvotes lvotes - 1 ]
      if hubnet-message-tag = "right-forward" [ set rvotes rvotes + 1 ]
      if hubnet-message-tag = "right-backward" [ set rvotes rvotes - 1 ]
    ]
  ]
  set lvotes round (lvotes / sensitivity)
  set rvotes round (rvotes / sensitivity)
  if lvotes > 3 [ set lvotes 3 ]
  if rvotes > 3 [ set rvotes 3 ]
  if lvotes < -3 [ set lvotes -3 ]
  if rvotes < -3 [ set rvotes -3 ]
  
  display-votes lvotes rvotes
  execute-votes lvotes rvotes
end