module main
author unknown
version 1 0 
description ''

script 96 109 {
whenButtonPressed 'A'
drawBMPfile 'paper.bmp' 0 0
'play tone' 'nt;c' 1 100
'play tone' 'nt;g' 1 100
}

script 438 204 {
whenButtonPressed 'A+B'
drawBMPfile 'scissors.bmp' 0 0
'play tone' 'nt;c' 1 100
'play tone' 'nt;g' 1 100
}

script 94 276 {
whenButtonPressed 'B'
drawBMPfile 'rock.bmp' 0 0
'play tone' 'nt;c' 1 100
'play tone' 'nt;g' 1 100
}


module BMP Output
author 'José García Yeste'
version 2 10 
depends _TFT _Files 
description 'Display BMP files on a TFT screen.

BMP files can be transferred to and from boards with a file systems using the put/get commands in the IDE file menu (available in "show advanced blocks" mode).

Dragging a BMP file over microBlocks also loads the file into the file system.

This library works with uncompressed BMP files with 1, 4, 8, 16, 24 and 32 bits per pixel. v2.0 supports 4 and 8 bit RLE compressed files.

Tested with GIMP, online-convert.com and convert.io generated files.

'
variables _bmpPalette _bmpIsRGB565 _bmpPixelsOffset _bmpWidth _bmpHeight _bmpBPP _bmpCompression _bmpLine _bmpPixelLine 

  spec ' ' 'drawBMPfile' 'display BMP file _ at x _ y _' 'str num num' 'image.bmp' 0 0
  space
  spec 'r' '_bmp_readHeader' '_bmp_readHeader file _' 'str' ''
  spec ' ' '_bmp_drawBMP' '_bmp_drawBMP file _ x _ y _' 'str num num' '' 0 0
  spec ' ' '_bmp_drawBMPPixels' '_bmp_drawBMPPixels file _ x _ y _ with function _' 'str num num str' '' 0 0 '[tft:setPixel]'
  space
  spec ' ' '_bmp_readPalette' '_bmp_readPalette file _' 'auto' ''
  spec 'r' '_bmp_readPixelLine' '_bmp_readPixelLine file _' 'str' ''
  spec ' ' '_bmp_readLine' '_bmp_readLine file _' 'auto' ''
  spec 'r' '_bmp_readRLEline' '_bmp_readRLELine file _' 'auto' ''
  spec ' ' '_bmp_mapIndexedPixels' '_bmp_mapIndexedPixels'
  space
  spec 'r' '_bmp_readRGBPixelLine' '_bmp_readRGBPixelLine file _' 'auto' ''
  spec 'r' '_bmp_16BitPixel' '_bmp_16BitPixel from _ starting at _' 'str num' '' 1
  spec 'r' '_bmp_24BitPixel' '_bmp_24BitPixel from _ starting at _' 'str num' '' 1
  space
  spec ' ' '_bmp_alignLineData' '_bmp_alignLineData file _' 'auto' ''
  spec 'r' '_bmp_readInt16' '_bmp read int16 file _' 'str' ''
  spec 'r' '_bmp_readInt32' '_bmp read int32 file _' 'str' ''

to '_bmp_16BitPixel' data i {
  local 'pixel' (((at (i + 1) data) << 8) | (at i data))
  if _bmpIsRGB565 {
    local 'r' ((pixel >> 8) & 248)
    local 'g' ((pixel >> 3) & 248)
  } else {
    local 'r' ((pixel >> 7) & 248)
    local 'g' ((pixel >> 2) & 248)
  }
  local 'b' ((pixel & 31) << 3)
  return ((r << 16) | ((g << 8) | b))
}

to '_bmp_24BitPixel' data i {
  return ((((at (i + 2) data) << 16) | ((at (i + 1) data) << 8)) | (at i data))
}

to '_bmp_alignLineData' filename {
  if (and (_bmpCompression != 1) (_bmpCompression != 2)) {
    local 'padding' (((32 - ((_bmpWidth * _bmpBPP) % 32)) / 8) % 4)
    if (padding > 0) {
      file_SkipBytes padding filename
    }
  }
}

to '_bmp_drawBMP' filename originX originY {
  '[file:setReadPosition]' _bmpPixelsOffset filename
  if (_bmpHeight >= 0) {
    local 'y' ((originY + _bmpHeight) - 1)
    local 'yIncrement' -1
  } else {
    local 'y' originY
    local 'yIncrement' 1
  }
  local 'bytesPerPixel' (_bmpBPP / 8)
  if (not _bmpIsRGB565) {
    bytesPerPixel = (0 - bytesPerPixel)
  }
  repeat (absoluteValue _bmpHeight) {
    local 'line' ('_bmp_readPixelLine' filename)
    '[tft:pixelRow]' line originX y bytesPerPixel _bmpPalette
    y += yIncrement
  }
}

to '_bmp_drawBMPPixels' filename originX originY setPixelFunction {
  comment 'Much slower than _bmp_drawBMP but allows client to supply
a setPixelFunction, so it can be used to draw a BMP file
file onto other devices such as a NeoPixel panel.'
  '[file:setReadPosition]' _bmpPixelsOffset filename
  if (_bmpHeight >= 0) {
    local 'y' ((originY + _bmpHeight) - 1)
    local 'yIncrement' -1
  } else {
    local 'y' originY
    local 'yIncrement' 1
  }
  local 'parameters' (newList 3)
  repeat (absoluteValue _bmpHeight) {
    local 'line' ('_bmp_readRGBPixelLine' filename)
    for x (size line) {
      atPut 1 parameters (originX + (x - 1))
      atPut 2 parameters y
      atPut 3 parameters (at x line)
      callCustomCommand setPixelFunction parameters
    }
    y += yIncrement
  }
}

to '_bmp_mapIndexedPixels' {
  local 'mask' ((1 << _bmpBPP) - 1)
  local 'pixelsPerByte' (8 / _bmpBPP)
  local 'i' 1
  for byte _bmpLine {
    local 'shift' (8 - _bmpBPP)
    repeat pixelsPerByte {
      if (i <= _bmpWidth) {
        local 'pixelIndex' (((byte >> shift) & mask) + 1)
        atPut i _bmpPixelLine (at pixelIndex _bmpPalette)
        shift = (shift - _bmpBPP)
        i += 1
      }
    }
  }
  return _bmpPixelLine
}

to '_bmp_readHeader' filename {
  local 'fileSize' ('[file:fileSize]' filename)
  if (fileSize < 0) {
    sayIt 'File not found: ' filename
    return 'false'
  }
  '[file:open]' filename
  if (or (fileSize < 54) ('BM' != ('[data:join]' '' ('[file:readBytes]' 2 filename)))) {
    sayIt 'Not a BMP file: ' filename
    return false
  }
  file_SkipBytes 8 filename
  _bmpPixelsOffset = ('_bmp_readInt32' filename)
  local 'headerSize' ('_bmp_readInt32' filename)
  _bmpWidth = ('_bmp_readInt32' filename)
  _bmpHeight = ('_bmp_readInt32' filename)
  _bmpPixelLine = (newList _bmpWidth)
  file_SkipBytes 2 filename
  _bmpBPP = ('_bmp_readInt16' filename)
  _bmpCompression = ('_bmp_readInt32' filename)
  if (_bmpCompression > 3) {
    sayIt ('[data:join]' 'Unsupported compression method: ' _bmpCompression)
    return false
  }
  _bmpPalette = 0
  if (_bmpBPP <= 8) {
    file_SkipBytes 12 filename
    local 'palSize' ('_bmp_readInt16' filename)
    if (palSize == 0) {
      palSize = (1 << _bmpBPP)
    }
    _bmpPalette = (newList palSize)
    file_SkipBytes (headerSize - 34) filename
    '_bmp_readPalette' filename
  } else {
    _bmpIsRGB565 = (booleanConstant false)
    if (_bmpCompression == 3) {
      file_SkipBytes 20 filename
      comment 'BITFIELD  Red'
      _bmpIsRGB565 = (('_bmp_readInt32' filename) == (hexToInt 'F800'))
    }
  }
  return true
}

to '_bmp_readInt16' filename {
  local 'data' ('[file:readBytes]' 2 filename)
  return (((at 2 data) << 8) | (at 1 data))
}

to '_bmp_readInt32' filename {
  local 'data' ('[file:readBytes]' 4 filename)
  return ((((at 4 data) << 24) | ((at 3 data) << 16)) | (((at 2 data) << 8) | (at 1 data)))
}

to '_bmp_readLine' filename {
  if (or (_bmpCompression == 1) (_bmpCompression == 2)) {
    _bmpLine = ('_bmp_readRLEline' filename)
  } else {
    local 'byteCount' (((_bmpWidth * _bmpBPP) + 7) / 8)
    if ((size _bmpLine) == byteCount) {
      comment 'Reuse existing line buffer if possible'
      local 'bytesRead' ('[file:readInto]' _bmpLine filename)
    } else {
      _bmpLine = ('[file:readBytes]' byteCount filename)
      repeatUntil ((size _bmpLine) >= byteCount) {
        _bmpLine = ('[data:join]' _bmpLine ('[file:readBytes]' (byteCount - (size _bmpLine)) filename))
      }
    }
  }
}

to '_bmp_readPalette' filename {
  local 'pal' ('[file:readBytes]' (4 * (size _bmpPalette)) filename)
  repeatUntil ((size pal) >= (4 * (size _bmpPalette))) {
    pal = ('[data:join]' pal ('[file:readBytes]' ((4 * (size _bmpPalette)) - (size pal)) filename))
  }
  for i (size _bmpPalette) {
    atPut i _bmpPalette (((at ((i * 4) - 1) pal) << 16) | (((at ((i * 4) - 2) pal) << 8) | (at ((i * 4) - 3) pal)))
  }
}

to '_bmp_readPixelLine' filename {
  '_bmp_readLine' filename
  '_bmp_alignLineData' filename
  return (ifExpression (_bmpBPP < 8) ('_bmp_mapIndexedPixels') _bmpLine)
}

to '_bmp_readRGBPixelLine' filename {
  local 'result' ('[data:makeList]')
  '_bmp_readLine' filename
  if (_bmpBPP <= 8) {
    for i _bmpLine {
      '[data:addLast]' (at (i + 1) _bmpPalette) result
    }
  } else {
    local 'i' 1
    repeatUntil ((size result) == _bmpWidth) {
      if (_bmpBPP == 16) {
        '[data:addLast]' ('_bmp_16BitPixel' _bmpLine i) result
      } else {
        '[data:addLast]' ('_bmp_24BitPixel' _bmpLine i) result
      }
      i += (maximum (_bmpBPP / 8) 1)
    }
  }
  '_bmp_alignLineData' filename
  return result
}

to '_bmp_readRLEline' filename {
  local 'result' ('[data:newByteArray]' 0)
  forever {
    if ((('[file:readPosition]' filename) & 1) == 1) {file_SkipBytes 1 filename}
    local 'rle' ('[file:readBytes]' 2 filename)
    if ((size rle) == 0) {
      return result
    }
    if ((at 1 rle) == 0) {
      if ((at 2 rle) > 2) {
        comment 'odd numbers for RL4?'
        result = ('[data:join]' result ('[file:readBytes]' ((((at 2 rle) * _bmpBPP) + 7) / 8) filename))
      } else {
        return result
      }
    } else {
      result = ('[data:join]' result ('[data:newByteArray]' ((((at 1 rle) * _bmpBPP) + 7) / 8) (at 2 rle)))
    }
  }
}

to drawBMPfile filename originX originY {
  if ('_bmp_readHeader' filename) {
    '_bmp_drawBMP' filename originX originY
    '[file:close]' filename
  }
}


module CoCube
author 'Liang Shuai'
version 2 17 
depends Tone Display TFT '_Motors/PID' 'Robots/CoCube Module' 
tags 'cocube robot' 
choices cocube_rotate_Menu 'cocube;left' 'cocube;right' 
choices cocube_move_Menu 'cocube;forward' 'cocube;backward' 
description 'CoCube Robot library

CoCube is a tabletop modular multi-robot platform for education and research.
https://www.cocubefun.com/
https://wiki.cocube.fun/
sliang23@m.fudan.edu.cn

v1.0 Basic functions
v2.0 Comprehensively add encoder control motor function
v2.1 Fix some bugs caused by broadcasting
v2.2 Improve positioning accuracy to 1/128
v2.3 Add blocks prompts
v2.4 Unify positioning accuracy to 1/64
v2.5 Adjust blocks order and delete RingTone
v2.6 Adjust blocks order, name and default parameters
v2.11 Improve the "CoCube move to" function logic
v2.12 Optimize PID implementation
v2.13 Optimize "set wheel" function
v2.14 Improve the accuracy of move_to_target and optimize the loop exit mechanism
v2.15 Remove "set TFT backlight" function
v2.16 Change PID global variables into local
v2.17 Modify the movement function on the map
'
variables _cocube_set_right_speed _cocube_set_left_speed _cocube_error_angle _cocube_pid_flag _cocube_error_distance _cocube_error_y _cocube_error_x _cocube_target_angle 

  spec ' ' 'CoCube move for msecs' 'move _ at _ speed (0-50) for _ msecs' 'menu.cocube_move_Menu num num' 'cocube;forward' 40 1000
  spec ' ' 'CoCube rotate for msecs' 'rotate _ at _ speed (0-50) for _ msecs' 'menu.cocube_rotate_Menu num num' 'cocube;left' 30 1000
  space
  spec ' ' 'CoCube move' 'move _ at _ speed (0-50)' 'menu.cocube_move_Menu num' 'cocube;forward' 40
  spec ' ' 'CoCube rotate' 'rotate _ at _ speed (0-50)' 'menu.cocube_rotate_Menu num' 'cocube;left' 30
  spec ' ' 'CoCube set wheel' 'set wheel left _ right _ (-50 to 50)' 'num num' 40 20
  space
  spec ' ' 'CoCube wheels stop' 'wheels stop'
  spec ' ' 'CoCube wheels break' 'wheels break'
  space
  spec 'r' 'CoCube position_X' 'X position'
  spec 'r' 'CoCube position_Y' 'Y position'
  spec 'r' 'CoCube direction' 'direction'
  space
  spec ' ' 'CoCube move by step' 'move _ at _ speed (0-50) by _ steps' 'menu.cocube_move_Menu num num' 'cocube;forward' 40 50
  spec ' ' 'CoCube rotate by degree' 'rotate _ at _ speed (0-50) by _ degrees' 'menu.cocube_rotate_Menu num num' 'cocube;left' 30 90
  space
  spec ' ' 'CoCube rotate to angle' 'rotate to angle _ at _ speed (0-50)' 'num num' 0 30
  spec ' ' 'CoCube point towards' 'point towards X _ Y _ at _ speed (0-50)' 'num num num' 0 0 30
  spec ' ' 'CoCube move to' 'move to X _ Y _ at _ speed (0-50)' 'num num num' 0 0 40
  space
  spec 'r' 'CoCube on the mat' 'on the mat'
  spec 'r' 'CoCube card ID' 'card ID'
  space
  spec ' ' 'CoCube draw ArUco marker on TFT' 'draw ArUco marker on TFT _' 'num' 0
  spec ' ' 'CoCube draw AprilTag on TFT' 'draw AprilTag on TFT _' 'num' 0
  space
  spec 'r' 'CoCube battery percentage' 'battery percentage'
  space
  spec ' ' '_cocube_motors_speed' '_cocube_motors_speed left _ right _' 'num num' 10 10
  spec ' ' '_cocube_pid_control' '_cocube_pid_control'
  spec ' ' '_cocube_calculate_angle_error_' '_cocube_calculate_angle_error angle _' 'num' 0
  spec ' ' '_cocube_calculate_distance_error target_x _ target_y _' '_cocube_calculate_distance_error Target_X _ Target_Y _' 'num num' 0 0

to 'CoCube battery percentage' {
  return (minimum (((254 * ('[misc:rescale]' (analogReadOp 34) 0 1023 0 3300)) - 680000) / 1600) 100)
}

to 'CoCube card ID' {
  return (callCustomReporter '[sensors:cube_index]')
}

to 'CoCube direction' {
  return (callCustomReporter '[sensors:position_yaw]')
}

to 'CoCube draw AprilTag on TFT' id {
  comment '0 <= id < 100'
  if (id >= 100) {
    sayIt '0 <= id < 100'
  } else {
    callCustomCommand '[tft:aprilTag]' ('[data:makeList]' id)
  }
}

to 'CoCube draw ArUco marker on TFT' id {
  comment '0 <= id < 100'
  if (id >= 100) {
    sayIt '0 <= id < 100'
  } else {
    callCustomCommand '[tft:aruco]' ('[data:makeList]' id)
  }
}

to 'CoCube move' direction speed {
  if (direction == 'cocube;forward') {
    'CoCube set wheel' speed speed
  } else {
    'CoCube set wheel' (0 - speed) (0 - speed)
  }
}

to 'CoCube move by step' direction speed steps {
  _cocube_move_flag = (booleanConstant true)
  if ('CoCube on the mat') {
    local 'begin_x' ('CoCube position_X')
    local 'begin_y' ('CoCube position_Y')
    'CoCube move' direction speed
    forever {
      if _cocube_move_flag {
        '_cocube_calculate_distance_error target_x _ target_y _' begin_x begin_y
        if ((steps - _cocube_error_distance) < 2) {
          'CoCube wheels break'
          exitLoop
        }
      } else {
        exitLoop
      }
    }
  } else {
    sayIt 'Please put CoCube on CoMaps'
  }
  _cocube_move_flag = (booleanConstant false)
}

to 'CoCube move for msecs' direction speed time {
  'CoCube move' direction speed
  waitMillis time
  'CoCube wheels break'
}

to 'CoCube move to' target_x target_y speed {
  _cocube_move_flag = (booleanConstant true)
  '_cocube_calculate_distance_error target_x _ target_y _' target_x target_y
  if (_cocube_error_distance > 3) {
    'CoCube point towards' target_x target_y speed
    repeatUntil (_cocube_error_distance < 3) {
      if _cocube_move_flag {
        '_cocube_calculate_distance_error target_x _ target_y _' target_x target_y
        _cocube_target_angle = (('[misc:atan2]' _cocube_error_y _cocube_error_x) / 100)
        '_cocube_calculate_angle_error_' _cocube_target_angle
        'CoCube set wheel' (speed - ((speed * _cocube_error_angle) / 20)) (speed + ((speed * _cocube_error_angle) / 20))
        waitMillis 10
      } else {
        exitLoop
      }
    }
    'CoCube wheels break'
  }
  _cocube_move_flag = (booleanConstant false)
}

to 'CoCube on the mat' {
  return (callCustomReporter '[sensors:cube_status]')
}

to 'CoCube point towards' target_x target_y speed {
  _cocube_target_angle = (('[misc:atan2]' (target_y - ('CoCube position_Y')) (target_x - ('CoCube position_X'))) / 100)
  'CoCube rotate to angle' _cocube_target_angle speed
}

to 'CoCube position_X' {
  return ((callCustomReporter '[sensors:position_x]') / 64)
}

to 'CoCube position_Y' {
  return ((callCustomReporter '[sensors:position_y]') / 64)
}

to 'CoCube rotate' direction speed {
  if (direction == 'cocube;left') {
    'CoCube set wheel' (0 - speed) speed
  } else {
    'CoCube set wheel' speed (0 - speed)
  }
}

to 'CoCube rotate by degree' direction speed degree {
  _cocube_move_flag = (booleanConstant true)
  if ('CoCube on the mat') {
    local 'begin_degree' ('CoCube direction')
    forever {
      if _cocube_move_flag {
        if (direction == 'cocube;left') {
          '_cocube_calculate_angle_error_' (begin_degree + degree)
        } else {
          '_cocube_calculate_angle_error_' (begin_degree - degree)
        }
        if (_cocube_error_angle > 1) {
          comment 'turn left'
          '_cocube_motors_speed' (0 - speed) speed
        } (_cocube_error_angle < -1) {
          comment 'turn right'
          '_cocube_motors_speed' speed (0 - speed)
        } else {
          comment 'motors break'
          'CoCube wheels break'
          exitLoop
        }
      } else {
        exitLoop
      }
    }
  } else {
    sayIt 'Please put CoCube on CoMaps'
  }
  _cocube_move_flag = (booleanConstant false)
}

to 'CoCube rotate for msecs' direction speed time {
  'CoCube rotate' direction speed
  waitMillis time
  'CoCube wheels break'
}

to 'CoCube rotate to angle' angle speed {
  local 'abs_speed' (absoluteValue speed)
  _cocube_move_flag = (booleanConstant true)
  forever {
    if _cocube_move_flag {
      '_cocube_calculate_angle_error_' angle
      if (_cocube_error_angle > 1) {
        comment 'turn left'
        '_cocube_motors_speed' (0 - abs_speed) abs_speed
      } (_cocube_error_angle < -1) {
        comment 'turn right'
        '_cocube_motors_speed' abs_speed (0 - abs_speed)
      } else {
        comment 'motors break'
        digitalWriteOp 9 true
        digitalWriteOp 10 true
        digitalWriteOp 26 true
        digitalWriteOp 25 true
        waitMillis 99
        digitalWriteOp 9 false
        digitalWriteOp 10 false
        digitalWriteOp 26 false
        digitalWriteOp 25 false
        exitLoop
      }
    } else {
      exitLoop
    }
  }
}

to 'CoCube set wheel' speed_left speed_right {
  if (and (speed_left == 0) (speed_right == 0)) {
    'CoCube wheels stop'
  } (and (speed_left == -99) (speed_right == -99)) {
    'CoCube wheels break'
  } else {
    _cocube_pid_flag = (booleanConstant true)
    _cocube_set_left_speed = speed_left
    _cocube_set_right_speed = speed_right
    sendBroadcast '_cocube_pid_control'
  }
}

to 'CoCube wheels break' {
  _cocube_pid_flag = (booleanConstant false)
  _cocube_move_flag = (booleanConstant false)
  waitMillis 1
  digitalWriteOp 9 true
  digitalWriteOp 10 true
  digitalWriteOp 26 true
  digitalWriteOp 25 true
  waitMillis 99
  digitalWriteOp 9 false
  digitalWriteOp 10 false
  digitalWriteOp 26 false
  digitalWriteOp 25 false
}

to 'CoCube wheels stop' {
  _cocube_pid_flag = (booleanConstant false)
  _cocube_move_flag = (booleanConstant false)
  waitMillis 1
  digitalWriteOp 9 false
  digitalWriteOp 10 false
  digitalWriteOp 26 false
  digitalWriteOp 25 false
}

to '_cocube_calculate_angle_error_' angle {
  _cocube_error_angle = ((angle % 360) - ('CoCube direction'))
  if (_cocube_error_angle > 180) {
    _cocube_error_angle += -360
  } (_cocube_error_angle < -180) {
    _cocube_error_angle += 360
  }
}

to '_cocube_calculate_distance_error target_x _ target_y _' target_x target_y {
  _cocube_error_x = (target_x - ('CoCube position_X'))
  _cocube_error_y = (target_y - ('CoCube position_Y'))
  _cocube_error_distance = ('[misc:sqrt]' ((_cocube_error_x * _cocube_error_x) + (_cocube_error_y * _cocube_error_y)))
}

to '_cocube_motors_speed' left_speed right_speed {
  comment 'without pid control'
  if (and (left_speed == 0) (right_speed == 0)) {
    'CoCube wheels stop'
  } else {
    local 'var_left' ((4 * (absoluteValue left_speed)) + 36)
    local 'var_right' ((4 * (absoluteValue right_speed)) + 36)
    if (left_speed > 0) {
      analogWriteOp 9 var_left
      digitalWriteOp 10 false
    } else {
      digitalWriteOp 9 false
      analogWriteOp 10 var_left
    }
    if (right_speed > 0) {
      analogWriteOp 26 var_right
      digitalWriteOp 25 false
    } else {
      digitalWriteOp 26 false
      analogWriteOp 25 var_right
    }
  }
}

to '_cocube_pid_control' {
  pid_resetPID 1
  pid_resetPID 2
  local 'pid_p' 320
  local 'pid_i' 1
  local 'pid_d' 3
  local 'pid_ratio' 1000
  local 'var_right' 0
  local 'var_left' 0
  forever {
    if _cocube_pid_flag {
      comment 'PID control'
      var_left = ((((4 * (absoluteValue _cocube_set_left_speed)) + 36) + (pid_computePID 1 ((18 * (absoluteValue _cocube_set_left_speed)) - (callCustomReporter '[sensors:speed_left]')) pid_p pid_i pid_d 0)) / pid_ratio)
      var_right = ((((4 * (absoluteValue _cocube_set_right_speed)) + 36) + (pid_computePID 2 ((18 * (absoluteValue _cocube_set_right_speed)) - (callCustomReporter '[sensors:speed_right]')) pid_p pid_i pid_d 0)) / pid_ratio)
      if (_cocube_set_left_speed > 0) {
        analogWriteOp 9 var_left
        digitalWriteOp 10 false
      } else {
        digitalWriteOp 9 false
        analogWriteOp 10 var_left
      }
      if (_cocube_set_right_speed > 0) {
        digitalWriteOp 25 false
        analogWriteOp 26 var_right
      } else {
        analogWriteOp 25 var_right
        digitalWriteOp 26 false
      }
    } else {
      exitLoop
    }
  }
}


module 'CoCube Module'
author 'Liang Shuai'
version 1 14 
depends _Servo NeoPixel 
choices trackerMenu '1' '2' '3' '4' '5' ALL 
description 'External modules of CoCube.



CoCube is a tabletop modular multi-robot platform for education and research.

https://www.cocubefun.com/

https://wiki.cocube.fun/

sliang23@m.fudan.edu.cn



Note: All functions start by turning on pin 13 to power the module.

The power pin is off by default and is also turned off by the IDE stop button.'
variables _cc_cam_init_flag _cc_asr_snc _cc_PCF8574_address 

  spec ' ' 'ccmodule_gripper open' 'gripper open'
  spec ' ' 'ccmodule_gripper close' 'gripper close'
  spec ' ' 'ccmodule_gripper degree' 'gripper degrees _ (0 to 70)' 'num' 0
  spec ' ' 'ccmodule_gripper stop' 'gripper stop'
  space
  spec ' ' 'ccmodule_attach NeoPixels' 'attach NeoPixels'
  spec ' ' 'ccmodule_set all NeoPixels color' 'set all NeoPixels color _' 'color'
  spec ' ' 'ccmodule_clear NeoPixels' 'clear NeoPixels'
  space
  spec 'r' 'ccmodule_ToF connected' 'ToF connected'
  spec 'r' 'ccmodule_ToF distance' 'ToF distance (mm)'
  space
  spec 'r' 'ccmodule_DLight level' 'DLight level'
  space
  spec 'r' 'ccmodule_ASR get command' 'ASR get command'
  spec ' ' 'ccmodule_ASR play command' 'ASR play command _' 'num' 19
  space
  spec 'r' 'ccmodule_tracker on black' 'Tracker _ on black' 'menu.trackerMenu' 1
  spec 'r' 'ccmodule_tracker on white' 'Tracker _ on white' 'menu.trackerMenu' 1
  space
  spec ' ' 'ccmodule_power on module' 'power on module'
  space
  spec ' ' '_ccmodule_check_camera' 'check_camera'

to '_ccmodule_check_camera' {
  if (_cc_cam_init_flag == 1) {
    '[sensors:i2cWrite]' (hexToInt '58') ('[data:asByteArray]' ('[data:makeList]' (hexToInt '3') 255))
  } else {
    stopServo 22
    digitalWriteOp 13 true
  }
}

to 'ccmodule_ASR get command' {
  '_ccmodule_check_camera'
  waitMillis 30
  '[sensors:i2cWrite]' (hexToInt '64') (hexToInt '2') false
  local 'data' (newList 2)
  '[sensors:i2cRead]' (hexToInt '64') data
  local 'new_asr_snc' (at 1 data)
  if (_cc_asr_snc != new_asr_snc) {
    _cc_asr_snc = new_asr_snc
    return (at 2 data)
  } else {
    return 0
  }
}

to 'ccmodule_ASR play command' id {
  '_ccmodule_check_camera'
  '[sensors:i2cWrite]' (hexToInt '64') ('[data:asByteArray]' ('[data:makeList]' (hexToInt '3') id (id + 3) (hexToInt '5A')))
}

to 'ccmodule_DLight level' {
  if (_cc_cam_init_flag == 1) {
    '[sensors:i2cWrite]' (hexToInt '58') ('[data:asByteArray]' ('[data:makeList]' (hexToInt '3') 255))
  } else {
    stopServo 22
    digitalWriteOp 13 true
  }
  i2cSet (hexToInt '23') 23 (hexToInt '21')
  read = (newList 2)
  '[sensors:i2cRead]' (hexToInt '23') read
  return (((at 1 read) << 8) | (at 2 read))
}

to 'ccmodule_ToF connected' {
  '_ccmodule_check_camera'
  return ((i2cGet 41 (hexToInt 'C0')) == (hexToInt 'EE'))
}

to 'ccmodule_ToF distance' {
  '_ccmodule_check_camera'
  i2cSet 41 0 1
  waitMillis 70
  if (((i2cGet 41 (hexToInt '14')) & 1) == 1) {
    '[sensors:i2cWrite]' 41 ('[data:makeList]' (hexToInt '14'))
    local 'buf' (newList 12)
    '[sensors:i2cRead]' 41 buf
    if ((at 1 buf) == 95) {
      return (((at 11 buf) << 8) | (at 12 buf))
    } else {
      return -1
    }
  } else {
    return -2
  }
}

to 'ccmodule_attach NeoPixels' {
  if (_cc_cam_init_flag == 1) {
    sayIt 'AI camera doesn''t support this module. Please connect directly.'
  } else {
    stopServo 22
    digitalWriteOp 13 true
    waitMillis 10
    neoPixelAttach 48 22
  }
}

to 'ccmodule_clear NeoPixels' {
  digitalWriteOp 13 true
  clearNeoPixels
}

to 'ccmodule_gripper close' {
  if (_cc_cam_init_flag == 1) {
    '[sensors:i2cWrite]' (hexToInt '58') ('[data:asByteArray]' ('[data:makeList]' (hexToInt '2') 0 28))
  } else {
    digitalWriteOp 13 true
    setServoAngle 22 -70
  }
}

to 'ccmodule_gripper degree' degree {
  if (_cc_cam_init_flag == 1) {
    stopServo 22
    local 'hex' (7168 - ((degree * 2048) / 70))
    '[sensors:i2cWrite]' (hexToInt '58') ('[data:asByteArray]' ('[data:makeList]' (hexToInt '2') (hex & (hexToInt 'FF')) (hex >> 8)))
  } else {
    digitalWriteOp 13 true
    local 'var' (degree - 70)
    if (var < -70) {
      setServoAngle 22 -70
    } (var > 0) {
      setServoAngle 22 0
    } else {
      setServoAngle 22 var
    }
  }
}

to 'ccmodule_gripper open' {
  if (_cc_cam_init_flag == 1) {
    '[sensors:i2cWrite]' (hexToInt '58') ('[data:asByteArray]' ('[data:makeList]' (hexToInt '2') 0 20))
  } else {
    digitalWriteOp 13 true
    setServoAngle 22 0
  }
}

to 'ccmodule_gripper stop' {
  if (_cc_cam_init_flag == 1) {
    '[sensors:i2cWrite]' (hexToInt '58') ('[data:asByteArray]' ('[data:makeList]' (hexToInt '2') 0 0))
  } else {
    digitalWriteOp 13 true
    stopServo 22
  }
}

to 'ccmodule_power on module' {
  digitalWriteOp 13 true
}

to 'ccmodule_set all NeoPixels color' color {
  digitalWriteOp 13 true
  neoPixelSetAllToColor color
}

to 'ccmodule_tracker on black' which {
  if (_cc_PCF8574_address != 32) {
    stopServo 22
    digitalWriteOp 13 true
    waitMillis 100
    _cc_PCF8574_address = 32
    if ((i2cGet _cc_PCF8574_address 0) < 0) {
      sayIt 'init fail, please check i2c address and try again.'
      return 0
    }
    '[sensors:i2cWrite]' _cc_PCF8574_address ('[data:makeList]' (hexToInt 'FF'))
  }
  local 'var' (newList 1)
  '[sensors:i2cRead]' _cc_PCF8574_address var
  if ('ALL' == which) {
    if ((at 1 var) == 7) {return (booleanConstant true)}
  } else {
    return (not ('[data:convertType]' (1 & ((at 1 var) >> (8 - which))) 'boolean'))
  }
}

to 'ccmodule_tracker on white' which {
  if (_cc_PCF8574_address != 32) {
    stopServo 22
    digitalWriteOp 13 true
    waitMillis 100
    _cc_PCF8574_address = 32
    if ((i2cGet _cc_PCF8574_address 0) < 0) {
      sayIt 'init fail, please check i2c address and try again.'
      return 0
    }
    '[sensors:i2cWrite]' _cc_PCF8574_address ('[data:makeList]' (hexToInt 'FF'))
  }
  local 'var' (newList 1)
  '[sensors:i2cRead]' _cc_PCF8574_address var
  if ('ALL' == which) {
    if ((at 1 var) == 255) {return (booleanConstant true)}
  } else {
    return ('[data:convertType]' (1 & ((at 1 var) >> (8 - which))) 'boolean')
  }
}


module Files Data
author MicroBlocks
version 1 4 
description 'Flash file system operations. Currently supports the LittleFS file system on ESP8266 and ESP32 boards. The GnuBlocks virtual machine (Linux and Raspberry Pi) supports the native system.'

  spec ' ' '[file:open]' 'open file _' 'str'
  spec ' ' '[file:close]' 'close file _' 'str'
  spec ' ' '[file:delete]' 'delete file _' 'str'
  space
  spec ' ' '[file:appendLine]' 'append line _ to file _' 'str str'
  spec ' ' '[file:appendBytes]' 'append bytes _ to file _' 'str str'
  space
  spec 'r' '[file:fileSize]' 'size of file _' 'str'
  spec 'r' 'file_contents' 'contents of file _ : binary _' 'str bool' '' false
  space
  spec 'r' '[file:readLine]' 'next line of file _' 'str'
  spec 'r' '[file:readBytes]' 'next _ bytes of file _ : starting at _' 'num str num' 100 '' 0
  spec 'r' '[file:readInto]' 'read into _ from file _' 'str str' 'a ByteArray' ''
  spec 'r' '[file:endOfFile]' 'end of file _' 'str'
  space
  spec 'r' '[file:readPosition]' 'read position of file _' 'str'
  spec ' ' '[file:setReadPosition]' 'set read position _ of file _' 'num str' 0 ''
  spec ' ' 'file_SkipBytes' 'skip _ bytes of file _' 'num str' 4 ''
  space
  spec 'r' 'file names' 'file names : in directory _' 'str'
  spec 'r' '[file:systemInfo]' 'file system info'

to 'file names' dir {
  '[file:startList]' dir
  local 'result' ('[data:makeList]')
  local 'fileName' ('[file:nextInList]')
  repeatUntil (fileName == '') {
    '[data:addLast]' fileName result
    fileName = ('[file:nextInList]')
  }
  return result
}

to file_SkipBytes offset file {
  local 'newPosition' (('[file:readPosition]' file) + offset)
  '[file:setReadPosition]' newPosition file
}

to file_contents fileName optionBinary {
  local 'binary' (argOrDefault 2 (booleanConstant false))
  local 'byteCount' ('[file:fileSize]' fileName)
  local 'result' ('[data:newByteArray]' byteCount)
  local 'i' 1
  '[file:open]' fileName
  repeatUntil (i > byteCount) {
    i += ('[file:readInto]' result fileName)
  }
  '[file:close]' fileName
  return (ifExpression binary result ('[data:convertType]' result 'string'))
}


module 'LED Display' Output
author MicroBlocks
version 1 15 
choices led_imageMenu heart 'small heart' yes no happy sad confused angry asleep surprised silly fabulous meh 't-shirt' 'roller skate' duck house tortoise butterfly 'stick figure' ghost sword giraffe skull umbrella snake rabbit cow 'quarter note' 'eight note' pitchfork target triangle 'left triangle' 'chess board' diamond 'small diamond' square 'small square' scissors 
description 'Display primitives for the 5x5 LED display on the BBC micro:bit, Calliope mini and M5Atom Matrix. Boards with TFT displays (such as the Citilab ED1 or the M5Stack family) support these primitives with a simulated "fat pixel" display.
'
variables _stop_scrolling_text 

  spec ' ' '[display:mbDisplay]' 'display _' 'microbitDisplay' 15237440
  spec ' ' 'led_displayImage' 'display image _ : x _ y _' 'menu.led_imageMenu num num' 'happy' 1 1
  spec ' ' '[display:mbDisplayOff]' 'clear display'
  space
  spec ' ' '[display:mbPlot]' 'plot x _ y _' 'num num' 3 3
  spec ' ' '[display:mbUnplot]' 'unplot x _ y _' 'num num' 3 3
  space
  spec ' ' 'displayCharacter' 'display character _' 'str' 'A'
  spec ' ' 'scroll_text' 'scroll text _ : pausing _ ms' 'str num' 'HELLO ROSA!' 100
  spec ' ' 'stopScrollingText' 'stop scrolling'
  advanced
  spec ' ' 'set display color' 'set display color _' 'color'
  spec 'r' 'led_image' 'LED image #BR# _' 'microbitDisplay' 15237440
  space
  spec 'r' '_led_namedImage' '_led_namedImage _' 'menu.led_imageMenu' 'happy'
  spec 'r' '_led_imageData' '_led_imageData'

to '_led_imageData' {
  return 'heart:4685802,small heart:145728,yes:2269696,no:18157905,happy:15237440,sad:18284864,confused:22348096,angry:23036241,asleep:459616,surprised:4526090,silly:25984017,fabulous:15008639,meh:2236443,t-shirt:15154043,roller skate:11534104,duck:489702,house:10976708,tortoise:359872,butterfly:29332475,stick figure:18158564,ghost:23068334,sword:4657284,giraffe:10946627,skull:15171246,umbrella:6460398,snake:469859,rabbit:16104613,cow:4685361,quarter note:7573636,eight note:7590276,pitchfork:4357813,target:4681156,triangle:1026176,left triangle:32805985,chess board:11184810,diamond:4539716,small diamond:141440,square:33080895,small square:469440,scissors:20287859,'
}

to '_led_namedImage' name {
  local 'data' ('_led_imageData')
  local 'i' ('[data:find]' name data)
  if (i == -1) {
    comment 'Name not found'
    return 0
  }
  local 'start' (('[data:find]' ':' data i) + 1)
  local 'end' (('[data:find]' ',' data i) - 1)
  return ('[data:convertType]' ('[data:copyFromTo]' data start end) 'number')
}

to displayCharacter s {
  s = ('[data:join]' '' s)
  if ((size s) == 0) {
    '[display:mbDisplayOff]'
    return 0
  }
  '[display:mbDrawShape]' ('[display:mbShapeForLetter]' (at 1 s)) 1 1
}

to led_displayImage imageName optionalX optionalY {
  local 'image' imageName
  if (isType image 'string') {
    image = ('_led_namedImage' imageName)
  }
  '[display:mbDrawShape]' image (argOrDefault 2 1) (argOrDefault 3 1)
}

to led_image twentyFiveBitInt {
  comment 'An LED image is a 25-bit integer'
  return twentyFiveBitInt
}

to scroll_text text optionalDelay {
  text = ('[data:join]' '' text)
  local 'delay' 100
  if ((pushArgCount) > 1) {
    delay = optionalDelay
  }
  _stop_scrolling_text = (booleanConstant false)
  if ('Pico:ed' == (boardType)) {
    for position (((size text) * 6) + 18) {
      if _stop_scrolling_text {return 0}
      '[display:mbDisplayOff]'
      '[tft:text]' text (17 - position) 0 (colorSwatch 125 125 125 255) 1 true
      waitMillis (delay / 2)
    }
  } (or ('KidsIOT' == (boardType)) ('CodingBox' == (boardType))) {
    for position (((size text) * 6) + 21) {
      if _stop_scrolling_text {return 0}
      '[tft:deferUpdates]'
      '[tft:clear]'
      '[tft:text]' text (128 - (6 * position)) 6 (colorSwatch 255 255 255 255) 6 false
      '[tft:resumeUpdates]'
      waitMillis (delay / 8)
    }
  } else {
    for position (((size text) * 6) + 6) {
      if _stop_scrolling_text {return 0}
      for i (size text) {
        '[display:mbDrawShape]' ('[display:mbShapeForLetter]' ('[data:unicodeAt]' i text)) (((i * 6) + 2) - position) 1
      }
      waitMillis delay
    }
  }
}

to 'set display color' color {
  '[display:mbSetColor]' color
}

to stopScrollingText {
  _stop_scrolling_text = (booleanConstant true)
  waitMillis 10
  '[display:mbDisplayOff]'
}


module NeoPixel Output
author MicroBlocks
version 1 15 
description 'Control NeoPixel (WS2812) RGB LED strips and rings.
'
variables _np_pixels _np_pin _np_haswhite 

  spec ' ' 'neoPixelAttach' 'attach _ LED NeoPixel strip to pin _ : has white _' 'num auto bool' 10 '' false
  spec ' ' 'setNeoPixelColors10' 'set NeoPixels _ _ _ _ _ _ _ _ _ _' 'color color color color color color color color color color'
  spec ' ' 'setNeoPixelColors25' 'set NeoPixels #BR# _ _ _ _ _ #BR# _ _ _ _ _ #BR# _ _ _ _ _ #BR# _ _ _ _ _ #BR# _ _ _ _ _' 'color color color color color color color color color color color color color color color color color color color color color color color color color'
  spec ' ' 'clearNeoPixels' 'clear NeoPixels'
  spec ' ' 'neoPixelSetAllToColor' 'set all NeoPixels color _' 'color'
  spec ' ' 'setNeoPixelColor' 'set NeoPixel _ color _' 'num color' 1
  space
  spec 'r' 'neoPixel_colorSwatch' '_' 'color'
  spec 'r' 'colorFromRGB' 'color r _ g _ b _ (0-255)' 'num num num' 0 100 100
  spec 'r' 'randomColor' 'random color'
  space
  spec ' ' 'rotateNeoPixelsBy' 'rotate NeoPixels by _' 'auto' 1
  space
  spec ' ' 'NeoPixel_brighten' 'brighten NeoPixel _ by _' 'num num' 1 10
  spec ' ' 'NeoPixel_brighten_all' 'brighten all NeoPixels by _' 'num' 10
  spec ' ' 'NeoPixel_shift_color' 'shift NeoPixel _ color by _' 'num num' 1 10
  spec ' ' 'NeoPixel_shift_all_colors' 'shift all NeoPixel colors by _' 'num' 10
  space
  spec ' ' '_NeoPixel_ensureInitialized' '_NeoPixel_ensureInitialized'
  spec ' ' '_NeoPixel_increaseRGB' '_NeoPixel_increaseRGB of _ by _' 'num num' 1 10
  spec ' ' '_NeoPixel_rotate' '_NeoPixel_rotate_left _' 'bool' true
  spec ' ' '_NeoPixel_update' '_NeoPixel_update'
  spec ' ' '_NeoPixel_shift_hue' '_NeoPixel_shift_hue of _ by _' 'auto auto' '10' '10'

to NeoPixel_brighten i delta {
  '_NeoPixel_increaseRGB' i delta
  '_NeoPixel_update'
}

to NeoPixel_brighten_all delta {
  for i (size _np_pixels) {
    '_NeoPixel_increaseRGB' i delta
  }
  '_NeoPixel_update'
}

to NeoPixel_shift_all_colors delta {
  for i (size _np_pixels) {
    '_NeoPixel_shift_hue' i delta
  }
  '_NeoPixel_update'
}

to NeoPixel_shift_color i delta {
  '_NeoPixel_shift_hue' i delta
  '_NeoPixel_update'
}

to '_NeoPixel_ensureInitialized' {
  if (_np_pixels == 0) {if (or ((boardType) == 'M5Atom-Matrix') (or ((boardType) == 'Mbits') ((boardType) == 'micro:STEAMakers'))) {
    neoPixelAttach 25 '' false
  } ((boardType) == 'D1-Mini') {
    comment 'D1 mini kit'
    neoPixelAttach 7 15 false
  } ((boardType) == 'Foxbit') {
    neoPixelAttach 35 '' false
  } ((boardType) == 'CodingBox') {
    neoPixelAttach 35 '' false
  } else {
    neoPixelAttach 10 '' false
  }}
}

to '_NeoPixel_increaseRGB' i delta {
  if (or (i < 1) (i > (size _np_pixels))) {return}
  local 'rgb' (at i _np_pixels)
  if (rgb != 0) {
    local 'h' ('[misc:hue]' rgb)
    local 's' ('[misc:saturation]' rgb)
    local 'v' (('[misc:brightness]' rgb) + delta)
    v = (maximum 20 (minimum v 100))
    atPut i _np_pixels ('[misc:hsvColor]' h s v)
  }
}

to '_NeoPixel_rotate' left {
  '_NeoPixel_ensureInitialized'
  local 'length' (size _np_pixels)
  if left {
    local 'first' (at 1 _np_pixels)
    for i (length - 1) {
      atPut i _np_pixels (at (i + 1) _np_pixels)
    }
    atPut length _np_pixels first
  } else {
    local 'last' (at length _np_pixels)
    for i (length - 1) {
      atPut ((length - i) + 1) _np_pixels (at (length - i) _np_pixels)
    }
    atPut 1 _np_pixels last
  }
}

to '_NeoPixel_shift_hue' i delta {
  if (or (i < 1) (i > (size _np_pixels))) {return}
  local 'rgb' (at i _np_pixels)
  if (rgb != 0) {
    local 'h' ((('[misc:hue]' rgb) + delta) % 360)
    local 's' ('[misc:saturation]' rgb)
    local 'v' ('[misc:brightness]' rgb)
    atPut i _np_pixels ('[misc:hsvColor]' h s v)
  }
}

to '_NeoPixel_update' {
  comment 'NeoPixel pin and hasWhite may have been changed by another library.'
  '[display:neoPixelSetPin]' _np_pin _np_hasWhite
  '[display:neoPixelSend]' _np_pixels
  waitMicros 300
}

to clearNeoPixels {
  '_NeoPixel_ensureInitialized'
  atPut 'all' _np_pixels 0
  '_NeoPixel_update'
}

to colorFromRGB r g b {
  r = (maximum 0 (minimum r 255))
  g = (maximum 0 (minimum g 255))
  b = (maximum 0 (minimum b 255))
  return (((r << 16) | (g << 8)) | b)
}

to neoPixelAttach number pinNumber optionalHasWhite {
  _np_pin = pinNumber
  _np_hasWhite = false
  if ((pushArgCount) > 2) {
    _np_hasWhite = optionalHasWhite
  }
  if (or (_np_pixels == 0) (number != (size _np_pixels))) {
    _np_pixels = (newList number)
  }
  atPut 'all' _np_pixels 0
  '[display:neoPixelSetPin]' _np_pin _np_hasWhite
}

to neoPixelSetAllToColor color {
  '_NeoPixel_ensureInitialized'
  atPut 'all' _np_pixels color
  '_NeoPixel_update'
}

to neoPixel_colorSwatch color {
  return color
}

to rotateNeoPixelsBy n {
  '_NeoPixel_ensureInitialized'
  local 'rotateLeft' (n < 0)
  if (or ((boardType) == 'CircuitPlayground') ((boardType) == 'CircuitPlayground Bluefruit')) {
    rotateLeft = (n > 0)
  }
  repeat (absoluteValue n) {
    '_NeoPixel_rotate' rotateLeft
  }
  '_NeoPixel_update'
}

to setNeoPixelColor i color {
  '_NeoPixel_ensureInitialized'
  if (and (1 <= i) (i <= (size _np_pixels))) {
    atPut i _np_pixels color
    '_NeoPixel_update'
  }
}

to setNeoPixelColors10 c1 c2 c3 c4 c5 c6 c7 c8 c9 c10 {
  '_NeoPixel_ensureInitialized'
  for i (minimum (size _np_pixels) (pushArgCount)) {
    atPut i _np_pixels (getArg i)
  }
  '_NeoPixel_update'
}

to setNeoPixelColors25 c1 c2 c3 c4 c5 c6 c7 c8 c9 c10 c11 c12 c13 c14 c15 c16 c17 c18 c19 c20 c21 c22 c23 c24 c25 {
  '_NeoPixel_ensureInitialized'
  for i (minimum (size _np_pixels) (pushArgCount)) {
    atPut i _np_pixels (getArg i)
  }
  '_NeoPixel_update'
}


module PID Operators
author 'Russell Owen'
version 0 9 
description 'PID Control Loop'
variables _pid__initialized _pid__numPIDs _pid__prevError _pid__prevMicros _pid__prevIntegral 

  spec 'r' 'pid_computePID' 'compute pid at index _ error _ p coeff _ i coeff _ d coeff _ max integral _ (ignored if 0)' 'num num num num num num' 1 0 1 0 0 0
  spec ' ' 'pid_resetPID' 'reset pid at index _' 'num' 1
  space
  spec 'r' 'pid_constrainValue' 'constrain value _ deadband _ minimum _ maximum _' 'num num num num' 0 0 0 1000
  spec 'r' 'pid_applySign' 'apply sign of _ to value _' 'num num' 1 0
  space
  spec ' ' '_pid_extendPIDLists' '_extend pid lists; index _' 'num' 1
  spec ' ' '_pid_initLibrary' '_init pid library'

to '_pid_extendPIDLists' index {
  comment 'Extend global list variables, if needed,
so that they are large enough to use the specified index.'
  '_pid_initLibrary'
  repeatUntil ((size _pid__prevError) >= index) {
    '[data:addLast]' 0 _pid__prevError
    '[data:addLast]' -1 _pid__prevMicros
    '[data:addLast]' 0 _pid__prevIntegral
  }
  comment 'Set _pid__numPIDs so pid_computePID can efficiently decide if the list needs to be extended'
  _pid__numPIDs = (size _pid__prevError)
}

to '_pid_initLibrary' {
  comment 'Create global lists.'
  if (not _pid__initialized) {
    _pid__prevError = (newList 0)
    _pid__prevMicros = (newList 0)
    _pid__prevIntegral = (newList 0)
    _pid__initialized = (booleanConstant true)
  }
}

to pid_applySign sign value {
  comment 'If sign < 0 return -value else return value'
  return (ifExpression (sign >= 0) value (0 - value))
}

to pid_computePID index error pCoeff iCoeff dCoeff maxIntegral {
  comment 'Compute the next PID value value, using inputs:
* index: index of the PID loop
* error: error to correct
* pCoeff: proportional coefficient (corr/error)
* iCoeff: integral coefficient (corr-msec/error)
* dCoeff: derivitive coefficient (corr/error-msec)
* maxIntegral: maximum absolute value of the integrated error; ignored if 0'
  if (_pid__numPIDs < index) {
    '_pid_extendPIDLists' index
  }
  local 'pValue' 0
  local 'iValue' 0
  local 'dValue' 0
  local 'integral' 0
  local 'currMicros' (microsOp)
  local 'prevMicros' (at index _pid__prevMicros)
  pValue = (pCoeff * error)
  if (prevMicros >= 0) {
    comment 'We have old data for this PID loop, so compute derivitive and integral contributions.'
    local 'deltaMicros' (microsSince prevMicros currMicros)
    local 'deltaErrorr' (error - (at index _pid__prevError))
    dValue = (((dCoeff * deltaErrorr) * 1000) / deltaMicros)
    integral = (((error * deltaMicros) / 1000) + (at index _pid__prevIntegral))
    if (and (maxIntegral > 0) ((absoluteValue integral) > maxIntegral)) {
      integral = (pid_applySign integral maxIntegral)
    }
    iValue = (iCoeff * integral)
  }
  atPut index _pid__prevMicros currMicros
  atPut index _pid__prevError error
  atPut index _pid__prevIntegral integral
  return (pValue + (iValue + dValue))
}

to pid_constrainValue value deadband minimum maximum {
  comment 'Constrain a value as follows:
If |value| < deadband: return 0.
If |value| < minimum: return minimum with sign of value.
If |value| > maximum: return maximum with sign of value.'
  local 'absValue' (absoluteValue value)
  if (absValue < deadband) {
    return 0
  } (absValue < minimum) {
    return (pid_applySign value minimum)
  } (absValue > maximum) {
    return (pid_applySign value maximum)
  } else {
    return value
  }
}

to pid_resetPID index {
  comment 'Zero the recorded error data for a specific PID loop.
Call this before starting each move, to avoid unwanted values
from the integral and derivitive terms.'
  if (_pid__numPIDs < index) {
    '_pid_extendPIDLists' index
  }
  atPut index _pid__prevError 0
  atPut index _pid__prevMicros -1
  atPut index _pid__prevIntegral 0
}


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 TFT Output
author MicroBlocks
version 1 16 
description 'Draw graphics and write text on boards with a TFT display, such as the M5Stack, M5Stick, Citilab ED1 or (discontinued) IoT-Bus.'

  spec ' ' '[tft:clear]' 'clear TFT display'
  space
  spec ' ' '[tft:rect]' 'draw rectangle on TFT at x _ y _ width _ height _ color _ : filled _' 'num num num num color bool' 10 10 40 30 nil true
  spec ' ' '[tft:roundedRect]' 'draw rounded rectangle on TFT at x _ y _ width _ height _ radius _ color _ : filled _' 'num num num num num color bool' 10 10 40 30 8 nil true
  spec ' ' '[tft:circle]' 'draw circle on TFT at x _ y _ radius _ color _ : filled _' 'num num num color bool' 40 40 30 nil true
  spec ' ' '[tft:triangle]' 'draw triangle on TFT at x _ y _ , x _ y _ , x _ y _ color _ : filled _' 'num num num num num num color bool' 20 20 30 80 60 5 nil true
  spec ' ' '[tft:line]' 'draw line on TFT from x _ y _ to x _ y _ color _' 'num num num num color' 12 8 25 15
  spec ' ' 'tft_drawVector' 'draw vector x _ y _ angle _ length _ color _' 'num num num num color' 40 40 45 40
  space
  spec ' ' '[tft:text]' 'write _ on TFT at x _ y _ color _ : scale _ wrap _ : bg color _' 'str num num color num bool color' 'Hello World!' 5 5 nil 2 true
  spec ' ' 'tft_drawText' 'draw text _ on TFT at x _ y _ color _ : scale _ : bg color _' 'str num num color num color' 'Line 1
Line 2' 50 20 nil 2
  space
  spec ' ' '[tft:setPixel]' 'set TFT pixel x _ y _ to _' 'num num color' 10 10
  spec ' ' '[tft:pixelRow]' 'draw pixel row _ x _ y _ : bytesPerPixel _ : palette _' 'auto num num num str' 'aList' 0 0 4
  spec ' ' '[tft:drawBitmap]' 'draw bitmap _ palette _ on TFT at x _ y _' 'str str num num' 'aBitmap' 'a list of colors' 10 10
  space
  spec 'r' 'tft_colorSwatch' '_' 'color'
  spec 'r' 'makeColor' 'color r _ g _ b _ (0-255)' 'num num num' 0 100 100
  spec 'r' 'makeGray' 'gray _ %' 'num' 50
  spec 'r' 'randomColor' 'random color'
  space
  spec 'r' '[tft:getWidth]' 'TFT width'
  spec 'r' '[tft:getHeight]' 'TFT height'
  space
  spec ' ' '[tft:setBacklight]' 'set TFT backlight _ (0-10)' 'num' 10
  spec ' ' '[tft:invertDisplay]' 'invert TFT display _' 'bool' false
  space
  spec ' ' '_deferMonochromeDisplayUpdates' '_defer monochrome display updates'
  spec ' ' '_resumeMonochromeDisplayUpdates' '_resume monochrome display updates'

to '_deferMonochromeDisplayUpdates' {
  '[tft:deferUpdates]'
}

to '_resumeMonochromeDisplayUpdates' {
  '[tft:resumeUpdates]'
}

to makeColor r g b {
  r = (maximum 0 (minimum r 255))
  g = (maximum 0 (minimum g 255))
  b = (maximum 0 (minimum b 255))
  return ((r << 16) | ((g << 8) | b))
}

to makeGray percent {
  gray = ((percent * 255) / 100)
  gray = (maximum 0 (minimum gray 255))
  return ((gray << 16) | ((gray << 8) | gray))
}

to randomColor {
  local 'n1' (random 100 200)
  local 'n2' (random 0 100)
  if (1 == (random 1 3)) {
    return ((n1 << 16) | (n2 << 8))
  } (1 == (random 1 2)) {
    return ((n2 << 16) | n1)
  } else {
    return ((n1 << 8) | n2)
  }
}

to tft_colorSwatch color {
  return color
}

to tft_drawText s x y color optionalScale optionalBGColor {
  s = ('[data:convertType]' s 'string')
  local 'scale' (argOrDefault 5 2)
  local 'bgColor' (argOrDefault 6 '')
  local 'lines' ('[data:split]' s ('[data:unicodeString]' 10))
  for line ('[data:split]' s ('[data:unicodeString]' 10)) {
    if (isType bgColor 'number') {
      '[tft:text]' line x y color scale false bgColor
    } else {
      '[tft:text]' line x y color scale false
    }
    y += (8 * scale)
  }
}

to tft_drawVector x y angle length color {
  local 'endX' (x + ((length * ('[misc:sin]' (100 * (angle + 90)))) >> 14))
  local 'endY' (y + ((length * ('[misc:sin]' (100 * angle))) >> 14))
  '[tft:line]' x y endX endY color
}


module Tone Output
author MicroBlocks
version 1 10 
tags tone sound music audio note speaker 
choices tone_NoteName 'nt;c' 'nt;c#' 'nt;d' 'nt;d#' 'nt;e' 'nt;f' 'nt;f#' 'nt;g' 'nt;g#' 'nt;a' 'nt;a#' 'nt;b' 
description 'Audio tone generation. Make music with MicroBlocks!
'
variables _tonePin _toneInitalized _toneLoopOverhead _toneNoteNames _toneArezzoNotes _toneFrequencies 

  spec ' ' 'play tone' 'play note _ octave _ for _ ms' 'str.tone_NoteName num num' 'nt;c' 0 500
  spec ' ' 'playMIDIKey' 'play midi key _ for _ ms' 'num num' 60 500
  spec ' ' 'play frequency' 'play frequency _ for _ ms' 'num num' 261 500
  space
  spec ' ' 'startTone' 'start tone _ Hz' 'num' 440
  spec ' ' 'stopTone' 'stop tone'
  space
  spec ' ' 'attach buzzer to pin' 'attach buzzer to pin _' 'auto' ''
  space
  spec 'r' '_measureLoopOverhead' '_measureLoopOverhead'
  spec 'r' '_baseFreqForNote' '_baseFreqForNote _' 'auto' 'c'
  spec 'r' '_baseFreqForSemitone' '_baseFreqForSemitone _' 'num' 0
  spec ' ' '_toneLoop' '_toneLoop _ for _ ms' 'num num' 440000 100
  spec 'r' '_trimmedLowercase' '_trimmedLowercase _' 'str' 'A. b C...'
  spec ' ' '_tone init note names' '_tone init note names'

to '_baseFreqForNote' note {
  comment 'Return the frequency for the given note in the middle-C octave
scaled by 1000. For example, return 440000 (440Hz) for A.
Note names may be upper or lower case. Note names
may be followed by # for a sharp or b for a flat.'
  local 'normalized note' ('_trimmedLowercase' note)
  'normalized note' = (ifExpression ((at 1 (v 'normalized note')) == 'n') (v 'normalized note') ('[data:join]' 'nt;' (v 'normalized note')))
  '_tone init note names'
  if (('[data:find]' (v 'normalized note') _toneArezzoNotes) > 0) {
    return ('_baseFreqForSemitone' ('[data:find]' (v 'normalized note') _toneArezzoNotes))
  } else {
    return ('_baseFreqForSemitone' ('[data:find]' (v 'normalized note') _toneNoteNames))
  }
}

to '_baseFreqForSemitone' semitone {
  if (_toneFrequencies == 0) {_toneFrequencies = ('[data:makeList]' 261626 277183 293665 311127 329628 349228 369994 391995 415305 440000 466164 493883 246942 277183 277183 311127 311127 349228 329628 369994 369994 415305 415305 466164 466164 523252)}
  if (and (1 <= semitone) (semitone <= (size _toneFrequencies))) {
    return (at semitone _toneFrequencies)
  } else {
    comment 'Bad note name; return 10 Hz'
    return 10000
  }
}

to '_measureLoopOverhead' {
  comment 'Measure the loop overhead on this device'
  local 'halfCycle' 100
  local 'startT' (microsOp)
  repeat 100 {
    digitalWriteOp _tonePin false
    waitMicros halfCycle
    digitalWriteOp _tonePin false
    waitMicros halfCycle
  }
  local 'usecs' ((microsOp) - startT)
  return ((usecs - 20000) / 200)
}

to '_tone init note names' {
  if (_toneNoteNames == 0) {
    _toneNoteNames = ('[data:makeList]' 'nt;c' 'nt;c#' 'nt;d' 'nt;d#' 'nt;e' 'nt;f' 'nt;f#' 'nt;g' 'nt;g#' 'nt;a' 'nt;a#' 'nt;b' 'nt;c_' 'nt;db' 'nt;d_' 'nt;eb' 'nt;e_' 'nt;e#' 'nt;f_' 'nt;gb' 'nt;g_' 'nt;ab' 'nt;a_' 'nt;bb' 'nt;b_' 'nt;b#')
    _toneArezzoNotes = ('[data:makeList]' 'nt;do' 'nt;do#' 'nt;re' 'nt;re#' 'nt;mi' 'nt;fa' 'nt;fa#' 'nt;sol' 'nt;sol#' 'nt;la' 'nt;la#' 'nt;si' 'nt;do_' 'nt;dob' 'nt;re_' 'nt;reb' 'nt;mi_' 'nt;mi#' 'nt;fa_' 'nt;solb' 'nt;sol_' 'nt;lab' 'nt;la_' 'nt;sib' 'nt;si_' 'nt;si#')
  }
}

to '_toneLoop' scaledFreq ms {
  if (_toneInitalized == 0) {'attach buzzer to pin' ''}
  if ('[io:hasTone]') {
    '[io:playTone]' _tonePin (scaledFreq / 1000)
    waitMillis ms
    '[io:playTone]' _tonePin 0
  } else {
    local 'halfCycle' ((500000000 / scaledFreq) - _toneLoopOverhead)
    local 'cycles' ((ms * 500) / halfCycle)
    repeat cycles {
      digitalWriteOp _tonePin true
      waitMicros halfCycle
      digitalWriteOp _tonePin false
      waitMicros halfCycle
    }
  }
}

to '_trimmedLowercase' s {
  comment 'Return a copy of the given string without whitespace
or periods and all lowercase.'
  local 'result' (newList (size s))
  '[data:delete]' 'all' result
  for i (size s) {
    local 'ch' ('[data:unicodeAt]' i s)
    if (and (ch > 32) (ch != 46)) {
      if (and (65 <= ch) (ch <= 90)) {ch = (ch + 32)}
      '[data:addLast]' ch result
    }
  }
  return ('[data:unicodeString]' result)
}

to 'attach buzzer to pin' pinNumber {
  if (pinNumber == '') {
    comment 'Pin number not specified; use default pin for this device'
    if ((boardType) == 'Citilab ED1') {
      _tonePin = 26
    } ((boardType) == 'M5Stack-Core') {
      _tonePin = 25
    } ((boardType) == 'M5StickC') {
      _tonePin = 26
    } ((boardType) == 'Calliope') {
      digitalWriteOp 23 true
      digitalWriteOp 24 true
      _tonePin = 25
    } ((boardType) == 'D1-Mini') {
      _tonePin = 12
    } ((boardType) == 'CodingBox') {
      _tonePin = 32
    } else {
      _tonePin = -1
    }
  } else {
    _tonePin = pinNumber
  }
  _toneLoopOverhead = ('_measureLoopOverhead')
  _toneInitalized = (booleanConstant true)
}

to 'play frequency' freq ms {
  '_toneLoop' (freq * 1000) ms
}

to 'play tone' note octave ms {
  local 'freq' ('_baseFreqForNote' note)
  if (freq <= 10000) {
    waitMillis ms
    return 0
  }
  if (octave < 0) {
    repeat (absoluteValue octave) {
      freq = (freq / 2)
    }
  }
  repeat octave {
    freq = (freq * 2)
  }
  '_toneLoop' freq ms
}

to playMIDIKey key ms {
  local 'freq' ('_baseFreqForSemitone' ((key % 12) + 1))
  local 'octave' ((key / 12) - 5)
  if (octave < 0) {
    repeat (absoluteValue octave) {
      freq = (freq / 2)
    }
  }
  repeat octave {
    freq = (freq * 2)
  }
  '_toneLoop' freq ms
}

to startTone freq {
  if (_toneInitalized == 0) {'attach buzzer to pin' ''}
  if ('[io:hasTone]') {'[io:playTone]' _tonePin freq}
}

to stopTone {
  startTone 0
}