plenbit.ts

//plenbit.ts

/**
 * Blocks for PLEN:bit
 */
//% weight=999 color=#00A654 icon="\uf0a0" block="PLEN:bit"
//% groups=['Motion', 'Sensor', 'Servo', 'Servo Adjust', 'PLEN:bit v1', 'PLEN:bit v2', 'SPECIAL KIT', 'others']
namespace plenbit {
  export enum DataPin {
    //% block="A button"
    AButtonSide = 0,
    //% block="B button"
    BButtonSide = 1
  }

  export enum LedOnOff {
    //% block="on"
    On = 0,
    //% block="off"
    Off = 1
  }

  export enum StdMotions {
    //% block="Walk Forward"
    WalkForward = 0x46,
    //% block="Walk Left Turn"
    WalkLTurn = 0x47,
    //% block="Walk Right Turn"
    WalkRTurn = 0x48,
    //% block="Walk Back"
    WalkBack = 0x49,
    //% block="Left step"
    LStep = 0x00,
    //% block="Forward step"
    FStep = 0x01,
    //% block="Right step"
    RStep = 0x02,
    //% block="A hem"
    AHem = 0x03,
    //% block="Bow"
    Bow = 0x04,
    //% block="Propose"
    Propose = 0x05,
    //% block="Hug"
    Hug = 0x06,
    //% block="Clap"
    Clap = 0x07,
    //% block="Highfive"
    HighFive = 0x08,
    //% block="Arm PataPata"
    ArmPataPata = 0x29
  }

  export enum BoxMotions {
    //% block="Shake A Box"
    ShakeABox = 0x0a,
    //% block="Pick Up High"
    PickUpHigh = 0x0b,
    //% block="Pick Up Low"
    PickUpLow = 0x0c,
    //% block="Receive a Box"
    ReceiveaBox = 0x0d,
    //% block="Present a Box"
    PresentaBox = 0x0e,
    //% block="Pass a Box"
    PassaBox = 0x0f,
    //% block="Throw a Box"
    ThrowaBox = 0x10,
    //% block="Put Down High"
    PutDownHigh = 0x11,
    //% block="Put Down Low"
    PutDownLow = 0x12,
    //% block="Carry For ward"
    CarryForward = 0x2A,
    //% block="Carry L Turn"
    CarryLTurn = 0x2B,
    //% block="Carry R Turn"
    CarryRTurn = 0x2c,
    //% block="Carry Back"
    CarryBack = 0x2d
  }

  export enum SocMotions {
    //% block="Defense Left Step"
    DefenseLStep = 0x14,
    //% block="Dribble"
    Dribble = 0x15,
    //% block="Defense Right Step"
    DefenseRStep = 0x16,
    //% block="Left Kick"
    LKick = 0x17,
    //% block="Long Dribble"
    LongDribble = 0x18,
    //% block="Right Kick"
    RKick = 0x19,
    //% block="Pass To Left"
    PassToLeft = 0x1a,
    //% block="Pass It To Me"
    PassItToMe = 0x1b,
    //% block="Pass To Right"
    PassToRight = 0x1c
  }

  export enum DanceMotions {
    //% block="Dance Left Step"
    DanceLStep = 0x1e,
    //% block="Dance Forward Step"
    DanceFStep = 0x1f,
    //% block="Dance Right Step"
    DanceRStep = 0x20,
    //% block="Dance Fisnish Pose"
    DanceFisnishPose = 0x21,
    //% block="Dance Up Down"
    DanceUpDown = 0x22,
    //% block="Wiggle Dance"
    WiggleDance = 0x23,
    //% block="Dance Back Step"
    DanceBStep = 0x24,
    //% block="Dance Bow"
    DanceBow = 0x25,
    //% block="Twist Dance"
    TwistDance = 0x26
  }

  export enum WalkMode {
    //% block="move"
    Move = 1,
    //% block="stop"
    Stop = 0
  }

  export enum THsensorMode {
    //% block="temperature(℃)"
    Temperature = 0,
    //% block="humidity(%)"
    Humidity = 1
  }

  export enum NeoPixelColors {
    //% block=green
    Green = 0x00FF00,
    //% block=red
    Red = 0xFF0000,
    //% block=orange
    Orange = 0xFFA500,
    //% block=yellow
    Yellow = 0xFFFF00,
    //% block=blue
    Blue = 0x0000FF,
    //% block=indigo
    Indigo = 0x4b0082,
    //% block=violet
    Violet = 0x8a2be2,
    //% block=purple
    Purple = 0xFF00FF,
    //% block=white
    White = 0xFFFFFF,
    //% block=off
    Black = 0x000000
  }

  let motionSpeed = 20;
  export let servoInitArray = [1000, 630, 300, 600, 240, 600, 1000, 720, 900, 900, 900, 900]
  let servoInitArraySave: number[] = []
  for (let i = 0; i < servoInitArray.length; i++) servoInitArraySave.push(servoInitArray[i])
  const servoReverse = [false, false, false, false, false, false, false, false, true, true, true, true] //サーボ反転
  let servoAngle = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
  let SERVO_NUM_USED = 8
  let romAdr1 = 0x56
  let initBle = false
  const eepromAdr = 0x56
  const PCA9865Adr = 0x6A
  let initEEPROMFlag = false
  let initPCA9865Flag = false
  let magneticForceOffset = { x: 50, z: 50 }
  let walkingMode = 0;
  let plenEyeCreated = false;
  let hardwareVersion = parseInt(control.hardwareVersion())
  let AM2320LastUpdateTime_A = 0
  let AM2320LastUpdateTime_B = 0
  let temperature = 0
  let humidity = 0
  let co2ppm = 0
  let eyeColor = pins.createBuffer(3)
  let eyeBrightnes = 127

  // 初期化
  servoInitialSet()
  eyeLed(LedOnOff.On)
  eyeColor[1] = 255
  setEyeBrightness(20)
  basic.pause(1000)
  servoFree()

  // 検査用フラグ
  export let Test_isTHsensorSuccess: boolean = false

  export function initPCA9865() { // PCA9685の初期設定
    if (readPCA9865(0xFE) != 0x00) { // PRE_SCALEが読み取れる <=> PCA9865が接続済み
      initPCA9865Flag = true
      writePCA9865(0x00, 0x10) // Sleep modeをONにして、内部クロックを停止
      writePCA9865(0xFE, 0x85) // PRE_SCALEを設定　※ cf.P13 Writes to PRE_SCALE register are blocked when SLEEP bit is logic 0 (MODE 1)
      writePCA9865(0x00, 0x00) // Sleep modeをOFFにして、内部クロックをPRE_SCALEで動かす

      writePCA9865(0xFA, 0x00) // ALL_LED_ON_L　全PWMのONのタイミングを0にする
      writePCA9865(0xFB, 0x00) // ALL_LED_ON_H　　　　　　　〃
      writePCA9865(0xFC, 0x00) // ALL_LED_OFF_L　全PWMのOFFのタイミングを0にする
      writePCA9865(0xFD, 0x00) // ALL_LED_OFF_H　　　　　　　〃
    }
  }

  function readPCA9865(addr: number) {
    pins.i2cWriteNumber(PCA9865Adr, addr, NumberFormat.UInt8LE, false)
    return pins.i2cReadNumber(PCA9865Adr, NumberFormat.UInt8LE, false)
  }

  function writePCA9865(addr: number, d: number) {
    let cmd = pins.createBuffer(2);
    cmd[0] = addr;
    cmd[1] = d;
    pins.i2cWriteBuffer(PCA9865Adr, cmd, false);
  }

  function initEEPROM() { // EEPROMのサーボ初期位置設定を反映する
    if (ReadEEPROM(0x32, 1)[0] != 0x00) { // モーションデータが読み取れる <=> EEPROMが接続済み
      initEEPROMFlag = true
      if (ReadEEPROM(0x00, 1)[0] == 0x01) {　 // サーボ初期位置調整済み
        let readBuf = ReadEEPROM(0x02, 16)
        for (let i = 0; i < 8; i++) {
          servoInitArray[i] = (readBuf[i * 2] << 8) | (readBuf[i * 2 + 1])
        }
      }
    }
  }

  export function ReadEEPROM(eepAdr: number, num: number) {
    let data = pins.createBuffer(2);
    data[0] = eepAdr >> 8;
    data[1] = eepAdr & 0xFF;
    // need adr change code
    pins.i2cWriteBuffer(eepromAdr, data);
    return pins.i2cReadBuffer(eepromAdr, num, false);
  }

  function WriteEEPROM(eepAdr: number, num: number) {
    let data = pins.createBuffer(3);
    data[0] = eepAdr >> 8;
    data[1] = eepAdr & 0xFF;
    data[2] = num;
    pins.i2cWriteBuffer(eepromAdr, data);
    basic.pause(5)
  }

  function motionFlame(fileName: number, flameNum: number) {
    doMotion(fileName, flameNum);
  }

  function doMotion(fileName: number, flameNum: number) {
    let data = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0];
    let command = 0x3e;//">"
    let listLen = 43;
    let loopBool = false;
    let plenDebug: boolean = false;
    if (flameNum == 0xFF) {
      flameNum = 0;
      loopBool = true;
    }
    let readAdr = 0x32 + 860 * fileName + flameNum * listLen;
    let error = 0;
    while (1) {
      let mBuf = ReadEEPROM(readAdr, listLen);
      readAdr += listLen;
      if (mBuf[0] == 0xFF) break;
      let mf = "";    //=null ?
      let listNum = 0;
      if (command != mBuf[listNum]) break;
      if (plenDebug) serial.writeString(",>OK");
      listNum += 1; // >
      if (0x4d46 != (mBuf[listNum] << 8 | mBuf[listNum + 1])) break;
      if (plenDebug) serial.writeString(",mfOK");
      listNum += 2; // MF
      if (fileName != (num2Hex(mBuf[listNum]) << 4 | num2Hex(mBuf[listNum + 1]))) break;
      if (plenDebug) serial.writeString(",fileOK");
      listNum += 4;// slot,flame
      let time = num2Hex(mBuf[listNum]) << 12 | num2Hex(mBuf[listNum + 1]) << 8 | num2Hex(mBuf[listNum + 2]) << 4 | num2Hex(mBuf[listNum + 3]);
      listNum += 4;
      for (let val = 0; val < SERVO_NUM_USED; val++) {
        let numHex = num2Hex(mBuf[listNum]) << 12 | num2Hex(mBuf[listNum + 1]) << 8 | num2Hex(mBuf[listNum + 2]) << 4 | num2Hex(mBuf[listNum + 3]);
        if (numHex >= 0x7fff) {
          numHex = numHex - 0x10000;
        } else {
          numHex = numHex & 0xFFff;
        }
        data[val] = numHex / 10;
        if (plenDebug) serial.writeNumber(data[val]);
        listNum += 4;
      }
      setAngle(data, time);
      if (!loopBool) break;
    }
  }

  function num2Hex(num: number) {
    let i: number = 0;
    if (48 <= num && num <= 57) {
      i = (num - 48);
    } else if (62 <= num && num <= 77) {
      switch (num) {
        //case 62: i = 0x3e; break;
        case 65: i = 0x0a; break;
        case 66: i = 0x0b; break;
        case 67: i = 0x0c; break;
        case 68: i = 0x0d; break;
        case 69: i = 0x0e; break;
        case 70: i = 0x0f; break;
        //case 77: i = 0x4d; break;
        default: i = 0; break;
      }
    } else if (97 <= num && num <= 102) {
      switch (num) {
        case 97: i = 0x0a; break;
        case 98: i = 0x0b; break;
        case 99: i = 0x0c; break;
        case 100: i = 0x0d; break;
        case 101: i = 0x0e; break;
        case 102: i = 0x0f; break;
        default: i = 0; break;
      }
    }
    return i;
  }

  function parseIntM(str: string) {
    let len = str.length;
    let num = [0, 0, 0, 0];
    for (let i = 0; i < len; i++) {
      switch (str[i]) {
        case "a": num[i] = 10; break;
        case "b": num[i] = 11; break;
        case "c": num[i] = 12; break;
        case "d": num[i] = 13; break;
        case "e": num[i] = 14; break;
        case "f": num[i] = 15; break;
        case "A": num[i] = 10; break;
        case "B": num[i] = 11; break;
        case "C": num[i] = 12; break;
        case "D": num[i] = 13; break;
        case "E": num[i] = 14; break;
        case "F": num[i] = 15; break;
        default:
          num[i] = parseInt(str[i]);
          break;
      }
    }
    let hex = 0;
    switch (len) {
      case 4: hex = (num[len - 4] * 0x1000);
      case 3: hex += (num[len - 3] * 0x0100);
      case 2: hex += (num[len - 2] * 0x0010);
      case 1: hex += (num[len - 1] * 0x0001);
    }
    return hex;
  }

  function bleInit() {
    serial.redirect(SerialPin.P8, SerialPin.P12, 115200);
    pins.digitalWritePin(DigitalPin.P16, 0);
    initBle = true;
  }

  // 温湿度センサー
  function UpdateAM2320(dataPin: DigitalPin, serialDebug: boolean) {
    Test_isTHsensorSuccess = false

    let isAside = false
    if (dataPin == DigitalPin.P0) isAside = true;

    let count

    let DataTimeArray = []

    let DataArray: boolean[] = []
    let ResultArray: number[] = []
    for (let index = 0; index < 40; index++) DataArray.push(false)
    for (let index = 0; index < 5; index++) ResultArray.push(0)

    function WaitHigh() {
      for (let i = 0; i < 10000; i++) {
        if (pins.digitalReadPin(dataPin) == 0) break
      }
    }

    function WaitLow() {
      for (let i = 0; i < 10000; i++) {
        if (pins.digitalReadPin(dataPin) == 1) break
      }
    }

    if (isAside) {
      while (input.runningTime() - AM2320LastUpdateTime_A < 2000);
    } else {
      while (input.runningTime() - AM2320LastUpdateTime_B < 2000);
    }

    pins.digitalWritePin(dataPin, 0)
    basic.pause(10)
    pins.setPull(dataPin, PinPullMode.PullUp)
    count = 0
    let errorFlag = true
    for (let i = 0; i < 10000; i++) {
      if (pins.digitalReadPin(dataPin) == 0) {
        errorFlag = false
        break
      }
    }

    if (errorFlag) {
      if (serialDebug) serial.writeLine("AM2320 is not responding! (Plsese check the TH Sensor connection)")
      if (isAside) {
        AM2320LastUpdateTime_A = input.runningTime() - 1000
      } else {
        AM2320LastUpdateTime_B = input.runningTime() - 1000
      }
    } else {
      WaitLow()
      WaitHigh()
      if (hardwareVersion == 2) { //v2の場合、割り込み処理が多い
        while (DataTimeArray.length < 41) {
          DataTimeArray.push(input.runningTimeMicros())
          WaitLow()
          WaitHigh()
        }

        if (DataTimeArray.length == 41) {
          for (let i = 0; i < 40; i++) {
            if (DataTimeArray[i + 1] - DataTimeArray[i] > 110) {
              DataArray[i] = true
            }
          }
        } else {
          errorFlag = true
          if (serialDebug) serial.writeLine("Data Get Error! (Sensor data has missed " + (41 - DataTimeArray.length).toString() + " data)")
        }

      } else { //v1の場合、動作周波数が低い
        for (let i = 0; i < 40; i++) {
          WaitHigh()
          WaitLow()
          control.waitMicros(30)
          if (pins.digitalReadPin(dataPin) == 1) DataArray[i] = true
        }
      }

      if (isAside) {
        AM2320LastUpdateTime_A = input.runningTime()
      } else {
        AM2320LastUpdateTime_B = input.runningTime()
      }

      if (errorFlag == false) {
        let checkSum = 0
        for (let i = 0; i < 5; i++) {
          for (let j = 0; j < 8; j++) {
            if (i == 0 && j != 0 || i == 2 && j != 0 || i == 1 || i == 3 || i == 4) { //1,3byteの8bit目はチェックデジットに含まない
              if (DataArray[8 * i + j]) {
                ResultArray[i] += 2 ** (7 - j)
              }
            }
          }
          if (i < 4) {
            checkSum += ResultArray[i]
          }
        }

        if (checkSum == 0 || ResultArray[4] == 0) { //全て0はありえないため、弾く
          if (serialDebug) serial.writeLine("Data Value Error! (Temperature & Humidity is zero, but it looks broken data)")
        } else {
          if (Math.abs(checkSum - ResultArray[4]) <= 256) { //Checksumが明らかにおかしい場合のみ弾く
            let flag1 = false
            let flag2 = false
            let valuecheck = 0
            valuecheck = (ResultArray[0] * 256 + ResultArray[1]) / 10
            if (0 < valuecheck && valuecheck < 100) { //異常な湿度を弾く
              flag1 = true
              humidity = valuecheck
            } else {
              if (serialDebug) serial.writeLine("Data Value Error! (Humidity is " + valuecheck.toString() + ", but it looks broken data)")
            }
            valuecheck = (ResultArray[2] * 256 + ResultArray[3]) / 10
            if (DataArray[16]) { //左から17bit目（温度の最上位bit）が1のとき、マイナス
              valuecheck *= -1
            }
            if (-40 < valuecheck && valuecheck < 80) { //異常な温度を弾く
              flag2 = true
              temperature = valuecheck
            } else {
              if (serialDebug) serial.writeLine("Data Value Error! (Temperature is " + valuecheck.toString() + ", but it looks broken data)")
            }
            if (flag1 && flag2) Test_isTHsensorSuccess = true; //読み取り成功
            if (serialDebug) serial.writeLine("Temperature: " + temperature + " C")
            if (serialDebug) serial.writeLine("Humidity: " + humidity + " %")
            if (serialDebug) serial.writeLine("-------------------")
          } else {
            if (serialDebug) serial.writeLine("Check Sum Error! (Sensor data has incorrect check digit [" + (checkSum - ResultArray[4]).toString() + "])")
          }
        }
      }
    }
  }

  // CO2センサー
  function ReadMHZ19C() {
    let dataPin = DigitalPin.P0
    let noSensor = false

    function WaitHigh() {
      if (!noSensor) {
        for (let i = 0; i < 1500; i++) {
          if (pins.digitalReadPin(dataPin) == 0) return -1
          basic.pause(1)
        }
        noSensor = true
      }
      return -1
    }

    function WaitLow() {
      if (!noSensor) {
        for (let i = 0; i < 1500; i++) {
          if (pins.digitalReadPin(dataPin) == 1) return -1
          basic.pause(1)
        }
        noSensor = true
      }
      return -1
    }

    let T = 0
    let TH = 0

    WaitHigh()
    WaitLow()
    T = control.millis()
    WaitHigh()
    TH = control.millis() - T
    WaitLow()
    T = control.millis() - T

    let co2ppmResult = co2ppm
    if (Math.abs(T - 1000) < 100) {
      co2ppmResult = Math.round(2000 * (TH - 2) / (T - 4))
      co2ppm = co2ppmResult
    }

    if (noSensor) serial.writeLine("MH-Z19C is not responding! (Plsese check the CO2 Sensor connection)")

    return co2ppmResult
  }

  //フルカラーLEDを点灯
  function setEyeLED() {
    let data = pins.createBuffer(6)
    for (let i = 0; i < 3; i++) {
      data[i] = (eyeColor[i] * eyeBrightnes / 255) & 0xFF
      data[i + 3] = data[i]
    }
    ws2812b.sendBuffer(data, DigitalPin.P16)
  }

  //PLEN:bitブロック
  //モーション

  /**
   * Play the Standard Motion on PLEN:bit.
   */
  //% blockId=PLEN:bit_motion_std
  //% block="play std motion %fileName"
  //% weight=10 group="Motion"
  export function stdMotion(fileName: StdMotions) {
    motion(fileName);
  }

  /**
   * Play the Soccer Motion on PLEN:bit.
   */
  //% blockId=PLEN:bit_motion_Soc
  //% block="play soccer motion %fileName"
  //% weight=9 group="Motion"
  export function soccerMotion(fileName: SocMotions) {
    motion(fileName);
  }

  /**
   * Play the Box Motion on PLEN:bit.
   */
  //% blockId=PLEN:bit_motion_box
  //% block="play box motion %fileName"
  //% weight=8 group="Motion"
  //% deprecated=true
  export function boxMotion(fileName: BoxMotions) {
    motion(fileName);
  }

  /**
   * Play the Dance Motion on PLEN:bit.
   */
  //% blockId=PLEN:bit_motion_dan
  //% block="play dance motion %fileName"
  //% weight=7 group="Motion"
  export function danceMotion(fileName: DanceMotions) {
    motion(fileName);
  }

  /**
   * Play the Continuous Walking Motion on PLEN:bit.
   */
  //% block="walk %mode"
  //% weight=5 group="Motion"
  export function walk(mode: WalkMode) {
    if (mode == 1) {
      if (walkingMode == 0) {
        walkingMode = 0;
      } else if (walkingMode == 100) {
        walkingMode = 0;
      }
    } else {
      if (walkingMode == 1) {
        walkingMode = 2;
      } else {
        walkingMode = 100;
      }
    }
    switch (walkingMode) {
      case 0:
        motionFlame(StdMotions.WalkForward, 0);
        motionFlame(StdMotions.WalkForward, 1);
        walkingMode = 1;
      //break;
      case 1:
        motionFlame(StdMotions.WalkForward, 2);
        motionFlame(StdMotions.WalkForward, 3);
        motionFlame(StdMotions.WalkForward, 4);
        motionFlame(StdMotions.WalkForward, 5);
        motionFlame(StdMotions.WalkForward, 6);
        motionFlame(StdMotions.WalkForward, 7);
        break;
      case 2:
        motionFlame(StdMotions.WalkForward, 8);
        motionFlame(StdMotions.WalkForward, 9);
        walkingMode = 0;
        servoFree()
        break;
      default:
        break;
    }
  }

  /**
   * Change the playing speed of the PLEN:bit motion.
   * @param speed 0 ~ 50, The larger this value, the faster.
   */
  //% block="motion Speed %speed"
  //% speed.min=1 speed.max=50 speed.defl=20
  //% weight=4 group="Motion"
  export function changeMotionSpeed(speed: number) {
    motionSpeed = speed
  }

  /**
   * Set Servo Motors to initial position.
   */
  //% block="servo motor initial"
  //% weight=3 group="Motion"
  export function servoInitialSet() {
    for (let n = 0; n < 12; n++) {
      servoWriteInit(n, 0);
    }
  }


  //センサー

  /**
   * Get the value of analog sensor.
   */
  //% blockId=PLEN:bit_Sensor
  //% block="read %num side analog sensor"
  //% weight=10 group="Sensor"
  export function sensorLR(num: DataPin) {
    return pins.analogReadPin((num == 1) ? AnalogPin.P2 : AnalogPin.P0);
  }

  /**
   * Get the direction angle that PLEN: bit faces.
   */
  //% block="direction(°)"
  //% weight=9 group="Sensor"
  export function direction() {
    let x = input.magneticForce(Dimension.X)
    let z = input.magneticForce(Dimension.Z)
    let s = input.magneticForce(Dimension.Strength) / 8

    let magneticForceX = x + magneticForceOffset.x
    let magneticForceZ = z + magneticForceOffset.z

    let check = Math.abs(magneticForceX)
    if (check > s) {
      let offset = check - s
      if (magneticForceX > 0) offset *= -1
      magneticForceX += offset
      magneticForceOffset.x += offset
    }
    check = Math.abs(magneticForceZ)
    if (check > s) {
      let offset = check - s
      if (magneticForceZ > 0) offset *= -1
      magneticForceZ += offset
      magneticForceOffset.z += offset
    }

    let dir = Math.atan2(magneticForceX, magneticForceZ) * 180 / 3.14 + 180
    if (dir >= 360) dir = 0
    if (dir < 0) dir = 359
    // TODO: 小数部分を切り捨てして返すかどうか選択できるようにする
    // return Math.floor(dir)
    return dir
  }

  //v1専用

  /**
   * [PLEN:bit v1] Switch the eye led of PLEN:bit.
   */
  //% block="eye led is %LedOnOff"
  //% weight=10 group="PLEN:bit v1"
  export function eyeLed(LedOnOff: LedOnOff) {
    //v2対応

    if (LedOnOff) {
      setColor(NeoPixelColors.Green)
    } else {
      setColor(NeoPixelColors.Black)
    }

    //NeoPixel通信終わるまでpause
    basic.pause(1)
    pins.digitalWritePin(DigitalPin.P8, LedOnOff)
    pins.digitalWritePin(DigitalPin.P16, LedOnOff)
  }


  //v2専用

  /**
   * [PLEN:bit v2] Change the eye led color of PLEN:bit.
   */
  //% block="show color %color for Eye LED"
  //% weight=10 group="PLEN:bit v2"
  export function setColor(color: NeoPixelColors) {
    for (let i = 0; i < 3; i++) eyeColor[2 - i] = (color >> i * 8) & 0xFF
    setEyeLED()
  }


  //その他
  //サーボモーター

  /**
   * Play the Motion on PLEN:bit.
   * You can check the list of Motion Number at GitHub.
   * @param fileName https://github.com/plenprojectcompany/pxt-PLENbit/blob/master/PLENbit_Motion.pdf
   */
  //% blockId=PLEN:bit_motion
  //% block="play motion %fileName"
  //% fileName.min=0 fileName.max=73 fileName.defl=0
  //% weight=10 group="Servo" advanced=true
  export function motion(fileName: number) {
    doMotion(fileName, 0xFF);
    servoFree()
  }

  /**
   * Controll the each servo motors. The servo will move max speed.
   * @param speed 0 ~ 50, The larger this value, the faster.
   */
  //% blockId=PLEN:bit_servo_Init
  //% block="servo motor %num|number %degrees|degrees"
  //% num.min=0 num.max=11
  //% degrees.min=-90 degrees.max=90 degrees.defl=0
  //% weight=8 group="Servo" advanced=true
  export function servoWriteInit(num: number, degrees: number) {
    let servoNum = 0x08;
    if (servoReverse[num]) degrees *= -1
    if (initEEPROMFlag == false) initEEPROM()
    if (initPCA9865Flag == false) initPCA9865()
    let highByte = false
    let pwmVal = (servoInitArray[num] / 10 - degrees) * 100 * 226 / 10000
    if (pwmVal < 0) pwmVal = 0
    if (pwmVal > 384) pwmVal = 384
    pwmVal = Math.round(pwmVal) + 0x66
    if (pwmVal > 0xFF) {
      highByte = true
    }
    writePCA9865(servoNum + num * 4, pwmVal)
    if (highByte) {
      writePCA9865(servoNum + num * 4 + 1, 0x01)
    } else {
      writePCA9865(servoNum + num * 4 + 1, 0x00)
    }
    servoAngle[num] = degrees
  }

  //% blockId=PLEN:bit_servo
  //% block="servo motor %num|number %degrees|degrees"
  //% num.min=0 num.max=7
  //% degrees.min=0 degrees.max=180
  //% weight=7 group="Servo" advanced=true
  //% deprecated=true
  export function servoWrite(num: number, degrees: number) {
    let servoNum = 0x08;
    if (initEEPROMFlag == false) initEEPROM()
    if (initPCA9865Flag == false) initPCA9865()
    let highByte = false
    let pwmVal = degrees * 100 * 226 / 10000
    if (pwmVal < 0) pwmVal = 0
    if (pwmVal > 384) pwmVal = 384
    pwmVal = Math.round(pwmVal) + 0x66
    if (pwmVal > 0xFF) {
      highByte = true;
    }
    writePCA9865(servoNum + num * 4, pwmVal)
    if (highByte) {
      writePCA9865(servoNum + num * 4 + 1, 0x01)
    } else {
      writePCA9865(servoNum + num * 4 + 1, 0x00)
    }
    servoAngle[num] = servoInitArray[num] / 10 - degrees
  }

  /**
   * Eight servos can be controlled at once
   * @param angle 8 arrays
   * @param msec milliseconds
   */
  //% block="set angle $angle, msec %msec"
  //% msec.min=100 msec.max=1000 msec.defl=500
  //% weight=6 group="Servo" advanced=true
  //% deprecated=true
  export function setAngle(angle: number[], msec: number) {

    let from = servoAngle.slice()

    if (motionSpeed < 1) motionSpeed = 1
    if (motionSpeed > 50) motionSpeed = 50
    msec = msec / motionSpeed * 20;//now 20//default 10 //speedy 30

    // Angle = at + b
    function Angle(t: number, ServoNum: number) {
      let Angle1 = from[ServoNum]
      let Angle2 = angle[ServoNum]
      let a = (Angle2 - Angle1) / msec
      let b = Angle1
      return a * t + b
    }

    let startTime = input.runningTime();
    let loopFlag = true
    while (loopFlag) {
      let nowTime = input.runningTime() - startTime
      for (let val = 0; val < SERVO_NUM_USED; val++) {
        let writeAngle = 0
        if (nowTime >= msec) {
          loopFlag = false
          writeAngle = angle[val]
        } else {
          writeAngle = Angle(input.runningTime() - startTime, val);
        }
        servoWriteInit(val, writeAngle);
      }// 1 loop: v1 10~17ms, v2 6~10ms
    }
  }

  //% block
  //% weight=5 group="Servo" advanced=true
  //% deprecated=true
  export function servoFree() {
    //Power Free!
    writePCA9865(0xFA, 0x00) // ALL_LED_ON_L　全PWMのONのタイミングを0にする
    writePCA9865(0xFB, 0x00) // ALL_LED_ON_H　　　　　　　〃
    writePCA9865(0xFC, 0x00) // ALL_LED_OFF_L　全PWMのOFFのタイミングを0にする
    writePCA9865(0xFD, 0x00) // ALL_LED_OFF_H　　　　　　　〃
  }

  /**
   * Eight servos can be controlled at once.
   * @param angle 8 arrays : -90 ~ 90
   * @param msec 100 ~ 1000
   * @param ls -90 ~ 90
   * @param lt -90 ~ 90
   * @param la -90 ~ 90
   * @param lf -90 ~ 90
   * @param rs -90 ~ 90
   * @param rt -90 ~ 90
   * @param ra -90 ~ 90
   * @param rf -90 ~ 90
   */
  //% block="aet angle -90.0 ~ 90.0|0:Left Shoulder : $ls|1:Left Thigh    : $lt|2:Left Arm      : $la|3:Left Foot     : $lf|4:Right Shoulder: $rs|5:Right Thigh   : $rt|6:Right Arm     : $ra|7:Right Foot    : $rf|msec %msec"
  //% msec.min=100 msec.max=1000 msec.defl=500
  //% ls.min=-30 ls.max=90 ls.defl=0
  //% lt.min=-90 lt.max=90 lt.defl=0
  //% la.min=-90 la.max=0 la.defl=0
  //% lf.min=-90 lf.max=90 lf.defl=0
  //% rs.min=-90 rs.max=30 rs.defl=0
  //% rt.min=-90 rt.max=90 rt.defl=0
  //% ra.min=0 ra.max=90 ra.defl=0
  //% rf.min=-90 rf.max=90 rf.defl=0
  //% weight=4 group="Servo" advanced=true
  export function setAngleToPosition
    (ls: number, lt: number, la: number, lf: number,
      rs: number, rt: number, ra: number, rf: number, msec: number) {
    let angle = [ls, lt, la, lf, rs, rt, ra, rf];
    setAngle(angle, msec);
  }

  // センサー

  /**
   * Receive a IR signal from a remote controller by IR sensor.
   */
  //% blockId=PLEN:bit_Sensor_IR
  //% block="read %num side IR sensor"
  //% weight=9 group="Sensor" advanced=true
  export function checkIRSignal(num: DataPin) {
    let data = []
    let startTime = input.runningTimeMicros()
    let signalLength = 0
    while (plenbit.sensorLR(num) >= 100) {
      signalLength = input.runningTimeMicros() - startTime
      if (signalLength > 20000) return []
    }
    while (true) {
      startTime = input.runningTimeMicros()
      signalLength = 0
      while (plenbit.sensorLR(num) < 100) { }
      while (plenbit.sensorLR(num) >= 100) {
        signalLength = input.runningTimeMicros() - startTime
        if (signalLength > 20000) break
      }
      if (signalLength > 20000) {
        break
      } else if (signalLength > 1500) {
        data.push(1)
      } else {
        data.push(0)
      }
    }
    if (data[0] == 1 && data.length > 1) data.shift()
    return data
  }

  /**
   * Read the value from the AM2320 Temperature & Humidity sensor.
   */
  //% blockId=PLEN:bit_Sensor_SensorTH
  //% block="read %num side TH sensor (serial output : %serial)"
  //% weight=8 group="Sensor" advanced=true
  export function sensorTH(num: DataPin, serial: boolean) {
    UpdateAM2320((num == 1) ? DigitalPin.P2 : DigitalPin.P0, serial)
  }

  /**
   * Get the value of AM2320 Temperature & Humidity sensor.
   */
  //% blockId=PLEN:bit_Sensor_SensorTHvalue
  //% block="%data"
  //% weight=7 group="Sensor" advanced=true
  export function sensorTHvalue(data: THsensorMode) {
    if (data == 1) {
      return humidity
    } else {
      return temperature
    }
  }

  /**
   * Get the value of MH-Z19C CO2 sensor.
   */
  //% blockId=PLEN:bit_Sensor_SensorCO2value
  //% block="read A button side CO2 sensor [ppm]"
  //% weight=6 group="Sensor" advanced=true
  export function sensorCO2value() {
    return ReadMHZ19C()
  }

  //v2専用

  /**
   * [PLEN:bit v2] Change the eye led color with RGB of PLEN:bit.
   */
  //% blockId=PLEN:bit_EyeLED_RGB
  //% block="show color red %r green %g blue %b for Eye LED"
  //% r.min=0 r.max=255 r.defl=0
  //% g.min=0 g.max=255 g.defl=255
  //% b.min=0 b.max=255 b.defl=0
  //% weight=10 group="PLEN:bit v2" advanced=true
  export function setColorRGB(r: number, g: number, b: number) {
    eyeColor[0] = r
    eyeColor[1] = g
    eyeColor[2] = b
    setEyeLED()
  }

  /**
   * [PLEN:bit v2] Change the eye led brightness of PLEN:bit.
   */
  //% blockId=PLEN:bit_EyeLED_Brightness
  //% block="set brightness %b for Eye LED"
  //% b.min=0 b.max=255 b.defl=127
  //% weight=9 group="PLEN:bit v2" advanced=true
  export function setEyeBrightness(b: number) {
    eyeBrightnes = b
    setEyeLED()
  }

  //スペシャルキット

  /**
   * [PLEN:bit SPECIAL KIT] Controll the PLEN:bit by PLEN Connect App.
   */
  //% blockId=PLEN:bit_BLE
  //% block="enable control from smartphone"
  //% weight=10 group="SPECIAL KIT" advanced=true
  export function serialRead() {
    if (initBle == false) bleInit();
    pins.digitalWritePin(DigitalPin.P16, 1);
    while (1) {
      let buf = serial.readString();
      if ((buf[0] != "$") && (buf[0] != "#")) {
        break;
      }
      let bufB = buf[1] + buf[2];
      if (bufB == "PM") {
        bufB = buf[3] + buf[4];
        //basic.showString(bufB);
        motion(parseIntM(bufB));
        break;
      } else if (bufB == "SM") {
        break;
      } else {
        //display.show("b")
        break;
      }
    }
    pins.digitalWritePin(DigitalPin.P16, 0);
  }

  //初期位置調整
  //% block
  //% blockId=PLEN:bit_ServoAdjust_Save
  //% block="save servo position"
  //% weight=9 group="Servo Adjust" advanced=true
  //% deprecated=true
  export function savePosition() {
    WriteEEPROM(0, 1) // サーボ初期位置調整フラグ
    for (let i = 0; i < 8; i++) {
      let servoInitValue = servoInitArray[i]
      WriteEEPROM(i * 2 + 2, (servoInitValue >> 8) & 0xFF)
      WriteEEPROM(i * 2 + 3, servoInitValue & 0xFF)
    }
  }

  //% block
  //% blockId=PLEN:bit_ServoAdjust_Reset
  //% block="reset servo position"
  //% weight=8 group="Servo Adjust" advanced=true
  //% deprecated=true
  export function resetPosition() {
    WriteEEPROM(0, 0) // サーボ初期位置調整フラグ
    for (let i = 0; i < servoInitArray.length; i++) servoInitArray[i] = servoInitArraySave[i]
  }
}