toio cubeを軌道にそって動かす

/**
 * Copyright (c) 2019-present, Sony Interactive Entertainment Inc.
 *
 * This source code is licensed under the MIT license found in the
 * LICENSE file in the root directory of this source tree.
 */

```
//const { NearScanner } = require('@toio/scanner')
const { NearestScanner } = require('../../0030/toio.js/node_modules/@toio/scanner')


function calc(jerryX, jerryY, tomX, tomY, tomAngle, speed) {

  const diffX = jerryX - tomX
  const diffY = jerryY - tomY
  const distance = Math.sqrt(diffX * diffX + diffY * diffY)

  let relAngle = (Math.atan2(diffY, diffX) * 180) / Math.PI - tomAngle
  relAngle = relAngle % 360
  if (relAngle < -180) {
    relAngle += 360
  } else if (relAngle > 180) {
    relAngle -= 360
  }

  const ratio = 1 - Math.abs(relAngle) / 90
  if (relAngle > 0) {
    return [speed, speed * ratio]
  } else {
    return [speed * ratio, speed]
  }
}

//サイクロイド
//x=a(θ−sinθ)
//y=a(1−cosθ)
function makeOrbit5()
{
  var aData  = []
  var a = 20
  for( var i = 0; i <= 360*4; i+=5)
  {
    var theta = i;
    var radian = (theta) * Math.PI / 180.0; // deg2rad
    
    var t_x = a*(radian -  Math.sin(radian)) * 0.5
    var t_y = a*(1 -  Math.cos(radian))
    
    aData.push([t_x, t_y])
  }
  return aData
} 

// 対数螺旋
//https://mathtrain.jp/sevencurve
/*
x=a*e^(bθ)*cosθ
y=a*e^(bθ)*sinθ
*/
function makeOrbit4()
{
  var aData  = []
  var a = 1
  var b = 0.2
  for( var i = 0; i <= 360*2; i+=10)
  {
    var theta = i;
    var radian = (theta) * Math.PI / 180.0; // deg2rad
    var t_x = a*Math.E**(b*radian) * Math.cos(radian)
    var t_y = a*Math.E**(b*radian) * Math.sin(radian)
    aData.push([t_x, t_y])
  }
  return aData
} 

//カージオイド曲線（心臓形）
function makeOrbit3()
{
  var aData  = []
  var a = 70
  for( var i = 0; i <= 360; i+=2)
  {
    var theta = i;
    var radian = (theta) * Math.PI / 180.0; // deg2rad
    var t_x = a*( 1 + Math.cos(radian)) * Math.cos(radian)
    var t_y = a*( 1 + Math.cos(radian)) * Math.sin(radian)
    //x=a(1+cosθ)cosθ
    // y=a(1+cosθ)sinθ
    aData.push([t_x, t_y])
  }
  return aData
} 

function makeOrbit() 
{
  var aData  = []

  for( var i = 0; i <= 360; i+=2)
  {
    var radian = (i) * Math.PI / 180.0; // deg2rad
    var t_y = Math.sin(radian) * 80
    var t_x = i*0.7
    aData.push([t_x, t_y])
  }

  return aData
}

//https://developer.mozilla.org/ja/docs/Web/JavaScript/Reference/Global_Objects/Array
function makeOrbit2(aData) 
{
  var aData2 = [
    [100,100],
    [200,100],
    [200,200],
    [100,200],
  ];
  aData = aData2.concat()
  return aData2
}
function tset1() 
{
  var aDataOut = makeOrbit2(aData)
  console.log("aDataO:", aDataOut)

  var data_index = 0
  let target_y = aDataOut[data_index][1]
  let target_x = aDataOut[data_index][0]
  
}

function test2() 
{
  var aData2 = [
    [1,2,3,4],
    [5,6,7,8],
    [9,10,11,12],
    [13,14,15,16],
  ];
  return aData2;
}



async function main() 
{ 
  var matT=[] 
  matT = test2()

  console.log(matT[2][1] ) //10
  console.log(matT[3][0] ) //13
  console.log(matT[0][3] ) // 4

  //const cubes = await new NearScanner(1).start()
  //const tom = await cubes[0].connect()
  tom = await new NearestScanner().start()
  await tom.connect()
  let tomX = 0
  let tomY = 0
  let tomAngle = 0
  //tom.turnOnLight({ durationMs: 0, red: 0, green: 255, blue: 255 })


  var t_x = -1
  var t_y = -1
  let enable  = false
  let first  = 1
// 位置と角度の更新のリスナー登録
  tom.on('id:position-id', data => {
    tomX = data.x
    tomY = data.y
    tomAngle = data.angle

    if(first){
      first = 0
      enable = true
      t_x = tomX
      t_y = tomY
    }
  })


  var target_x = 0
  var target_y = 0

  
  let interval = 50
  let data_index = 0
  var speed = 20

  var aData = makeOrbit5()
  var offset_y = 200
  var offset_x = 100
  var old_data_index = -1

  setInterval(() => {
    if( enable == true) 
    {
      target_x = aData[data_index][0]+ offset_x
      target_y = 455-(aData[data_index][1]+ offset_y)

      const diffX = target_x - tomX
      const diffY = target_y - tomY
      const distance = Math.sqrt(diffX * diffX + diffY * diffY)
      if ( distance< 20 ){
        data_index++
        data_index = data_index  % aData.length
      }

      if( old_data_index != data_index )
      {
        var lr = [0,0]
        lr = calc(target_x,target_y, tomX, tomY, tomAngle, speed)
        tom.move(lr[0],lr[1], interval*1.5)
        
        //出力
        target_x = target_x.toFixed(2)
        target_y = target_y.toFixed(2)
        lr[0] = lr[0].toFixed(2)
        lr[1] = lr[1].toFixed(2)
        console.log("target:", target_x, target_y, "tom:", tomX, tomY, tomAngle, "calc()", lr);
      }
    }

  }, interval)
}

main()



/*
      var t_val = (tomX / 400.0)*180 // 0から180のSINカーブを描く
      var radian = (t_val) * Math.PI / 180.0; // deg2rad
      var r = Math.sin(radian) * 200
      target_y = t_y + (r)
      target_x = t_x + t_val

      const diffX = target_x - tomX
      const diffY = target_y - tomY
      const distance = Math.sqrt(diffX * diffX + diffY * diffY)
      var lr = [0,0]
      lr = calc(target_x,target_y, tomX, tomY, tomAngle)
      
      tom.move(lr[0],lr[1], interval*2)
      
      target_x = target_x.toFixed(2)
      target_y = target_y.toFixed(2)
      lr[0] = lr[0].toFixed(2)
      lr[1] = lr[1].toFixed(2)
      console.log("target:", target_x, target_y, "tom:", tomX, tomY, tomAngle, "calc()", lr);
     
*/

 /* setInterval(() => {
    if( enable == true) {
      var radian = (degree) * Math.PI / 180.0; // deg2rad

      degree+=addval
      if( degree>180) addval *= -1
      if( degree<0) addval *= -1

      target_x = t_x + degree
      var r = Math.sin(radian) * 50
      target_y = t_y + r
      var lr = calc(target_x,target_y, tomX, tomY, tomAngle)
      
      //tom.move(...lr, interval)

      console.error("tom ", tomX, tomY, tomAngle);
      //console.error("calc():", lr);
      //console.error("target:", target_x, target_y ,degree)
      //console.error("hoge:", tomX, tomY,tomAngle );
    }

  }, interval)*/

```