リモコン移動作業ロボット：ソースコード：ServoSupport.cpp

//ServoSupport.cpp
//copyright 2022 https://robot-creation-diary.com/
//20230320角度周りの整数値を100倍にした.角度を小数点第2位まで受け取れるようにした。
#include "ServoSupport.h"
#include <Wire.h>                     // ライブラリのインクルード
#include <Adafruit_PWMServoDriver.h>

CServoSupport::CServoSupport():mError(0){
  int i;
  for (i = 0 ; i < SERBO_NUMBER ;i++ ){
     mServoAngles[i] = WEAKNESS_ANGLE;//脱力状態にする。
     mPreServoAngles[i] = WEAKNESS_ANGLE;
  }
  mServoAngles[18] = 0;//脱力状態にする。
  mPreServoAngles[18] = 0;
  mServoAngles[19] = 0;//脱力状態にする。
  mPreServoAngles[19] = 0;

  //初期化
  //Adafruit_PWMServoDriver pwm1 = Adafruit_PWMServoDriver(0x40);
  //Adafruit_PWMServoDriver pwm2 = Adafruit_PWMServoDriver(0x41);
  mpwm1 = Adafruit_PWMServoDriver(0x40);
  mpwm2 = Adafruit_PWMServoDriver(0x41);
  CWheelSupport ws;//wheel pin の有効化
//  pwm.begin();
//  pwm.begin();
//  pwm.setPWMFreq(PWM_FREQ);
//  mpwm1.begin(); // これをやったらsetPWMをやらないと固まる
//  mpwm1.setPWMFreq(PWM_FREQ);
//  mpwm2.begin(); // これをやったらsetPWMをやらないと固まる
//  mpwm2.setPWMFreq(PWM_FREQ);
//  mpwm1.sleep();
//  mpwm2.sleep();
//  mpwm1.setPWM(6, 0, 0);                 // サーボを動作させる
//  mpwm2.setPWM(7, 0, 0);                 // サーボを動作させる
//  SetAllWeakness();

}
CServoSupport::~CServoSupport(){
}
void CServoSupport::Continue(){
  Adafruit_PWMServoDriver pwm1 = Adafruit_PWMServoDriver(0x40);
  Adafruit_PWMServoDriver pwm2 = Adafruit_PWMServoDriver(0x41);
  pwm1.begin(); // これをやったらsetPWMをやらないと固まる
  pwm1.setPWMFreq(PWM_FREQ);
  pwm2.begin(); // これをやったらsetPWMをやらないと固まる
  pwm2.setPWMFreq(PWM_FREQ);

}
int CServoSupport::GetRecievedDataSize(){
  return mRecievedData.GetDataPosition();
}

void CServoSupport::SetSerialDataSize(int dsize){
  mRecievedData.SetSize(dsize);
  
}
unsigned char CServoSupport::GetIndexRecievedData(int i ){
  return mRecievedData.GetIndexData(i );
}

int  CServoSupport::SetRecievedData(unsigned char rdata){
  int i;
  if(START_FLAG == rdata){
    //チェックサムの場合は通常のデータ追加
    if (false == mRecievedData.CheckPositionisChecksum(mRecievedData.GetDataPosition() + 1)){
      mRecievedData.ResetDataPosition();
      mRecievedData.AddData(rdata);
      return 1; 
    }else{
      mRecievedData.AddData(rdata);
      return 3; 
    }
  }else if(END_FLAG == rdata){
    mRecievedData.AddData(rdata);
    //チェックサムの場合は通常のデータ追加
    if (mRecievedData.CheckPositionisChecksum(mRecievedData.GetDataPosition())){
      return 3;
    }else{
      //データにエラーがあるかどうか確認
      if ( mRecievedData.CheckDataError()){
      //エラーがないとき
        for (i = 0 ; i < DATA_NUMBER ;i++ ){
          mServoAngles[i] =(int) mRecievedData.GetIndexData(i*2 + 1 + 2 );//開始フラグとデータサイズを飛ばす
          mServoAngles[i] +=(int) mRecievedData.GetIndexData(i*2 + 2 ) * 100;//開始フラグとデータサイズを飛ばす

          if (1 == mRecievedData.GetLatestError()){//データサイズが小さいとき
            mServoAngles[i] = WEAKNESS_ANGLE;//脱力状態にする。
          }
        }
      }
    }
//    mRecievedData.ResetDataPosition();
    return 20 + mRecievedData.GetLatestError();
  }else{
    mRecievedData.AddData(rdata);
    return 3;
  }
  
}

int CServoSupport::GetIndexServoAngle(unsigned int i){
  if(i<SERBO_NUMBER){
    return mServoAngles[i];
  }
  return WEAKNESS_ANGLE;
}

void CServoSupport::SetSerovoData(){
  int i;
  int angle=0;
//  Adafruit_PWMServoDriver pwm1=Adafruit_PWMServoDriver(0x40) ;  
//  Adafruit_PWMServoDriver pwm2=Adafruit_PWMServoDriver(0x41) ;  
//  pwm1.begin(); // これをやったらsetPWMをやらないと固まる
//  pwm1.setPWMFreq(PWM_FREQ);
//  pwm2.begin(); // これをやったらsetPWMをやらないと固まる
//  pwm2.setPWMFreq(PWM_FREQ);

//  return;
//  mpwm1.wakeup();
//  mpwm2.wakeup();
//  mpwm1.begin(); // これをやったらsetPWMをやらないと固まる
//  mpwm1.setPWMFreq(PWM_FREQ);
//  mpwm2.begin(); // これをやったらsetPWMをやらないと固まる
//  mpwm2.setPWMFreq(PWM_FREQ);

  for (i = 0 ; i < SERBO_NUMBER ; i++){
    if(mServoAngles[i] <= ANGLE_MAX){
      if (mPreServoAngles[i] != mServoAngles[i]){
        //必要な時1度だけ初期化する。
        break;
      }}
  }
  CWheelSupport::SetWheelData(mServoAngles[18]/100 - 100, mServoAngles[19]/100 - 100);

  for (i = 0 ; i < SERBO_NUMBER ; i++){
    if(mServoAngles[i] <= ANGLE_MAX){
      if (mPreServoAngles[i] != mServoAngles[i]){
        
        angle = mServoAngles[i];
        mPreServoAngles[i] = mServoAngles[i];
        if (i % 8 < 5){
          if (i < 16){
            //大きいサーボ
            if (2 < i%8){
              //ノーブランドサーボ
              angle = map(angle, 0, 18000, UNWKNOW_SERVOMIN, UNWKNOW_SERVOMAX);  // 角度(0~180)を4096の範囲に変換 1/100も考慮
            }else{
              angle = map(angle, 0, 18000, MG996R_SERVOMIN, MG996R_SERVOMAX);  // 角度(0~180)を4096の範囲に変換 1/100も考慮
            }
            if( 0 < i / 8 ){
              mpwm2.setPWM(i -3, 0, angle);                 // サーボを動作させる
            }else{
              mpwm2.setPWM(i, 0, angle);                 // サーボを動作させる
            }
          }else if(i < 18){
            //首
            //小さいサーボ
            angle = map(angle, 0, 18000, MS18_SERVOMIN, MS18_SERVOMAX);  // 角度(0~180)を4096の範囲に変換
            mpwm1.setPWM(i - 10, 0, angle);                 // サーボを動作させる
          }
        }else if(5 <= i % 8){
          //小さいサーボ
          angle = map(angle, 0, 18000, MS18_SERVOMIN, MS18_SERVOMAX);  // 角度(0~180)を4096の範囲に変換
          if( 0 < i / 8 ){
            mpwm1.setPWM(i - 10, 0, angle);                 // サーボを動作させる
          }else{
            mpwm1.setPWM(i - 5, 0, angle);                 // サーボを動作させる
          }
        }
    
      }
    }else{
      if (mPreServoAngles[i] != mServoAngles[i]){
        
      //脱力
        //_SetWeakness(i, pwm1, pwm2);
        mPreServoAngles[i] = mServoAngles[i];
        if (18 <= i){
            CWheelSupport::SetWheelData(0, 0);
        }
      }
    }
 
  }

// delay(1000);
}
void CServoSupport::SetAllWeakness(){
  int i = 0;
  int angle = 0;
  //Adafruit_PWMServoDriver pwm1=Adafruit_PWMServoDriver(0x40) ;  
  //Adafruit_PWMServoDriver pwm2=Adafruit_PWMServoDriver(0x41) ;  
//
  //pwm1.begin(); // これをやったらsetPWMをやらないと固まる
  //pwm1.setPWMFreq(PWM_FREQ);
  //pwm2.begin(); // これをやったらsetPWMをやらないと固まる
  //pwm2.setPWMFreq(PWM_FREQ);
  mpwm1.begin(); // これをやったらsetPWMをやらないと固まる
  mpwm1.setPWMFreq(PWM_FREQ);
  mpwm2.begin(); // これをやったらsetPWMをやらないと固まる
  mpwm2.setPWMFreq(PWM_FREQ);
  //右
//  mpwm1.wakeup();
//  mpwm2.wakeup();
  for (i = 0; i < 16 ; i++){
    //_SetWeakness(i, pwm1, pwm2);
    angle = map(0, 0, 18000, 0, UNWKNOW_SERVOMAX);  // 角度(0~180)を4096の範囲に変換
    mpwm1.setPWM(i, 0, angle);                 // サーボを動作させる
    mpwm2.setPWM(i, 0, angle);                 // サーボを動作させる
  }
  for (i = 0; i < DATA_NUMBER ; i++){
    mPreServoAngles[i] = mServoAngles[i];
  }
  CWheelSupport::SetWheelData(0, 0);
  
}
void CServoSupport::_SetWeakness(int i, Adafruit_PWMServoDriver &pwm1, Adafruit_PWMServoDriver &pwm2){ //サーボを脱力する。
  int angle = 0;

  angle = map(0, 0, 18000, 0, UNWKNOW_SERVOMAX);  // 角度(0~180)を4096の範囲に変換
  if (i % 8 < 5){
    if (i < 16){
      //大きいサーボ
      if( 0 < i / 8 ){
        pwm2.setPWM(i -3, 0, angle);                 // サーボを動作させる
      }else{
        pwm2.setPWM(i, 0, angle);                 // サーボを動作させる
      }
    }else{//首
        //小さいサーボ
      pwm1.setPWM(i - 10, 0, angle);                 // サーボを動作させる
    }
  }else if (5 <= i % 8){
    //小さいサーボ
    if( 0 < i / 8 ){
      pwm1.setPWM(i - 10, 0, angle);                 // サーボを動作させる
    }else{
      pwm1.setPWM(i - 5, 0, angle);                 // サーボを動作させる
    }
  }
  mPreServoAngles[i] = mServoAngles[i];
}