HongMingLin_robot_yes_sir/playGround/xyzPilot/RXTX.pde
void processingJ(JSONObject J) {
  JSONArray inJArr=J.getJSONArray(JSONKEYWORD.Robots);
  if (inJArr!=null&&inJArr.size()==12) {
    String LINEStr="";
    for (int i=0; i<12; i++) {
      int ID=inJArr.getJSONObject(i).getInt(JSONKEYWORD.ID)-1;
      PVector p=new PVector(inJArr.getJSONObject(i).getFloat("X"), 
        inJArr.getJSONObject(i).getFloat("Y"), 
        inJArr.getJSONObject(i).getFloat("Z"));
      Rs[ID].XYZ=p.normalize().mult(50);
      PVector p2=new PVector(inJArr.getJSONObject(i).getFloat("A"), 
        inJArr.getJSONObject(i).getFloat("B"), 
        inJArr.getJSONObject(i).getFloat("C"));
      Rs[ID].RYP=p2.normalize().mult(10);
    }
    println("Rs[0].XYZ="+Rs[0].XYZ+" Rs[0].RYP="+Rs[0].RYP);
  } else {
    println("[X]J12Arr Error");
  }
}
void receive(byte[] bb, String ip, int port) {
  String rxStr=new String(bb);
  switch(port) {
  case 6666:
  JSONObject j=null;
    try {
      j=parseJSONObject(rxStr);
      
    }catch(Exception e) {
      e.printStackTrace();
      //println("[X]parseJ Error"+rxStr);
    }
    if (j!=null) {
        rXJSONObj=j;
        processingJ(rXJSONObj);
        //println(rXJSONObj);
      }
    break;
  case 1313:
    break;
  }
}


/*

{"TIMESTAMP":"346144","Robots":[{"A":"-0.168","B":"-0.262","C":"30.846","A1":"31.095","A2":"-24.38","A3":"-29.883","A4":"26.574","A5":"89.322","A6":"-17.729","X":"-510.933","Y":"942.416","Z":"288.521","ID":"1","CMD":"STOP"},{"A":"-0.144","B":"-0.283","C":"30.85","A1":"31.044","A2":"-24.659","A3":"-29.956","A4":"26.55","A5":"89.633","A6":"-17.521","X":"-510.401","Y":"943.318","Z":"283.771","ID":"2","CMD":"STOP"},{"A":"-0.138","B":"-0.307","C":"30.843","A1":"31.036","A2":"-24.662","A3":"-29.921","A4":"26.568","A5":"89.604","A6":"-17.525","X":"-510.4","Y":"943.654","Z":"284.024","ID":"3","CMD":"STOP"},{"A":"-0.158","B":"-0.286","C":"30.842","A1":"31.057","A2":"-24.639","A3":"-29.976","A4":"26.566","A5":"89.631","A6":"-17.543","X":"-510.467","Y":"942.993","Z":"283.872","ID":"4","CMD":"STOP"},{"A":"-0.085","B":"-0.393","C":"30.845","A1":"31.063","A2":"-24.657","A3":"-29.962","A4":"26.671","A5":"89.67","A6":"-17.471","X":"-510.442","Y":"943.106","Z":"283.733","ID":"5","CMD":"STOP"},{"A":"-0.153","B":"-0.27","C":"30.848","A1":"31.069","A2":"-24.615","A3":"-30.011","A4":"26.558","A5":"89.644","A6":"-17.539","X":"-510.47","Y":"942.559","Z":"283.913","ID":"6","CMD":"STOP"},{"A":"-0.163","B":"-0.261","C":"30.846","A1":"31.057","A2":"-24.632","A3":"-29.989","A4":"26.54","A5":"89.632","A6":"-17.545","X":"-510.448","Y":"942.876","Z":"283.867","ID":"7","CMD":"STOP"},{"A":"-0.141","B":"-0.29","C":"30.849","A1":"31.064","A2":"-24.629","A3":"-30.005","A4":"26.573","A5":"89.657","A6":"-17.52","X":"-510.424","Y":"942.689","Z":"283.757","ID":"8","CMD":"STOP"},{"A":"-0.142","B":"-0.282","C":"30.849","A1":"31.055","A2":"-24.644","A3":"-29.984","A4":"26.558","A5":"89.648","A6":"-17.519","X":"-510.421","Y":"942.967","Z":"283.725","ID":"9","CMD":"STOP"},{"A":"-0.135","B":"-0.322","C":"30.844","A1":"31.058","A2":"-24.647","A3":"-29.968","A4":"26.6","A5":"89.644","A6":"-17.52","X":"-510.463","Y":"943.063","Z":"283.82","ID":"10","CMD":"STOP"},{"A":"-0.147","B":"-0.294","C":"30.847","A1":"31.073","A2":"-24.63","A3":"-30.017","A4":"26.584","A5":"89.67","A6":"-17.525","X":"-510.483","Y":"942.532","Z":"283.633","ID":"11","CMD":"STOP"},{"A":"-0.128","B":"-0.313","C":"30.85","A1":"31.07","A2":"-24.631","A3":"-30.008","A4":"26.6","A5":"89.671","A6":"-17.509","X":"-510.458","Y":"942.616","Z":"283.704","ID":"12","CMD":"STOP"}]}
*/
