TwoMotorRobotControl-Tester

extensions [ gpio ]

to setup 
  ca
  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