#pragma once
//ServoSupport.h
//copyright 2022 https://robot-creation-diary.com/
//20230320角度周りの整数値を100倍にした
#include <Wire.h>
#include "SerialSupport.h"
#include <Adafruit_PWMServoDriver.h>
#define SERBO_NUMBER (16)
#define WEAKNESS_ANGLE (100000)
#define ANGLE_MAX (22000)
#define PWM_FREQ (358)
#define PULSE_LENGTH ((long)1000000/PWM_FREQ)
#define MG996R_SERVOMIN ((long)400 * (long)4096 / PULSE_LENGTH) // 最小パルス幅(μs)
#define MG996R_SERVOMAX ((long)2600 * (long)4096 / PULSE_LENGTH) // 最大パルス幅(μs)
#define UNWKNOW_SERVOMIN ((long)500 * (long)4096 / PULSE_LENGTH) // 最小パルス幅(μs)
#define UNWKNOW_SERVOMAX ((long)2500 * (long)4096 / PULSE_LENGTH) // 最大パルス幅(μs)
#define MS18_SERVOMIN ((long)550 * (long)4096 / PULSE_LENGTH) // 最小パルス幅(μs)
#define MS18_SERVOMAX ((long)2400 * (long)4096 / PULSE_LENGTH) // 最大パルス幅(μs)
class CServoSupport{
public:
CServoSupport();
~CServoSupport();
int GetRecievedDataSize(); //受信したデータの大きさを得る
void SetSerialDataSize( int dsize); //シリアル通信で使うデータの大きさを設定
unsigned char GetIndexRecievedData(int i ); //シリアル通信で得た指定位置のデータを得る
int SetRecievedData(unsigned char rdata); //シリアル通信で得たデータをサーボの角度データとして保存
int GetIndexServoAngle(unsigned int i); //指定番号のサーボデータを設定
void SetSerovoData(); //保存されたサーボデータを実際にサーボモータに設定する
void SetAllWeakness(); //すべてのサーボを脱力する。
private:
CSerialSupport mRecievedData; //シリアル通信からデータの受け取り
int mServoAngles[SERBO_NUMBER]; //各サーボの角度
int mPreServoAngles[SERBO_NUMBER];// サーボの角度設定を何度も呼ばないようにするために
int mError;
};
//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;
}
//初期化
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);
// pwm.begin();
// pwm.setPWMFreq(PWM_FREQ);
}
CServoSupport::~CServoSupport(){
}
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 < SERBO_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 pwm=Adafruit_PWMServoDriver(0x40) ;
for (i = 0 ; i < 16 ; i++){
if(mServoAngles[i] <= ANGLE_MAX){
if (mPreServoAngles[i] != mServoAngles[i]){
//必要な時1度だけ初期化する。
break;
}}
}
for (i = 0 ; i < 16 ; i++){
if(mServoAngles[i] <= ANGLE_MAX){
if (mPreServoAngles[i] != mServoAngles[i]){
angle = mServoAngles[i];
mPreServoAngles[i] = mServoAngles[i];
if (i % 8 < 3){
angle = map(angle, 0, 18000, MG996R_SERVOMIN, MG996R_SERVOMAX); // 角度(0~180)を4096の範囲に変換 1/100も考慮
pwm.setPWM(i, 0, angle); // サーボを動作させる
}else if ((3 <= i % 8) && (i % 8 < 5)){
angle = map(angle, 0, 18000, UNWKNOW_SERVOMIN, UNWKNOW_SERVOMAX); // 角度(0~180)を4096の範囲に変換
pwm.setPWM(i, 0, angle); // サーボを動作させる
}else if(5 <= i % 8){
angle = map(angle, 0, 18000, MS18_SERVOMIN, MS18_SERVOMAX); // 角度(0~180)を4096の範囲に変換
pwm.setPWM(i, 0, angle); // サーボを動作させる
}
}
}else{
if (mPreServoAngles[i] != mServoAngles[i]){
//脱力
angle = map(0, 0, 18000, 0, UNWKNOW_SERVOMAX); // 角度(0~180)を4096の範囲に変換
pwm.setPWM(i, 0, angle); // サーボを動作させる
mPreServoAngles[i] = mServoAngles[i];
}else{
}
}
}
// delay(1000);
}
void CServoSupport::SetAllWeakness(){
int i = 0;
int angle = 0;
Adafruit_PWMServoDriver pwm=Adafruit_PWMServoDriver(0x40) ;
pwm.begin(); // これをやったらsetPWMをやらないと固まる
pwm.setPWMFreq(PWM_FREQ);
for (i = 0; i < 16 ; i++){
angle = map(0, 0, 18000, 0, UNWKNOW_SERVOMAX); // 角度(0~180)を4096の範囲に変換
pwm.setPWM(i, 0, angle); // サーボを動作させる
mPreServoAngles[i] = mServoAngles[i];
}
}
