module main
author unknown
version 1 0 
description ''

  spec ' ' 'servo270' 'set servo270 _ to _ degress(-135 to 135)' 'auto auto' '10' '10'

to servo270 pin val {
  '[io:setServo]' pin ('[misc:rescale]' val -135 135 500 2500)
}


script 58 74 {
whenStarted
sendBroadcast 'servo_90'
waitMillis 5000
sendBroadcast 'servo_180'
waitMillis 5000
sendBroadcast 'servo_270'
}

script 474 40 {
whenBroadcastReceived 'servo_180'
setServoAngle 14 -90
waitMillis 1000
setServoAngle 14 -45
waitMillis 1000
setServoAngle 14 0
waitMillis 1000
setServoAngle 14 45
waitMillis 1000
setServoAngle 14 90
waitMillis 1000
}


script 56 316 {
whenBroadcastReceived 'servo_270'
servo270 13 -135
waitMillis 1000
servo270 13 0
waitMillis 1000
servo270 13 135
waitMillis 1000
}

script 485 362 {
whenBroadcastReceived 'servo_90'
setServoAngle 26 0
waitMillis 1000
setServoAngle 26 45
waitMillis 1000
setServoAngle 26 90
waitMillis 1000
setServoAngle 26 45
waitMillis 1000
setServoAngle 26 0
waitMillis 1000
}

script 40 537 {
to servo270 {}
}


module Servo Output
author MicroBlocks
version 1 4 
tags servo motor angle rotation position 
description 'Control both positional (angle) and rotational servo motors.
'
variables _servoPin _servoPulseWidth 

  spec ' ' 'setServoAngle' 'set servo _ to _ degrees (-90 to 90)' 'num num' 1 90
  spec ' ' 'setServoSpeed' 'set servo _ to speed _ (-100 to 100)' 'num num' 1 100
  spec ' ' 'stopServo' 'stop servo _' 'num' 1
  spec 'r' '_servoIndex' '_servoIndex _' 'num' 1
  spec ' ' '_servoPulse' '_servoPulse pin _ usecs _' 'num num' 1 1500
  spec ' ' '_servoUpdateLoop' '_servoUpdateLoop'

to '_servoIndex' which {
  if (_servoPin == 0) {
    _servoPin = ('[data:makeList]')
    _servoPulseWidth = ('[data:makeList]')
    sendBroadcast '_servoUpdateLoop'
  }
  local 'i' ('[data:find]' which _servoPin)
  if (i < 0) {
    comment 'Add new pin'
    '[data:addLast]' which _servoPin
    '[data:addLast]' 1500 _servoPulseWidth
    i = (size _servoPin)
  }
  return i
}

to '_servoPulse' pin usecs {
  if (usecs == 0) {
    comment 'Servo stopped; do nothing'
    return 0
  }
  usecs = (maximum 500 (minimum usecs 2900))
  comment 'Split wait into a long wait followed by a wait of <= 30 usecs for greater accuracy'
  local 'endTime' ((microsOp) + usecs)
  digitalWriteOp pin true
  waitMicros (usecs - 30)
  waitMicros (endTime - (microsOp))
  digitalWriteOp pin false
}

to '_servoUpdateLoop' {
  forever {
    if (_servoPin != 0) {
      comment 'If the _servoPin list is not 0, update the servos'
      for i (size _servoPin) {
        local 'pin' (at i _servoPin)
        local 'usecs' (at i _servoPulseWidth)
        if (and (pin >= 0) (usecs != 0)) {
          '_servoPulse' pin usecs
        }
      }
      waitMillis 15
    }
  }
}

to setServoAngle which degrees optionalReverse {
  local 'reversed' false
  if ((pushArgCount) > 2) {
    reversed = optionalReverse
  }
  if reversed {
    degrees = (0 - degrees)
  }
  local 'pulseWidth' (1500 - (10 * degrees))
  if ('[io:hasServo]') {
    '[io:setServo]' which pulseWidth
  } else {
    atPut ('_servoIndex' which) _servoPulseWidth pulseWidth
  }
}

to setServoSpeed which speed optionalReverse {
  local 'reversed' false
  if ((pushArgCount) > 2) {
    reversed = optionalReverse
  }
  if reversed {
    speed = (0 - speed)
  }
  local 'pulseWidth' (1500 - (10 * speed))
  if ((absoluteValue speed) < 2) {
    pulseWidth = 0
  }
  if ('[io:hasServo]') {
    '[io:setServo]' which pulseWidth
  } else {
    atPut ('_servoIndex' which) _servoPulseWidth pulseWidth
  }
}

to stopServo which {
  if ('[io:hasServo]') {
    '[io:setServo]' which 0
  } else {
    atPut ('_servoIndex' which) _servoPulseWidth 0
  }
}


module 'Servo Primitives' Output
author MicroBlocks
version 1 0 
tags servo motor angle rotation position 
description 'Low level servo motor primitives. You are probably looking for the Servo library in the folder above.'

  spec 'r' '[io:hasServo]' 'has servo support'
  spec ' ' '[io:setServo]' 'set servo pin _ to _ usecs' 'num num' 0 1500