RPLIDAR with obniz

このエントリーをはてなブックマークに追加

RPLIDAR is a low cost Laser Scanner.
It works like a Radar.

One is connected to an obniz.
And draw results on HTML.

Program

<!-- HTML Example -->
<!DOCTYPE html>
<html>
<head>
  <meta charset="utf-8">
  <meta name="viewport" content="width=device-width, initial-scale=1">

  <link rel="stylesheet" href="https://maxcdn.bootstrapcdn.com/bootstrap/4.0.0/css/bootstrap.min.css" integrity="sha384-Gn5384xqQ1aoWXA+058RXPxPg6fy4IWvTNh0E263XmFcJlSAwiGgFAW/dAiS6JXm" crossorigin="anonymous">
  <script  src="https://code.jquery.com/jquery-3.2.1.min.js" integrity="sha256-hwg4gsxgFZhOsEEamdOYGBf13FyQuiTwlAQgxVSNgt4="  crossorigin="anonymous"></script>
  <script src="https://cdnjs.cloudflare.com/ajax/libs/popper.js/1.12.3/umd/popper.min.js" integrity="sha384-vFJXuSJphROIrBnz7yo7oB41mKfc8JzQZiCq4NCceLEaO4IHwicKwpJf9c9IpFgh" crossorigin="anonymous"></script>
  <script src="https://maxcdn.bootstrapcdn.com/bootstrap/4.0.0/js/bootstrap.min.js" integrity="sha384-JZR6Spejh4U02d8jOt6vLEHfe/JQGiRRSQQxSfFWpi1MquVdAyjUar5+76PVCmYl" crossorigin="anonymous"></script>

  <style>
  textarea {
    width: 100%;
  }
  </style>

  <script src="https://unpkg.com/obniz@2.0.2/obniz.js"></script>
</head>

<body>
<div id="obniz-debug"></div>
<br>
<div class="text-center">
  <h1> RPLIDAR </h1>
</div>

<div id="info"> </div>
<div id="dot" class="btn btn-primary">Dot Mode</div>
<div id="line" class="btn btn-primary">Line Mode</div>
<canvas id="canvas" width="700" height="700"></canvas>

<script>

/* This will be over written on obniz.io webapp page */
var obniz = new Obniz("OBNIZ_ID_HERE");

obniz.onconnect = async () => {
  obniz.reset();

  obniz.io0.output(0);
  obniz.io4.output(1);
  obniz.io5.output(1);
  obniz.io9.output(0);
  obniz.io10.output(1);
  obniz.io11.output(1);
  
  var mode = "dot"
  $("#dot").click(function(){
    mode ="dot";
  })
  $("#line").click(function(){
    mode ="line";
  })

  const uart = obniz.getFreeUart();
  uart.start({tx: 1, rx: 2, baud:115200, drive:"5v"});
  
  const info = await getDeviceInfo(uart);
  $("#info").text("firmwareVersion:" + info.firmwareVersion)

  const health = await getHealth(uart)
  if (health.status != 0) {
    throw new Error("invalid status " + health.status);
  }

  await startScan(uart);

  var canvas = $("#canvas")[0];
  var ctx = ctx = canvas.getContext('2d', { alpha: false });
  ctx.fillStyle = 'red';
  ctx.strokeStyle = '#A00';

  obniz.repeat(async function(){
    var datas = await getNextData(uart);
    ctx.clearRect(0, 0, canvas.width, canvas.height);
    for (var i=0; i<datas.length; i++) {
      // console.log(""+datas[i].angle+" - "+datas[i].distance + " Q:"+datas[i].quality)
      var x = canvas.width/2;
      var y = canvas.height/2;
      x += datas[i].distance * Math.cos(datas[i].angle /360.0 * 2 * Math.PI) / 10
      y += datas[i].distance * Math.sin(datas[i].angle /360.0 * 2 * Math.PI) / 10
	  if (mode == "line") {
        ctx.beginPath();
        ctx.moveTo(canvas.width/2, canvas.height/2);
        ctx.lineTo(x, y);
        ctx.stroke();
      } else {
        ctx.fillRect(x, y, 1, 1);
      }
    }
    await obniz.wait(1);
  }, 1);
}

async function reset(uart) {
  const RPLIDAR_CMD_RESET = 0x40
  sendCommand(uart, RPLIDAR_CMD_RESET); // no payload
  var payload = await getFrame(uart);
}

let _parseOffset = 0;
let _savedDatas = [];
async function getNextData(uart) {
  let datas = [];
  let r = _savedDatas;
  _savedDatas = [];
  while(true) {
    r = r.concat( uart.readBytes());
    for (var i=0; i<r.length; i++) {
      switch(_parseOffset) {
        case 0:
          datas.push({});
          datas[datas.length-1].sync_quality = r[i];
          break;
        case 1:
          datas[datas.length-1].angle_q6_checkbit = r[i];
          break;
        case 2:
          datas[datas.length-1].angle_q6_checkbit |= r[i] << 8;
          break;
        case 3:
          datas[datas.length-1].distance_q2 = r[i];
          break;
        case 4:
          datas[datas.length-1].distance_q2 |= r[i] << 8;

          const last = datas[datas.length-1];

          last.isStart = (last.sync_quality & 0x01 != 0) ? true : false;
          last.quality = last.quality >> 2;
          last.angle = (last.angle_q6_checkbit >> 1) / 64;
          last.check = last.angle_q6_checkbit & 0x01;
          last.distance = last.distance_q2 /4;

          break;
      }
      if (++_parseOffset > 4) {
        _parseOffset = 0;
        if (datas[datas.length-1].isStart) {
          if (datas.length != 1) {
            r.splice(0, i + 1);
            _savedDatas = r;
            return datas;
          }
        }
      }
    }
    await obniz.wait(1);
  }
}

async function startScan(uart) {

  const RPLIDAR_CMD_SCAN  = 0x20;
  const RPLIDAR_CMD_FORCE_SCAN = 0x21;
  sendCommand(uart, RPLIDAR_CMD_SCAN); // no payload
  var payload = await getFrame(uart);
  // type should be 129
}

async function startExpressScan(uart) {
  const RPLIDAR_CMD_EXPRESS_SCAN = 0x82
  const fixedAngle = false;
  let workingMode = fixedAngle ? 1 : 0;
  var payload = [];
  var index = 0;
  payload.push(workingMode);
  payload.push(0);
  payload.push(0);
  payload.push(0);
  payload.push(0);
  sendCommand(uart, RPLIDAR_CMD_EXPRESS_SCAN, payload); // no payload
  var payload = await getFrame(uart);
  // type should be RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED 0x82(130)
}

async function getHealth(uart) {
  const RPLIDAR_CMD_GET_DEVICE_HEALTH = 0x52

  sendCommand(uart, RPLIDAR_CMD_GET_DEVICE_HEALTH); // no payload

  var payload = await getFrame(uart);

  var health = {};
  health.status     = payload.splice(0, 1);
  health.error_code = parseInt(payload.splice(0, 1));
  health.error_code |= payload.splice(0, 1) << 8;
  return health;
}

async function getExpressSupporeted(uart) {
  const RPLIDAR_CMD_GET_SAMPLERATE = 0x59;
  sendCommand(uart, RPLIDAR_CMD_GET_SAMPLERATE); // no payload
  var payload = await getFrame(uart);
}

async function getDeviceInfo(uart) {
  const RPLIDAR_CMD_GET_DEVICE_INFO = 0x50

  sendCommand(uart, RPLIDAR_CMD_GET_DEVICE_INFO); // no payload

  var payload = await getFrame(uart);
  
  var deviceInfo = {};
  deviceInfo.model = payload.splice(0, 1);
  deviceInfo.firmwareVersion = parseInt(payload.splice(0, 1));
  deviceInfo.firmwareVersion |= payload.splice(0, 1) << 8;
  deviceInfo.hardware_version = payload.splice(0, 1);
  deviceInfo.serialNumber = payload;
  return deviceInfo;
}

function sendCommand(uart, command, payload) {
  const SYNC = 0xA5;
  const RPLIDAR_CMD_SYNC_BYTE = 0xA5
  
  var data = [SYNC, command]
  if (payload) {
    var checksum = 0;
    checksum ^= RPLIDAR_CMD_SYNC_BYTE;
    checksum ^= command;
    checksum ^= (payload.length & 0xFF);
    for (let pos = 0; pos < payload.length; ++pos) {
        checksum ^= payload[pos];
    }
    data.push(payload.length);
    data = data.concat(payload);
    data.push(checksum);
  }

  uart.send(data);
}

  // const RPLIDAR_ANS_TYPE_DEVINFO  = 0x4;
  // const RPLIDAR_ANS_TYPE_DEVHEALTH = 0x6;

async function getFrame(uart) {
  const RPLIDAR_ANS_SYNC_BYTE1 = 0xA5;
  const RPLIDAR_ANS_SYNC_BYTE2 = 0x5A;
  let recv = [];
  let waiting = true

  // parse header
  while(waiting) {
    const r = uart.readBytes();
    recv = recv.concat(r);
    for (var i=0; i<recv.length-1; i++) {
      if (recv[i] == RPLIDAR_ANS_SYNC_BYTE1 && recv[i+1] == RPLIDAR_ANS_SYNC_BYTE2) {
        // remain is 7?
        if(recv.length - (i+2) >= 5) {
          recv.splice(0, i+2);
          waiting = false;
          break;
        }
      }
    }
    await obniz.wait(10);
  }
  var subType = parseInt(recv.splice(0, 1));
  subType |=  recv.splice(0, 1) << (8*1);
  subType |=  recv.splice(0, 1) << (8*2);
  subType |=  recv.splice(0, 1) << (8*3);
  var type =  recv.splice(0, 1);

  const RPLIDAR_ANS_HEADER_SIZE_MASK = 0x3FFFFFFF;

  var size = subType & RPLIDAR_ANS_HEADER_SIZE_MASK;

  console.log(`type${type} size${size}`);

  // parse payload
  while(true) {
    const r = uart.readBytes();
    recv = recv.concat(r);
    if (recv.length >= size) {
      break;
    }
    await obniz.wait(10);
  }
  return recv.splice(0, size);
}
    
</script>
</body>
</html>

Run Now

The html will be opened to run a program.

You will Get in Few Days

Circuit for Starter “obniz Board” is available on Amazon and other online stores.
You can get it at below

Our products and resellers

Forum

Visit our developer’s forum to discuss and discover technologies.

Forum

Contact

Feel free to contact out support and technical team.

Contact us