STM32G031でアナログサーボをmodbusで制御(サーボ(RS485)(その3)


x 動作確認済み2022/5/18 14:54


目的
サーボ関係の続き
都合よくmobusの規格を解釈して
最小限で実装


![o_con462.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/840352/bfc6110a-a6d2-281f-bdf2-563fb69a6799.jpeg)


![o_con463.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/840352/14edc9ac-8b79-1c00-ffeb-6ac72aa1d6e9.jpeg)



詳しくは、


https://qiita.com/caa45040/items/81a6287ea9d6866f84c6



プログラム


```c++



//ser_modbus_servo_S_031_1

#include <Arduino.h>


//サーボID 複数使う場合は、別のIDにしてください。
#define SERVO_ID   (0)


#define DD PA11    // potentiometer connected to analog pin x

// tx PA14(PB6) 8PIN
// rx PC14(PB7) 1PIN


//プロトタイプ宣言

//8ビット有効　シンクロ(同期)が速い　戻りは、int
int modbus_receive(int id); //ADD 0xc0 re(0x40)
//読み込みステータス
int modbus_receive_status();
//CRCの計算
unsigned short calc_crc(unsigned char *buf, int length);


#define DW   digitalWrite
#define TX1        PA12   // 4pin
#define UART_DELAY 106  // 002-110 9600bps ok 031

//仮想シリアルへの一文字出力 9600bps
void pc_putc(char ch)
{
  DW(TX1, HIGH);

  DW(TX1, LOW);//START
  delayMicroseconds(UART_DELAY); //START BIT WAIT

  for (int ii = 0; ii < 8; ii++) {
    DW(TX1, (ch >> ii) & 1  );
    delayMicroseconds(UART_DELAY); //DATA 1-8BIT WAIT
  }//for

  DW(TX1, HIGH);//Stop
  delayMicroseconds(UART_DELAY); //StOP BIT WAIT

}//pc_putc


//文字列の表示
int pc_printf(char *str1) {

  //文字の中身がゼロか
  while (*str1) {

    //一文字出力
    pc_putc(*str1 ++);

  } //while

  //戻り値
  return (0);
}//pc_printf


int u_key;      // 受信データを格納するchar型の変数
int ch_in_key() {

  // 受信データがあった時だけ、処理を行う
  while (!Serial.available()) {}      // 受信データがあるか？
  u_key = Serial.read();            // 1文字だけ読み込む

  return (u_key);
}


//初期化処理
void setup() {

  delay(3000); //not Delete

  //TXポートの初期化
  pinMode(TX1, OUTPUT);
  DW(TX1, HIGH);

  Serial.begin( 500000 ); // シリアル通信の速度を500000bpsにする。

  //PWMの初期化
  pinMode(DD, OUTPUT);   // sets the LED pin as output
  analogWriteFrequency(50);
  analogWriteResolution(16);

} //setup


//メインループ
void loop() {

  int pp;
  int re_status;

  //サーボの値を受信する　失敗は、_statusで読み取りる
  pp = modbus_receive(0x10 + SERVO_ID); //ID 0
  //直前の受信成功の有無　0失敗　0以外成功
  re_status = modbus_receive_status();

  //デバッグ出力　受信した値の表示
  String thisString;
  thisString = String(re_status);
  pc_printf((char *)thisString.c_str());
  pc_printf(",");
  if ( re_status != 0 ) {
    thisString = String(pp);
    pc_printf((char *)thisString.c_str());
    //pc_printf("\r\n");
  }
  pc_printf("\r\n");

  if ( re_status != 0 ) {
    analogWrite(DD, 1310 + (26 * pp) );   // analogRead values go from 0
                                          // to 4095, pwmWrite values
                                          // from 0 to 65535, so scale roughly
  }//endif

}//loop


//直前のパケットの読み取り失敗の有無
int re_status = 0;
int modbus_receive_status() {

  return (re_status);

}//modbus_receive_status


//さらに下位関数 8ビット ex ADD 0xc0  re(0x40)
int modbus_receive(int id) {

  int ii;             //カウンター
  int pp;             //リターン値
  char out_buff[8];   //バッファー
  int chk;            //チェックサム
  unsigned char read_record[8];
  char x;             //一時領域

  re_status = 0; //読み取り失敗にセット
  pp = 0;

  while (1) {

    //第一バイト目
    //ヘッダー
    x = (char)ch_in_key();
    read_record[1 - 1] = x;
    if (x != 0x00 ) break;

    //第二バイト目
    //アドレス
    x = (char)ch_in_key();
    read_record[2 - 1] = x;
    if (x != id ) break;

    //第三バイト目
    //データの1
    x = (char)ch_in_key();
    read_record[3 - 1] = x;
    if (x != 0x00  ) break;

    //第四バイト目
    //データの2
    x = (char)ch_in_key();
    read_record[4 - 1] = x;
    if (x != 0x00 ) break;

    //第五バイト目
    //データの3
    x = (char)ch_in_key();
    read_record[5 - 1] = x;
    if (x != 0x00 ) break;

    //第六バイト目
    //データの4
    x = (char)ch_in_key();
    read_record[6 - 1] = x;



  //チェックサムの計算
  //Checksum Calculation
  int ret = calc_crc( read_record , 6);
  char crc1 = (char)(  ret     & 0xff);
  char crc2 = (char)( (ret>>8) & 0xff);



    //第七バイト目
    //チェックサム
    x = (char)ch_in_key();
    read_record[7 - 1] = x;
    //if (x != (0x81) ) break;
    if (x != (crc1) ) break;

    //第八バイト目
    //フッター
    x = (char)ch_in_key();
    read_record[8 - 1] = x;
    //if (x != (0xfb) ) break;
    if (x != (crc2) ) break;

    //オールクリア OK
    re_status = 1; //読み取り成功
    pp = read_record[6 - 1];

    break;

  }//while

  //リターン
  return (pp);

} //modbus_receive




// buf    受信データ
// length 受信データ長(CRCを除く)
unsigned short calc_crc(unsigned char *buf, int length) {
  unsigned short crc = 0xFFFF;
  int i,j;
  unsigned char carrayFlag;
  for (i = 0; i < length; i++) {
    crc ^= buf[i];
    for (j = 0; j < 8; j++) {
      carrayFlag = crc & 1;
      crc = crc >> 1;
      if (carrayFlag) {
        crc ^= 0xA001;
      }
    }
  }
  return crc;
}






```