2023年7月 – ページ 3

ＤＣモーター／ソレノイドの台形駆動ライブラリを作ってみた。前回投稿のSoftwarePWMの精度は高いが計算負荷が高すぎて周波数が出せなかったので、今度は可能な限り高速化してみた。

arm/48MHzなら100KHzでも動作可能。駆動時間が加速時間＋減速時間以上で台形駆動、それ未満では三角駆動となる。

汎用的にするため信号出力のみとしているが、output()/handle()をオーバーライドすることでより複雑な制御を行うこともできる。

【台形駆動の波形】

【サンプル・スケッチ】

#include "dcmtd.h"

#define PIN_MOTOR 0

DCMTD dcmotor(PIN_MOTOR);

void setup()
{
  /* 1KHz(1000us)、0-100%で台形駆動(加速期間:100000us, 減速期間:100000us)する設定 */
  dcmotor.begin(1000, 100000, 100000, 100, 0);
}

void loop()
{
  /* 100%で300000us間駆動する */
  dcmotor.run(300000);
  delay(1000);
}

#include "dcmtd.h"

#define PIN_MOTOR 0

DCMTD dcmotor(PIN_MOTOR);

void setup()

{

/* 1KHz(1000us)、0-100%で台形駆動(加速期間:100000us, 減速期間:100000us)する設定 */

dcmotor.begin(1000, 100000, 100000, 100, 0);

}

void loop()

{

/* 100%で300000us間駆動する */

dcmotor.run(300000);

delay(1000);

}

【ライブラリ】

/*
  dcmtd.h - DC Motor Trapezoidal drive Library for Arduino

  Copyright (c) 2023 Sasapea's Lab. All right reserved.

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU Lesser General
  Public License along with this library; if n, write to the
  Free Software Foundation, Inc., 59 Temple Place, Suite 330,
  Boston, MA  02111-1307  USA
*/
#pragma once

class DCMTD
{
  public:

    DCMTD(uint16_t port, bool polarity = 0)
    : _port(port)
    , _polarity(polarity)
    , _clock(0)
    , _accel(0)
    , _decel(0)
    , _maxdc(0)
    , _mindc(0)
    , _status(0)
    {
    }

    virtual ~DCMTD(void)
    {
    }

    void begin(uint32_t clock, uint32_t accel, uint32_t decel, uint8_t maxdc = 100, uint8_t mindc = 0)
    {
      _clock = clock;
      _accel = accel;
      _decel = decel;
      _maxdc = maxdc > 100 ? 100 : maxdc;
      _mindc = mindc > 100 ? 100 : mindc;
      pinMode(_port, OUTPUT);
      digitalWrite(_port, _polarity);
    }

    void end(void)
    {
      pinMode(_port, INPUT);
    }

    uint8_t run(uint32_t width)
    {
      return run(width, _maxdc);
    }

    uint8_t run(uint32_t width, uint8_t duty)
    {
      _status = 0;
      if (duty > 100)
        duty = 100;
      uint32_t maxdc = (uint64_t)_clock * duty / 100;
      uint32_t mindc = (uint64_t)_clock * _mindc / 100;
      uint32_t accel = (uint64_t)_accel * duty / 100;
      uint32_t decel = (uint64_t)_decel * duty / 100;
      if (maxdc <= mindc)
        accel = decel = 0;
      uint32_t acdlt = accel ? (uint64_t)(maxdc - mindc) * 256 * _clock / accel : 0;
      uint32_t dedlt = decel ? (uint64_t)(maxdc - mindc) * 256 * _clock / decel : 0;
      if (width < accel + decel)
        decel = (uint64_t)decel * width / (accel + decel);
      uint32_t coord = 0;
      uint32_t start = micros();
      uint32_t t, clock = start;
      while (handle(), (_status == 0) && ((t = micros() - start) < width))
      {
        if (t + decel >= width)
        {
          if (coord < dedlt)
            coord = 0;
          else
            coord -= dedlt;
          t = mindc + coord / 256;
        }
        else if (t < accel)
        {
          t = mindc + coord / 256;
          coord += acdlt;
        }
        else
          t = maxdc;
        if (t)
        {
          output(_port, _polarity ^ HIGH);
          while (micros() - clock < t)
            continue;
        }
        if (t < _clock)
        {
          output(_port, _polarity ^ LOW);
          while (micros() - clock < _clock)
            continue;
        }
        clock += _clock;
      }
      output(_port, _polarity);
      return _status;
    }

    void stop(uint8_t status)
    {
      _status = status;
    }

  protected:

    virtual void output(uint16_t port, uint8_t value)
    {
      digitalWrite(port, value);
    }

    virtual void handle(void)
    {
    }

  private:

    uint16_t _port;
    uint8_t  _polarity;
    uint32_t _clock;
    uint32_t _accel;
    uint32_t _decel;
    uint8_t  _maxdc;
    uint8_t  _mindc;
    volatile uint8_t _status;
};

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

dcmtd.h - DC Motor Trapezoidal drive Library for Arduino

This library is free software; you can redistribute it and/or

modify it under the terms of the GNU Lesser General Public

License as published by the Free Software Foundation; either

version 2.1 of the License, or (at your option) any later version.

This library is distributed in the hope that it will be useful,

but WITHOUT ANY WARRANTY; without even the implied warranty of

MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU

Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General

Public License along with this library; if n, write to the

Free Software Foundation, Inc., 59 Temple Place, Suite 330,

Boston, MA 02111-1307 USA

#pragma once

class DCMTD

{

public:

DCMTD(uint16_t port, bool polarity = 0)

: _port(port)

, _polarity(polarity)

, _clock(0)

, _accel(0)

, _decel(0)

, _maxdc(0)

, _mindc(0)

, _status(0)

{

}

virtual ~DCMTD(void)

{

}

void begin(uint32_t clock, uint32_t accel, uint32_t decel, uint8_t maxdc = 100, uint8_t mindc = 0)

{

_clock = clock;

_accel = accel;

_decel = decel;

_maxdc = maxdc > 100 ? 100 : maxdc;

_mindc = mindc > 100 ? 100 : mindc;

pinMode(_port, OUTPUT);

digitalWrite(_port, _polarity);

}

void end(void)

{

pinMode(_port, INPUT);

}

uint8_t run(uint32_t width)

{

return run(width, _maxdc);

}

uint8_t run(uint32_t width, uint8_t duty)

{

_status = 0;

if (duty > 100)

duty = 100;

uint32_t maxdc = (uint64_t)_clock * duty / 100;

uint32_t mindc = (uint64_t)_clock * _mindc / 100;

uint32_t accel = (uint64_t)_accel * duty / 100;

uint32_t decel = (uint64_t)_decel * duty / 100;

if (maxdc <= mindc)

accel = decel = 0;

uint32_t acdlt = accel ? (uint64_t)(maxdc - mindc) * 256 * _clock / accel : 0;

uint32_t dedlt = decel ? (uint64_t)(maxdc - mindc) * 256 * _clock / decel : 0;

if (width < accel + decel)

decel = (uint64_t)decel * width / (accel + decel);

uint32_t coord = 0;

uint32_t start = micros();

uint32_t t, clock = start;

while (handle(), (_status == 0) && ((t = micros() - start) < width))

{

if (t + decel >= width)

{

if (coord < dedlt)

coord = 0;

else

coord -= dedlt;

t = mindc + coord / 256;

}

else if (t < accel)

{

t = mindc + coord / 256;

coord += acdlt;

}

else

t = maxdc;

if (t)

{

output(_port, _polarity ^ HIGH);

while (micros() - clock < t)

continue;

}

if (t < _clock)

{

output(_port, _polarity ^ LOW);

while (micros() - clock < _clock)

continue;

}

clock += _clock;

}

output(_port, _polarity);

return _status;

}

void stop(uint8_t status)

{

_status = status;

}

protected:

virtual void output(uint16_t port, uint8_t value)

{

digitalWrite(port, value);

}

virtual void handle(void)

{

}

private:

uint16_t _port;

uint8_t _polarity;

uint32_t _clock;

uint32_t _accel;

uint32_t _decel;

uint8_t _maxdc;

uint8_t _mindc;

volatile uint8_t _status;

};