ev3_ctrl/index.js

const dgram = require('dgram')
const server = dgram.createSocket('udp4')
const net = require('net')
const WebSocket = require('ws')

let lego = {
  sn: null,
  port: null,
  name: null,
  proto: null,
  addr: null,
  connected: false
}
let legoClient = null

function padleft(v, n) {
  if (typeof v !== 'string') v = v.toString()
  if (v.length < n) {
    return padleft(`0${v}`, n)
  }
  return v
}

function convertLength(num, blng = 1) {
  let str = num.toString(16)
  let tlng = blng * 2

  switch (str.length) {
    case 1:
      if (tlng > 2) return `0${str}00`
      else return `0${str}`
    case 2:
      if (tlng > 2) return `${str}00`
      else return `${str}`
    case 3:
      return `${str.substr(-2)}0${str.substr(0, 1)}`
    case 4:
      return `${str.substr(-2)}${str.substr(0, 2)}`
  }

  return padleft('0', tlng)
}

function getMsgByte(name, val) {
  let name_buf = Buffer.from(name)
  name_buf = Buffer.concat([name_buf, Buffer.from([0x00])], (name_buf.length + 1))
  let name_lng = name_buf.length

  let val_buf = null
  if (typeof val === 'number') {
    val_buf = Buffer.allocUnsafe(4)
    val_buf.writeFloatLE(val)
  } else {
    if (typeof val !== 'string') val.toString()
    val_buf = Buffer.from(val)
    val_buf = Buffer.concat([val_buf, Buffer.from([0x00])], (val_buf.length + 1))
  }
  if (val_buf === null) return null
  let val_lng = val_buf.length

  return `${convertLength(name_lng, 1)}${name_buf.toString('hex')}${convertLength(val_lng, 2)}${val_buf.toString('hex')}`
}

server.on('error', err => {
  console.log('error ', err)
  server.close()
})
server.on('message', (msg, rinfo) => {
  console.log('message', msg.toString())
  console.log('rinfo', rinfo)
  let str = msg.toString()
  let regex = /Serial\-Number:\s(\w+)\r\nPort:\s(\d+)\r\nName:\s(\w+)\r\nProtocol:\s(\w+)/
  let match = str.match(regex)
  if (match.length > 4) {
    lego.sn = match[1]
    lego.port = match[2]
    lego.name = match[3]
    lego.proto = match[4]
  }
  lego.addr = rinfo.address
  console.log('lego info ', lego)

  let client = dgram.createSocket('udp4')
  client.send(Buffer.from([0x01]), lego.port, lego.addr, (err) => {
    if (err) {
      console.log('send udp error')
      client.close()
      return
    }

    try {
      server.close()
    } catch (err) { }

    connectEV3()

    try {
      client.close()
    } catch (err) { }
  })
})
server.bind(3015)

async function connectEV3() {
  legoClient = net.createConnection({
    port: lego.port,
    host: lego.addr
  }, () => {
    console.log('connected')
    console.log('send unlock')
    legoClient.write(`GET /target?sn=${lego.sn} VMTP1.0\nProtocol: ${lego.proto}`)
  })

  legoClient.on('data', data => {
    console.log('get ev3 data', data.toString())
    let str = data.toString()
    if (/Accept/i.test(str)) {
      lego.connected = true
    }
  })

  legoClient.on('end', () => {
    console.log('connect end')
    legoClient = null
    lego.connected = false
  })
}

const ws = new WebSocket.Server({
  port: 9999
})

ws.on('connection', client => {
  client.on('message', msg => {
    console.log(msg)
    let s = ''
    let wbuf = null
    let lng = 0
    switch (msg) {
      case 'check':
        client.send(`ev3 connect: ${lego.connected}`)
        break
      case 'forward':
        if (!lego.connected) break
        s = `0000819e${getMsgByte('a', 360 )}`
        s = `${convertLength(s.length / 2, 2)}${s}`
        wbuf = Buffer.from(s, 'hex')
        console.log('write byte ', s)
        legoClient.write(wbuf)
        break
      case 'back':
        if (!lego.connected) break
        s = `0000819e${getMsgByte('c', 360)}`
        s = `${convertLength(s.length / 2, 2)}${s}`
        wbuf = Buffer.from(s, 'hex')
        console.log('write byte ', s)
        legoClient.write(wbuf)
        break
      case 'click':
        if (!lego.connected) break
        s = `0000819e${getMsgByte('b', 90)}`
        s = `${convertLength(s.length / 2, 2)}${s}`
        wbuf = Buffer.from(s, 'hex')
        console.log('write byte ', s)
        legoClient.write(wbuf)
        break
      case 'right':
        if (!lego.connected) break
        s = `0000819e${getMsgByte('d', 180)}`
        s = `${convertLength(s.length / 2, 2)}${s}`
        wbuf = Buffer.from(s, 'hex')
        console.log('write byte ', s)
        legoClient.write(wbuf)
        break
      case 'left':
        if (!lego.connected) break
        s = `0000819e${getMsgByte('e', 180)}`
        s = `${convertLength(s.length / 2, 2)}${s}`
        wbuf = Buffer.from(s, 'hex')
        console.log('write byte ', s)
        legoClient.write(wbuf)
        break
      case 'restart':
        process.exit(0)
        break
    }
  })
})