Meta(Oculus)Quest のコントローラーの動きでロボットの手の動きを制御:ソースコード:SerialSupport.h, SerialSupport.cpp

#pragma once
//SerialSupport.h
//copyright 2022 https://robot-creation-diary.com/
#define START_FLAG (0xFF)
#define END_FLAG (0xFE)
class CSerialSupport{
  public:
    CSerialSupport();
    ~CSerialSupport();
    void SetSize(unsigned  int dsize); // データサイズ変更：データは初期化される
    void StretchSize(unsigned  int dsize); // データサイズ変更：データは維持される。新たなデータサイズが小さくてもメモリは開放しない。大きいと確保後元データをコピーしたのち開放。
    unsigned char GetIndexData(unsigned  int i); // 指定番号のデータを取得
    unsigned char* GetData(); // データの生データを取得
    void ResetDataPosition(); // データ位置を0にする。
    void SetDataPosition(unsigned int pos); // データ位置の設定
    int GetDataPosition(); // データ位置の取得
    bool AddData(unsigned char d); // データ位置にデータを追加。データ位置をインクリメントする

    int GetLatestError(); // エラーコード取得

    bool CheckPositionisChecksum(int position); // 指定の位置がチェックサムかどうか
    bool CheckPositionisEnd(int position); // 指定の位置が、データの終了位置かどうか
  private:
    unsigned char* mData; //データ
    unsigned int mSize; //データ領域のの大きさ
    int mDataPosition; //データの位置

    int mError;
    
///////////////////////    
public:
    bool CheckDataError();//データにエラーがないか確認
private:
    bool __CheckChecksum();//チェックサムを確認
    
  };

//SerialSupport.cpp
//copyright 2022 https://robot-creation-diary.com/

 #include "SerialSupport.h"
 #include <string.h>
 //error 0 :エラーなし
 //error 1 :GetIndexData でデータ範囲を超えている
 //error 2 :AddData 確保済みデータ範囲を超えた
 //error 3 :SetDataPosition 確保済みデータ範囲の外にデータ位置を設定しようとした。
 //error 4 :CheckError 読み込んだデータが小さすぎる。
 //error 5 :CheckError データサイズが読み込んだデータと異なっている。
 //error 6 :CheckError データ開始フラグがおかしい
 //error 7 :CheckError データ終了フラグがおかしい
 //error 8 :__CheckChecksum 読み込んだデータが小さすぎる。//本来呼ばれないはず
 //error 9 :__CheckChecksum チェックサムがあっていない
 
 CSerialSupport::CSerialSupport():
    mData(0),
    mSize(0),
    mDataPosition(0),
    mError(0){
      
 }
 CSerialSupport::~CSerialSupport(){
  if (mData){
    delete [] mData;
  }
 }
      
void CSerialSupport::SetSize(unsigned int dsize){
      
  if (mSize != dsize){
    if (mData){
        delete [] mData;
    }
    mData = new unsigned char[dsize];
    memset(mData, 0, mSize);
  }else{
    memset(mData, 0, mSize);
  }
      
  mSize = dsize;
  mDataPosition = -1;
            
}

//データサイズ変更：データは維持される。
void CSerialSupport::StretchSize(unsigned  int dsize){
  unsigned char *newData;
  if ( mSize < dsize ){
    newData = new unsigned char[dsize];
    memset(newData,0,mSize);
    memcpy(newData,mData,mSize);
    delete [] mData;
    mData = newData;
  }
  mSize = dsize;
  mDataPosition = -1;
}

//指定番号のデータを取得する
unsigned char CSerialSupport::GetIndexData(unsigned  int i){
  if (mDataPosition < i){
    mError = 1;
    return 0;
  }
  mError = 0;
  return mData[i];
}

//データの生データを取得する
unsigned char* CSerialSupport::GetData(){
  return mData;
}

//データ追加場所を初期化する。
void CSerialSupport::ResetDataPosition(){
  mDataPosition = -1; 
}

//データ追加場所を指定する
void CSerialSupport::SetDataPosition(unsigned int pos){
  if ((0 < pos) && (pos < mSize)){
    mDataPosition=pos;
    mError = 0;
  }else{
    mError = 3;
  }
}
int  CSerialSupport::GetDataPosition(){
  return mDataPosition;
}
//データを追加する。
bool CSerialSupport::AddData(unsigned char d){
  mDataPosition++;
 if (mDataPosition < mSize){
    mData[mDataPosition]  = d;
    mError = 0;
    return true;
  }else{
    mError = 2;
    mDataPosition = mSize - 1;
    return false;
  }
}
///////////////ここから先は特化コード　汎用性が低い
bool CSerialSupport::CheckDataError(){
   //一番目と最後のチェックデータが違うときエラーチェックサムが違うときエラー  
   if (mDataPosition < 4  ){
      mError = 4;
      return false;
   }
   //データサイズが読み込んだデータと異なっている。
   if (mDataPosition != mData[1] - 1){
      mError = 5;
      return false;
   }
   if (START_FLAG    != mData[0] ){
      mError = 6;
      return false;
   }
   if (END_FLAG      != mData[mDataPosition] ){
      mError = 7;
      return false;
   }
   if (__CheckChecksum()){
    mError = 0;
    return true;
   }
   return false;
}
int CSerialSupport::GetLatestError(){
  return mError;
}

bool CSerialSupport::__CheckChecksum(){//チェックサムを確認
  int i;
  unsigned char cdata;
  cdata = 0;
  if(mDataPosition < 4){
    //サイズがおかしい。
    mError = 8;
    return false;
  }
  for  (i = 2 ; i <= mDataPosition - 2 ; i++){
    cdata = cdata ^ mData[i];
  }
  if (mData[mDataPosition-1] == cdata){
    mError = 0;
    return true;
  }
  //チェックサムが合わなかった
  mError = 9;

  return false;
}
bool CSerialSupport::CheckPositionisChecksum(int position){
  if(2 < position){
    if (mData[1] - 2 == position) {
        return true;
    }
  }
  return false;
}
bool CSerialSupport::CheckPositionisEnd(int position){
  if(2 < position){
    if (mData[1] - 1  == position) {
        return true;
    }
  }
  return false;
}