2023年8月 – Sasapea's Lab

ステッピング・モーターの台形駆動ライブラリ
 ＤＣモーター／ソレノイドの台形駆動ライブラリ

上記投稿のコードを改良し複数モーターを同時駆動できる台形駆動ライブラリを作ってみた。

最大クループ数は256(規定値は32)、最大モーター数はグループ毎に255個、最大駆動周波数はCPUの処理性能とモーター同時駆動数に依存する。32ビット以上のCPUがお勧めだが、モーター１個の場合arm(48MHz)なら20KHz程度まで行けそうだ。

波形生成をソフトウェアのみで処理しているので全てのARDUINOで利用可能である。

下記投稿内のソースコード(hardware.cpp)を参考にタイマー割込みを利用することもできる。

Micro Core Library for Arduino

【サンプル・スケッチ】

#include "a4988.h"
#include "stmtd.h"
#include "dcmtd.h"

#define PIN_DCM1 0
#define PIN_DCM2 1
#define PIN_STM1 2
#define PIN_STM2 3
#define PIN_DIR1 4
#define PIN_DIR2 5
#define PIN_SW   10

/* for A4988 */
A4988<0, PIN_STM1, PIN_DIR1> driver1;
A4988<0, PIN_STM2, PIN_DIR2> driver2;

/* for stepper */
STMTD stepper1(0, PIN_STM1);
STMTD stepper2(0, PIN_STM2);

/* for dc-motor */
DCMTD dcmotor1(0, PIN_DCM1);
DCMTD dcmotor2(0, PIN_DCM2);

void setup()
{
  pinMode(PIN_SW, INPUT_PULLUP);

  /* for A4988 */
  driver1.begin(1000, 10000, 100, 100);
  driver2.begin(1000, 10000, 100, 100);

  /* for stepper */
  stepper1.begin(1000, 10000, 100, 100);
  stepper2.begin(1000, 10000, 100, 100);

  /* for dc-motor */
  dcmotor1.begin(1000, 100000, 100000, 100, 10);
  dcmotor2.begin(1000, 100000, 100000, 100, 10);
}

void loop()
{
  /* for A4988 */
  if (!STMTD::handle(0))
  {
    if (digitalRead(PIN_SW))
    {
      driver1.start(300);
      driver2.start(500);
    }
  }

  /* for stepper */
  if (!STMTD::handle(0))
  {
    if (digitalRead(PIN_SW))
    {
      stepper1.start(300);
      stepper2.start(500);
    }
  }

  /* for dc-motor */
  if (!DCMTD::handle(0))
  {
    if (digitalRead(PIN_SW))
    {
      dcmotor1.start(300000);
      dcmotor2.start(500000);
    }
  }
}

#include "a4988.h"

#include "stmtd.h"

#include "dcmtd.h"

#define PIN_DCM1 0

#define PIN_DCM2 1

#define PIN_STM1 2

#define PIN_STM2 3

#define PIN_DIR1 4

#define PIN_DIR2 5

#define PIN_SW 10

/* for A4988 */

A4988<0, PIN_STM1, PIN_DIR1> driver1;

A4988<0, PIN_STM2, PIN_DIR2> driver2;

/* for stepper */

STMTD stepper1(0, PIN_STM1);

STMTD stepper2(0, PIN_STM2);

/* for dc-motor */

DCMTD dcmotor1(0, PIN_DCM1);

DCMTD dcmotor2(0, PIN_DCM2);

void setup()

{

pinMode(PIN_SW, INPUT_PULLUP);

/* for A4988 */

driver1.begin(1000, 10000, 100, 100);

driver2.begin(1000, 10000, 100, 100);

/* for stepper */

stepper1.begin(1000, 10000, 100, 100);

stepper2.begin(1000, 10000, 100, 100);

/* for dc-motor */

dcmotor1.begin(1000, 100000, 100000, 100, 10);

dcmotor2.begin(1000, 100000, 100000, 100, 10);

}

void loop()

{

/* for A4988 */

if (!STMTD::handle(0))

{

if (digitalRead(PIN_SW))

{

driver1.start(300);

driver2.start(500);

}

/* for stepper */

if (!STMTD::handle(0))

{

if (digitalRead(PIN_SW))

{

stepper1.start(300);

stepper2.start(500);

}

/* for dc-motor */

if (!DCMTD::handle(0))

{

if (digitalRead(PIN_SW))

{

dcmotor1.start(300000);

dcmotor2.start(500000);

}

【修正履歴】
2023-09-18
リミット入力が入った状態からの原点復帰動作を改良。

2023-09-10
リミット入力のデバウンス処理の代わりとして20msの駆動開始遅延を入れていたがデバウンス処理を追加し駆動開始遅延をなくすとともにデバウンス処理によってリミット入力による停止遅延が発生しないように改良。

2023-09-07
複数のモーターをグループとして管理できるように改良。handle(グループ番号)の戻り値で指定グループに含まれるすべてのモーターの動作状況を返す。

2023-08-25
A4988(DRV8825)に対応したクラスを作成してみた。リミットスイッチの指定をすることでhome()の呼び出しにより原点復帰動作が可能。
start()の駆動ステップ数は相対指定。その符号により回転方向が変わる。

2023-08-19
handle()の仕様が少しわかりにくかったので仕様変更を行った。
handle()は駆動処理を行い、まだ駆動中のモーターがあればtrue、なければfalseを返す。新たに追加したrunning()は個々のモーターの駆動状態を返す。

2023-08-17
AVRシリーズで全く動作しなかったため修正。

【ライブラリ】

/*
  a4988.h - Microstepping Driver 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

#include <stdint.h>
#include <stdbool.h>
#include "stmtd.h"

template
<
uint8_t GROUP,
int     STEP,
int     DIR,
bool    DIR_POLARITY   = true,
int     LIMIT          = -1,
bool    LIMIT_POLARITY = true,
int32_t LIMIT_STEPS    = INT32_MAX,
int     RESET          = -1,
int     SLEEP          = -1,
int     ENABLE         = -1,
int     MS1            = -1,
int     MS2            = -1,
int     MS3            = -1,
int32_t DEBOUNCE       = 20000
>
class A4988 : public STMTD
{
  public:

    typedef enum
    {
      /* A4988 */
      MODE1  = 0,
      MODE2  = 1,
      MODE4  = 2,
      MODE8  = 3,
      MODE16 = 7,
      /* DRV8825 */
      MODE1_DRV8825  = 0,
      MODE2_DRV8825  = 1,
      MODE4_DRV8825  = 2,
      MODE8_DRV8825  = 3,
      MODE16_DRV8825 = 4,
      MODE32_DRV8825 = 5,
      /* TMC2208 */
      MODE2_TMC2208  = 1,
      MODE4_TMC2208  = 2,
      MODE8_TMC2208  = 0,
      MODE16_TMC2208 = 3,
      /* TMC2209 */
      MODE8_TMC2209  = 0,
      MODE16_TMC2209 = 3,
      MODE32_TMC2209 = 2,
      MODE64_TMC2209 = 1,
      /* ST820 */
      MODE1_ST820   = 0,
      MODE2_ST820   = 4,
      MODE4_ST820   = 2,
      MODE8_ST820   = 6,
      MODE16_ST820  = 1,
      MODE32_ST820  = 5,
      MODE128_ST820 = 3,
      MODE256_ST820 = 7,
      /* LV8729 */
      MODE1_LV8729   = 0,
      MODE2_LV8729   = 1,
      MODE4_LV8729   = 2,
      MODE8_LV8729   = 3,
      MODE16_LV8729  = 4,
      MODE32_LV8729  = 5,
      MODE64_LV8729  = 6,
      MODE128_LV8729 = 7,
    } MODE;

    A4988(void) : STMTD(GROUP, STEP)
    {
    }

    void begin(uint32_t maxspd, uint32_t minspd, uint32_t acstep, uint32_t destep) override
    {
      STMTD::begin(maxspd, minspd, acstep, destep);
      _origin   = false;
      _control  = 0;
      _position = 0;
      _debounce = micros() - DEBOUNCE;
      limit();
      if (DIR >= 0)    pinMode(DIR, OUTPUT);
      if (LIMIT >= 0)  pinMode(LIMIT, INPUT_PULLUP);
      if (RESET >= 0)  pinMode(RESET, OUTPUT);
      if (SLEEP >= 0)  pinMode(SLEEP, OUTPUT);
      if (ENABLE >= 0) pinMode(ENABLE, OUTPUT);
      if (MS1 >= 0)    pinMode(MS1, OUTPUT);
      if (MS2 >= 0)    pinMode(MS2, OUTPUT);
      if (MS3 >= 0)    pinMode(MS3, OUTPUT);
      sleep(false);
      enable(true);
      direction(true);
      microstep(MODE1);
      reset();
    }

    void end(void) override
    {
      STMTD::end();
      if (DIR >= 0)    pinMode(DIR, INPUT);
      if (LIMIT >= 0)  pinMode(LIMIT, INPUT);
      if (RESET >= 0)  pinMode(RESET, INPUT);
      if (SLEEP >= 0)  pinMode(SLEEP, INPUT);
      if (ENABLE >= 0) pinMode(ENABLE, INPUT);
      if (MS1 >= 0)    pinMode(MS1, INPUT);
      if (MS2 >= 0)    pinMode(MS2, INPUT);
      if (MS3 >= 0)    pinMode(MS3, INPUT);
    }

    virtual ~A4988(void)
    {
    }

    void enable(bool en)
    {
      if ((ENABLE >= 0) && !running()) digitalWrite(ENABLE, !en);
    }

    void sleep(bool en)
    {
      if ((SLEEP >= 0) && !running())
      {
        uint8_t val = digitalRead(SLEEP);
        digitalWrite(SLEEP, !en);
        if (!en && !val)
        {
          delayMicroseconds(1000);
          _position = 0;
          _control  = 0;
        }
      }
    }

    void microstep(MODE mode)
    {
      if (((MS1 >= 0) || (MS2 >= 0) || (MS3 >= 0)) && !running())
      {
        if (MS1 >= 0) digitalWrite(MS1, (mode >> 0) & 1);
        if (MS2 >= 0) digitalWrite(MS2, (mode >> 1) & 1);
        if (MS3 >= 0) digitalWrite(MS3, (mode >> 2) & 1);
      }
    }

    void reset(void)
    {
      if ((RESET >= 0) && !running())
      {
        digitalWrite(RESET, LOW);
        delayMicroseconds(1);
        digitalWrite(RESET, HIGH);
        _position = 0;
        _control  = 0;
      }
    }

    bool home(uint32_t delay = 0)
    {
      _origin = false;
      return (LIMIT >= 0) && !running() ? 
        (limit() ? start(-(_position = -LIMIT_STEPS), _minspd, delay, 1)  /* limit-sw on --> off */
                 : start(-(_position = +LIMIT_STEPS), _minspd, delay, 2)) /* limit-sw off --> on */
        : false;
    }

    bool start(int32_t steps, uint32_t maxspd, uint32_t delay = 0) override
    {
      return start(steps, maxspd, delay, 0);
    }

    bool limit(void) override
    {
      return limit(_control);
    }

  protected:

    bool start(int32_t steps, uint32_t maxspd, uint32_t delay, uint8_t ctrl)
    {
      bool rv = false;
      if (!running() && (!limit(ctrl) || (steps >= 0))
        && (!_origin || ((_position + steps >= 0) && (_position + steps <= LIMIT_STEPS))))
      {
        direction(steps < 0);
        if ((rv = STMTD::start(abs(steps), maxspd, delay)))
        {
          _control = ctrl;
          _position += steps;
        }
      }
      return rv;
    }

    bool limit(uint8_t ctrl)
    {
      int32_t now = micros();
      if ((uint32_t)(now - _debounce) >= DEBOUNCE)
      {
        uint8_t val = (LIMIT >= 0) && (digitalRead(LIMIT) ^ (ctrl & 1) ^ LIMIT_POLARITY);
        if (val == _limit)
          _debounce = now - DEBOUNCE;
        else
        {
          _debounce = now;
          _limit = val;
        }
      }
      return _limit;
    }

    bool finish(CAUSE cause) override
    {
      bool rv = STMTD::finish(cause);
      uint8_t ctrl = _control;
      _control = 0;
      if ((ctrl != 0) && (cause == CAUSE_LIMIT))
      {
        if (ctrl & 1)
          home(DEBOUNCE);
        else
          _origin = true;
      }
      return rv;
    }

    void direction(bool change)
    {
      if (DIR >= 0) digitalWrite(DIR, change ^ DIR_POLARITY);
    }

  private:

    bool    _origin;
    uint8_t _control;
    int32_t _position;
    int32_t _debounce;
    uint8_t _limit;
};

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

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

a4988.h - Microstepping Driver 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

#include <stdint.h>

#include <stdbool.h>

#include "stmtd.h"

template

uint8_t GROUP,

int STEP,

int DIR,

bool DIR_POLARITY = true,

int LIMIT = -1,

bool LIMIT_POLARITY = true,

int32_t LIMIT_STEPS = INT32_MAX,

int RESET = -1,

int SLEEP = -1,

int ENABLE = -1,

int MS1 = -1,

int MS2 = -1,

int MS3 = -1,

int32_t DEBOUNCE = 20000

class A4988 : public STMTD

{

public:

typedef enum

{

/* A4988 */

MODE1 = 0,

MODE2 = 1,

MODE4 = 2,

MODE8 = 3,

MODE16 = 7,

/* DRV8825 */

MODE1_DRV8825 = 0,

MODE2_DRV8825 = 1,

MODE4_DRV8825 = 2,

MODE8_DRV8825 = 3,

MODE16_DRV8825 = 4,

MODE32_DRV8825 = 5,

/* TMC2208 */

MODE2_TMC2208 = 1,

MODE4_TMC2208 = 2,

MODE8_TMC2208 = 0,

MODE16_TMC2208 = 3,

/* TMC2209 */

MODE8_TMC2209 = 0,

MODE16_TMC2209 = 3,

MODE32_TMC2209 = 2,

MODE64_TMC2209 = 1,

/* ST820 */

MODE1_ST820 = 0,

MODE2_ST820 = 4,

MODE4_ST820 = 2,

MODE8_ST820 = 6,

MODE16_ST820 = 1,

MODE32_ST820 = 5,

MODE128_ST820 = 3,

MODE256_ST820 = 7,

/* LV8729 */

MODE1_LV8729 = 0,

MODE2_LV8729 = 1,

MODE4_LV8729 = 2,

MODE8_LV8729 = 3,

MODE16_LV8729 = 4,

MODE32_LV8729 = 5,

MODE64_LV8729 = 6,

MODE128_LV8729 = 7,

} MODE;

A4988(void) : STMTD(GROUP, STEP)

{

}

void begin(uint32_t maxspd, uint32_t minspd, uint32_t acstep, uint32_t destep) override

{

STMTD::begin(maxspd, minspd, acstep, destep);

_origin = false;

_control = 0;

_position = 0;

_debounce = micros() - DEBOUNCE;

limit();

if (DIR >= 0) pinMode(DIR, OUTPUT);

if (LIMIT >= 0) pinMode(LIMIT, INPUT_PULLUP);

if (RESET >= 0) pinMode(RESET, OUTPUT);

if (SLEEP >= 0) pinMode(SLEEP, OUTPUT);

if (ENABLE >= 0) pinMode(ENABLE, OUTPUT);

if (MS1 >= 0) pinMode(MS1, OUTPUT);

if (MS2 >= 0) pinMode(MS2, OUTPUT);

if (MS3 >= 0) pinMode(MS3, OUTPUT);

sleep(false);

enable(true);

direction(true);

microstep(MODE1);

reset();

}

void end(void) override

{

STMTD::end();

if (DIR >= 0) pinMode(DIR, INPUT);

if (LIMIT >= 0) pinMode(LIMIT, INPUT);

if (RESET >= 0) pinMode(RESET, INPUT);

if (SLEEP >= 0) pinMode(SLEEP, INPUT);

if (ENABLE >= 0) pinMode(ENABLE, INPUT);

if (MS1 >= 0) pinMode(MS1, INPUT);

if (MS2 >= 0) pinMode(MS2, INPUT);

if (MS3 >= 0) pinMode(MS3, INPUT);

}

virtual ~A4988(void)

{

}

void enable(bool en)

{

if ((ENABLE >= 0) && !running()) digitalWrite(ENABLE, !en);

}

void sleep(bool en)

{

if ((SLEEP >= 0) && !running())

{

uint8_t val = digitalRead(SLEEP);

digitalWrite(SLEEP, !en);

if (!en && !val)

{

delayMicroseconds(1000);

_position = 0;

_control = 0;

}

void microstep(MODE mode)

{

if (((MS1 >= 0) || (MS2 >= 0) || (MS3 >= 0)) && !running())

{

if (MS1 >= 0) digitalWrite(MS1, (mode >> 0) & 1);

if (MS2 >= 0) digitalWrite(MS2, (mode >> 1) & 1);

if (MS3 >= 0) digitalWrite(MS3, (mode >> 2) & 1);

}

void reset(void)

{

if ((RESET >= 0) && !running())

{

digitalWrite(RESET, LOW);

delayMicroseconds(1);

digitalWrite(RESET, HIGH);

_position = 0;

_control = 0;

}

bool home(uint32_t delay = 0)

{

_origin = false;

return (LIMIT >= 0) && !running() ?

(limit() ? start(-(_position = -LIMIT_STEPS), _minspd, delay, 1) /* limit-sw on --> off */

: start(-(_position = +LIMIT_STEPS), _minspd, delay, 2)) /* limit-sw off --> on */

: false;

}

bool start(int32_t steps, uint32_t maxspd, uint32_t delay = 0) override

{

return start(steps, maxspd, delay, 0);

}

bool limit(void) override

{

return limit(_control);

}

protected:

bool start(int32_t steps, uint32_t maxspd, uint32_t delay, uint8_t ctrl)

{

bool rv = false;

if (!running() && (!limit(ctrl) || (steps >= 0))

&& (!_origin || ((_position + steps >= 0) && (_position + steps <= LIMIT_STEPS))))

{

direction(steps < 0);

if ((rv = STMTD::start(abs(steps), maxspd, delay)))

{

_control = ctrl;

_position += steps;

}

return rv;

}

bool limit(uint8_t ctrl)

{

int32_t now = micros();

if ((uint32_t)(now - _debounce) >= DEBOUNCE)

{

uint8_t val = (LIMIT >= 0) && (digitalRead(LIMIT) ^ (ctrl & 1) ^ LIMIT_POLARITY);

if (val == _limit)

_debounce = now - DEBOUNCE;

else

{

_debounce = now;

_limit = val;

}

return _limit;

}

bool finish(CAUSE cause) override

{

bool rv = STMTD::finish(cause);

uint8_t ctrl = _control;

_control = 0;

if ((ctrl != 0) && (cause == CAUSE_LIMIT))

{

if (ctrl & 1)

home(DEBOUNCE);

else

_origin = true;

}

return rv;

}

void direction(bool change)

{

if (DIR >= 0) digitalWrite(DIR, change ^ DIR_POLARITY);

}

private:

bool _origin;

uint8_t _control;

int32_t _position;

int32_t _debounce;

uint8_t _limit;

};

/*
  stmtd.h - Stepping 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 clock, write to the
  Free Software Foundation, Inc., 59 Temple Place, Suite 330,
  Boston, MA  02111-1307  USA
*/
#pragma once

#include <stdint.h>
#include <stdbool.h>
#include "alarm.h"

#if !defined(STMTD_MAX_GROUPS)
  #define STMTD_MAX_GROUPS 32
#endif

class STMTD
{
  public:

    typedef enum
    {
      CAUSE_COMPLETE,
      CAUSE_LIMIT,
      CAUSE_ABORT,
    } CAUSE;

    STMTD(uint8_t group, uint16_t port, bool polarity = false);
    virtual ~STMTD(void);
    virtual void begin(uint32_t maxspd, uint32_t minspd, uint32_t acstep, uint32_t destep);
    virtual void end(void);
    virtual bool start(int32_t steps);
    virtual bool start(int32_t steps, uint32_t maxspd, uint32_t delay = 0);
    virtual void stop(void);
    virtual bool limit(void);
    virtual bool running(void);
    static bool handle(uint8_t group);

  protected:

    uint8_t  _group;
    uint16_t _port;
    uint8_t  _polarity;
    uint32_t _maxspd;
    uint32_t _minspd;
    uint32_t _acstep;
    uint32_t _destep;

    bool rise(Alarm& alarm);
    bool fall(Alarm& alarm);
    virtual bool finish(CAUSE cause);
    virtual void output(uint16_t port, uint8_t value);

  private:

    static bool rise0(Alarm& alarm);
    static bool fall0(Alarm& alarm);

    Alarm    _alarm;

    uint32_t _run_steps;
    uint32_t _run_maxspd;
    uint32_t _run_acstep;
    uint32_t _run_destep;
    uint32_t _run_acdlt;
    uint32_t _run_dedlt;
    uint32_t _run_coord;
    uint32_t _run_clock;
    uint32_t _run_count;
};

stmtd.h - Stepping 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 clock, write to the

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

Boston, MA 02111-1307 USA

#pragma once

#include <stdint.h>

#include <stdbool.h>

#include "alarm.h"

#if !defined(STMTD_MAX_GROUPS)

#define STMTD_MAX_GROUPS 32

#endif

class STMTD

{

public:

typedef enum

{

CAUSE_COMPLETE,

CAUSE_LIMIT,

CAUSE_ABORT,

} CAUSE;

STMTD(uint8_t group, uint16_t port, bool polarity = false);

virtual ~STMTD(void);

virtual void begin(uint32_t maxspd, uint32_t minspd, uint32_t acstep, uint32_t destep);

virtual void end(void);

virtual bool start(int32_t steps);

virtual bool start(int32_t steps, uint32_t maxspd, uint32_t delay = 0);

virtual void stop(void);

virtual bool limit(void);

virtual bool running(void);

static bool handle(uint8_t group);

protected:

uint8_t _group;

uint16_t _port;

uint8_t _polarity;

uint32_t _maxspd;

uint32_t _minspd;

uint32_t _acstep;

uint32_t _destep;

bool rise(Alarm& alarm);

bool fall(Alarm& alarm);

virtual bool finish(CAUSE cause);

virtual void output(uint16_t port, uint8_t value);

private:

static bool rise0(Alarm& alarm);

static bool fall0(Alarm& alarm);

Alarm _alarm;

uint32_t _run_steps;

uint32_t _run_maxspd;

uint32_t _run_acstep;

uint32_t _run_destep;

uint32_t _run_acdlt;

uint32_t _run_dedlt;

uint32_t _run_coord;

uint32_t _run_clock;

uint32_t _run_count;

};

/*
  stmtd.cpp - Stepping 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 clock, write to the
  Free Software Foundation, Inc., 59 Temple Place, Suite 330,
  Boston, MA  02111-1307  USA
*/
#include "stmtd.h"

static uint8_t actives[STMTD_MAX_GROUPS];

STMTD::STMTD(uint8_t group, uint16_t port, bool polarity)
: _group(group)
, _port(port)
, _polarity(polarity)
, _maxspd(0)
, _minspd(0)
, _acstep(0)
, _destep(0)
, _run_steps(0)
{
  if (_group > STMTD_MAX_GROUPS - 1)
    _group = STMTD_MAX_GROUPS - 1;
}

STMTD::~STMTD(void)
{
}

void STMTD::begin(uint32_t maxspd, uint32_t minspd, uint32_t acstep, uint32_t destep)
{
  end();
  _maxspd = maxspd;
  _minspd = minspd;
  _acstep = acstep;
  _destep = destep;
  _run_steps = 0;
  pinMode(_port, OUTPUT);
  digitalWrite(_port, _polarity ^ LOW);
}

void STMTD::end(void)
{
  stop();
  pinMode(_port, INPUT);
}

bool STMTD::start(int32_t steps)
{
  return start(steps, _maxspd);
}

bool STMTD::start(int32_t steps, uint32_t maxspd, uint32_t delay)
{
  if ((steps <= 0) || _run_steps)
    return false;
  ++actives[_group];
  _run_steps  = steps;
  _run_maxspd = maxspd;
  _run_acstep = _minspd > _run_maxspd ? _acstep * (_minspd - _run_maxspd) / (_minspd - _maxspd) : 0;
  _run_destep = _minspd > _run_maxspd ? _destep * (_minspd - _run_maxspd) / (_minspd - _maxspd) : 0;
  if (_minspd <= _run_maxspd)
    _run_acstep = _run_destep = 0;
  _run_acdlt = _run_acstep ? (_minspd - _run_maxspd) * 256 / _run_acstep : 0;
  _run_dedlt = _run_destep ? (_minspd - _run_maxspd) * 256 / _run_destep : 0;
  if (_run_steps < _run_acstep + _run_destep)
    _run_destep = _run_destep * _run_steps / (_run_acstep + _run_destep);
  _run_coord  = 0;
  _run_count  = 0;
  AlarmTimer.start(_alarm.param(0, this).handler(rise0).interval(delay ? delay : 1));
  return true;
}

void STMTD::stop(void)
{
  if (AlarmTimer.cancel(_alarm))
    finish(CAUSE_ABORT);
}

bool STMTD::limit(void)
{
  return false;
}

bool STMTD::running(void)
{
  return _run_steps != 0;
}

bool STMTD::handle(uint8_t group)
{
  AlarmTimer.handle();
  return actives[group > STMTD_MAX_GROUPS - 1 ? STMTD_MAX_GROUPS - 1 : group] != 0;
}

bool STMTD::rise(Alarm& alarm)
{
  if (limit())
    return finish(CAUSE_LIMIT);
  output(_port, _polarity ^ HIGH);
  if (_run_count + _run_destep >= _run_steps)
  {
    if (_run_coord < _run_dedlt)
      _run_coord = 0;
    else
      _run_coord -= _run_dedlt;
    _run_clock = _minspd - _run_coord / 256;
  }
  else if (_run_count < _run_acstep)
  {
    _run_clock = _minspd - _run_coord / 256;
    _run_coord += _run_acdlt;
  }
  else
    _run_clock = _run_maxspd;
  alarm.handler(fall0).interval(_run_clock >> 1, false);
  return true;
}

bool STMTD::fall(Alarm& alarm)
{
  if (limit())
    return finish(CAUSE_LIMIT);
  if (++_run_count >= _run_steps)
    return finish(CAUSE_COMPLETE);
  output(_port, _polarity ^ LOW);
  alarm.handler(rise0).interval(_run_clock - (_run_clock >> 1), false);
  return true;
}

bool STMTD::rise0(Alarm& alarm)
{
  return ((STMTD *)alarm.param(0))->rise(alarm);
}

bool STMTD::fall0(Alarm& alarm)
{
  return ((STMTD *)alarm.param(0))->fall(alarm);
}

bool STMTD::finish([[maybe_unused]] CAUSE cause)
{
  output(_port, _polarity ^ LOW);
  _run_steps = 0;
  --actives[_group];
  return false;
}

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

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

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

stmtd.cpp - Stepping 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 clock, write to the

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

Boston, MA 02111-1307 USA

#include "stmtd.h"

static uint8_t actives[STMTD_MAX_GROUPS];

STMTD::STMTD(uint8_t group, uint16_t port, bool polarity)

: _group(group)

, _port(port)

, _polarity(polarity)

, _maxspd(0)

, _minspd(0)

, _acstep(0)

, _destep(0)

, _run_steps(0)

{

if (_group > STMTD_MAX_GROUPS - 1)

_group = STMTD_MAX_GROUPS - 1;

}

STMTD::~STMTD(void)

{

}

void STMTD::begin(uint32_t maxspd, uint32_t minspd, uint32_t acstep, uint32_t destep)

{

end();

_maxspd = maxspd;

_minspd = minspd;

_acstep = acstep;

_destep = destep;

_run_steps = 0;

pinMode(_port, OUTPUT);

digitalWrite(_port, _polarity ^ LOW);

}

void STMTD::end(void)

{

stop();

pinMode(_port, INPUT);

}

bool STMTD::start(int32_t steps)

{

return start(steps, _maxspd);

}

bool STMTD::start(int32_t steps, uint32_t maxspd, uint32_t delay)

{

if ((steps <= 0) || _run_steps)

return false;

++actives[_group];

_run_steps = steps;

_run_maxspd = maxspd;

_run_acstep = _minspd > _run_maxspd ? _acstep * (_minspd - _run_maxspd) / (_minspd - _maxspd) : 0;

_run_destep = _minspd > _run_maxspd ? _destep * (_minspd - _run_maxspd) / (_minspd - _maxspd) : 0;

if (_minspd <= _run_maxspd)

_run_acstep = _run_destep = 0;

_run_acdlt = _run_acstep ? (_minspd - _run_maxspd) * 256 / _run_acstep : 0;

_run_dedlt = _run_destep ? (_minspd - _run_maxspd) * 256 / _run_destep : 0;

if (_run_steps < _run_acstep + _run_destep)

_run_destep = _run_destep * _run_steps / (_run_acstep + _run_destep);

_run_coord = 0;

_run_count = 0;

AlarmTimer.start(_alarm.param(0, this).handler(rise0).interval(delay ? delay : 1));

return true;

}

void STMTD::stop(void)

{

if (AlarmTimer.cancel(_alarm))

finish(CAUSE_ABORT);

}

bool STMTD::limit(void)

{

return false;

}

bool STMTD::running(void)

{

return _run_steps != 0;

}

bool STMTD::handle(uint8_t group)

{

AlarmTimer.handle();

return actives[group > STMTD_MAX_GROUPS - 1 ? STMTD_MAX_GROUPS - 1 : group] != 0;

}

bool STMTD::rise(Alarm& alarm)

{

if (limit())

return finish(CAUSE_LIMIT);

output(_port, _polarity ^ HIGH);

if (_run_count + _run_destep >= _run_steps)

{

if (_run_coord < _run_dedlt)

_run_coord = 0;

else

_run_coord -= _run_dedlt;

_run_clock = _minspd - _run_coord / 256;

}

else if (_run_count < _run_acstep)

{

_run_clock = _minspd - _run_coord / 256;

_run_coord += _run_acdlt;

}

else

_run_clock = _run_maxspd;

alarm.handler(fall0).interval(_run_clock >> 1, false);

return true;

}

bool STMTD::fall(Alarm& alarm)

{

if (limit())

return finish(CAUSE_LIMIT);

if (++_run_count >= _run_steps)

return finish(CAUSE_COMPLETE);

output(_port, _polarity ^ LOW);

alarm.handler(rise0).interval(_run_clock - (_run_clock >> 1), false);

return true;

}

bool STMTD::rise0(Alarm& alarm)

{

return ((STMTD *)alarm.param(0))->rise(alarm);

}

bool STMTD::fall0(Alarm& alarm)

{

return ((STMTD *)alarm.param(0))->fall(alarm);

}

bool STMTD::finish([[maybe_unused]] CAUSE cause)

{

output(_port, _polarity ^ LOW);

_run_steps = 0;

--actives[_group];

return false;

}

void STMTD::output(uint16_t port, uint8_t value)

{

digitalWrite(port, value);

}

/*
  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

#include <stdint.h>
#include <stdbool.h>
#include "alarm.h"

#if !defined(DCMTD_MAX_GROUPS)
  #define DCMTD_MAX_GROUPS 32
#endif

class DCMTD
{
  public:

    typedef enum
    {
      CAUSE_COMPLETE,
      CAUSE_LIMIT,
      CAUSE_ABORT,
    } CAUSE;

    DCMTD(uint8_t group, uint16_t port, bool polarity = false);
    virtual ~DCMTD(void);
    virtual void begin(uint32_t clock, uint32_t accel, uint32_t decel, uint8_t maxdc = 100, uint8_t mindc = 0);
    virtual void end(void);
    bool start(int32_t width);
    virtual bool start(int32_t width, uint8_t duty, uint32_t delay = 0);
    virtual void stop(void);
    virtual bool limit(void);
    virtual bool running(void);
    static bool handle(uint8_t group);

  protected:

    bool rise(Alarm& alarm);
    bool fall(Alarm& alarm);
    virtual bool finish(CAUSE cause);
    virtual void output(uint16_t port, uint8_t value);

  private:

    static bool rise0(Alarm& alarm);
    static bool fall0(Alarm& alarm);

    Alarm    _alarm;

    uint8_t  _group;
    uint16_t _port;
    uint8_t  _polarity;
    uint32_t _clock;
    uint32_t _accel;
    uint32_t _decel;
    uint8_t  _maxdc;
    uint8_t  _mindc;

    uint32_t _run_width;
    uint32_t _run_maxdc;
    uint32_t _run_mindc;
    uint32_t _run_accel;
    uint32_t _run_decel;
    uint32_t _run_acdlt;
    uint32_t _run_dedlt;
    uint32_t _run_coord;
    uint32_t _run_clock;
    uint32_t _run_start;
};

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

#include <stdint.h>

#include <stdbool.h>

#include "alarm.h"

#if !defined(DCMTD_MAX_GROUPS)

#define DCMTD_MAX_GROUPS 32

#endif

class DCMTD

{

public:

typedef enum

{

CAUSE_COMPLETE,

CAUSE_LIMIT,

CAUSE_ABORT,

} CAUSE;

DCMTD(uint8_t group, uint16_t port, bool polarity = false);

virtual ~DCMTD(void);

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

virtual void end(void);

bool start(int32_t width);

virtual bool start(int32_t width, uint8_t duty, uint32_t delay = 0);

virtual void stop(void);

virtual bool limit(void);

virtual bool running(void);

static bool handle(uint8_t group);

protected:

bool rise(Alarm& alarm);

bool fall(Alarm& alarm);

virtual bool finish(CAUSE cause);

virtual void output(uint16_t port, uint8_t value);

private:

static bool rise0(Alarm& alarm);

static bool fall0(Alarm& alarm);

Alarm _alarm;

uint8_t _group;

uint16_t _port;

uint8_t _polarity;

uint32_t _clock;

uint32_t _accel;

uint32_t _decel;

uint8_t _maxdc;

uint8_t _mindc;

uint32_t _run_width;

uint32_t _run_maxdc;

uint32_t _run_mindc;

uint32_t _run_accel;

uint32_t _run_decel;

uint32_t _run_acdlt;

uint32_t _run_dedlt;

uint32_t _run_coord;

uint32_t _run_clock;

uint32_t _run_start;

};

/*
  dcmtd.cpp - 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
*/
#include "dcmtd.h"

static uint8_t actives[DCMTD_MAX_GROUPS];

DCMTD::DCMTD(uint8_t group, uint16_t port, bool polarity)
: _group(group)
, _port(port)
, _polarity(polarity)
, _clock(0)
, _accel(0)
, _decel(0)
, _maxdc(0)
, _mindc(0)
, _run_width(0)
{
  if (_group > DCMTD_MAX_GROUPS - 1)
    _group = DCMTD_MAX_GROUPS - 1;
}

DCMTD::~DCMTD(void)
{
}

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

void DCMTD::end(void)
{
  stop();
  pinMode(_port, INPUT);
}

bool DCMTD::start(int32_t width)
{
  return start(width, _maxdc);
}

bool DCMTD::start(int32_t width, uint8_t duty, uint32_t delay)
{
  if ((width <= 0) || _run_width)
    return false;
  ++actives[_group];
  if (duty > 100)
    duty = 100;
  _run_width = width;
  _run_maxdc = (uint64_t)_clock * duty / 100;
  _run_mindc = (uint64_t)_clock * _mindc / 100;
  _run_accel = (uint64_t)_accel * duty / 100;
  _run_decel = (uint64_t)_decel * duty / 100;
  if (_run_maxdc <= _run_mindc)
    _run_accel = _run_decel = 0;
  _run_acdlt = _run_accel ? (uint64_t)(_run_maxdc - _run_mindc) * 256 * _clock / _run_accel : 0;
  _run_dedlt = _run_decel ? (uint64_t)(_run_maxdc - _run_mindc) * 256 * _clock / _run_decel : 0;
  if (_run_width < _run_accel + _run_decel)
    _run_decel = (uint64_t)_run_decel * _run_width / (_run_accel + _run_decel);
  _run_coord = 0;
  AlarmTimer.start(_alarm.param(0, this).handler(rise0).interval(delay ? delay : 1));
  _run_start  = _alarm.alarm();
  return true;
}

void DCMTD::stop(void)
{
  if (AlarmTimer.cancel(_alarm))
    finish(CAUSE_ABORT);
}

bool DCMTD::limit(void)
{
  return false;
}

bool DCMTD::running(void)
{
  return _run_width != 0;
}

bool DCMTD::handle(uint8_t group)
{
  AlarmTimer.handle();
  return actives[group > DCMTD_MAX_GROUPS - 1 ? DCMTD_MAX_GROUPS - 1 : group] != 0;
}

bool DCMTD::rise(Alarm& alarm)
{
  if (limit())
    return finish(CAUSE_LIMIT);
  _run_clock = _alarm.micros() - _run_start;
  if (_run_clock + _run_decel >= _run_width)
  {
    if (_run_coord < _run_dedlt)
      _run_coord = 0;
    else
      _run_coord -= _run_dedlt;
    _run_clock = _run_mindc + _run_coord / 256;
  }
  else if (_run_clock < _run_accel)
  {
    _run_clock = _run_mindc + _run_coord / 256;
    _run_coord += _run_acdlt;
  }
  else
    _run_clock = _run_maxdc;
  if (_run_clock)
    output(_port, _polarity ^ HIGH);
  alarm.handler(fall0).interval(_run_clock, false);
  return true;
}

bool DCMTD::fall(Alarm& alarm)
{
  if (limit())
    return finish(CAUSE_LIMIT);
  if (_alarm.micros() - _run_start + _clock >= _run_width)
    return finish(CAUSE_COMPLETE);
  if (_run_clock < _clock)
    output(_port, _polarity ^ LOW);
  alarm.handler(rise0).interval(_clock - _run_clock, false);
  return true;
}

bool DCMTD::rise0(Alarm& alarm)
{
  return ((DCMTD *)alarm.param(0))->rise(alarm);
}

bool DCMTD::fall0(Alarm& alarm)
{
  return ((DCMTD *)alarm.param(0))->fall(alarm);
}

bool DCMTD::finish([[maybe_unused]] CAUSE cause)
{
  output(_port, _polarity ^ LOW);
  _run_width = 0;
  --actives[_group];
  return false;
}

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

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

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

dcmtd.cpp - 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

#include "dcmtd.h"

static uint8_t actives[DCMTD_MAX_GROUPS];

DCMTD::DCMTD(uint8_t group, uint16_t port, bool polarity)

: _group(group)

, _port(port)

, _polarity(polarity)

, _clock(0)

, _accel(0)

, _decel(0)

, _maxdc(0)

, _mindc(0)

, _run_width(0)

{

if (_group > DCMTD_MAX_GROUPS - 1)

_group = DCMTD_MAX_GROUPS - 1;

}

DCMTD::~DCMTD(void)

{

}

void DCMTD::begin(uint32_t clock, uint32_t accel, uint32_t decel, uint8_t maxdc, uint8_t mindc)

{

end();

_clock = clock;

_accel = accel;

_decel = decel;

_maxdc = maxdc > 100 ? 100 : maxdc;

_mindc = mindc > 100 ? 100 : mindc;

_run_width = 0;

pinMode(_port, OUTPUT);

digitalWrite(_port, _polarity ^ LOW);

}

void DCMTD::end(void)

{

stop();

pinMode(_port, INPUT);

}

bool DCMTD::start(int32_t width)

{

return start(width, _maxdc);

}

bool DCMTD::start(int32_t width, uint8_t duty, uint32_t delay)

{

if ((width <= 0) || _run_width)

return false;

++actives[_group];

if (duty > 100)

duty = 100;

_run_width = width;

_run_maxdc = (uint64_t)_clock * duty / 100;

_run_mindc = (uint64_t)_clock * _mindc / 100;

_run_accel = (uint64_t)_accel * duty / 100;

_run_decel = (uint64_t)_decel * duty / 100;

if (_run_maxdc <= _run_mindc)

_run_accel = _run_decel = 0;

_run_acdlt = _run_accel ? (uint64_t)(_run_maxdc - _run_mindc) * 256 * _clock / _run_accel : 0;

_run_dedlt = _run_decel ? (uint64_t)(_run_maxdc - _run_mindc) * 256 * _clock / _run_decel : 0;

if (_run_width < _run_accel + _run_decel)

_run_decel = (uint64_t)_run_decel * _run_width / (_run_accel + _run_decel);

_run_coord = 0;

AlarmTimer.start(_alarm.param(0, this).handler(rise0).interval(delay ? delay : 1));

_run_start = _alarm.alarm();

return true;

}

void DCMTD::stop(void)

{

if (AlarmTimer.cancel(_alarm))

finish(CAUSE_ABORT);

}

bool DCMTD::limit(void)

{

return false;

}

bool DCMTD::running(void)

{

return _run_width != 0;

}

bool DCMTD::handle(uint8_t group)

{

AlarmTimer.handle();

return actives[group > DCMTD_MAX_GROUPS - 1 ? DCMTD_MAX_GROUPS - 1 : group] != 0;

}

bool DCMTD::rise(Alarm& alarm)

{

if (limit())

return finish(CAUSE_LIMIT);

_run_clock = _alarm.micros() - _run_start;

if (_run_clock + _run_decel >= _run_width)

{

if (_run_coord < _run_dedlt)

_run_coord = 0;

else

_run_coord -= _run_dedlt;

_run_clock = _run_mindc + _run_coord / 256;

}

else if (_run_clock < _run_accel)

{

_run_clock = _run_mindc + _run_coord / 256;

_run_coord += _run_acdlt;

}

else

_run_clock = _run_maxdc;

if (_run_clock)

output(_port, _polarity ^ HIGH);

alarm.handler(fall0).interval(_run_clock, false);

return true;

}

bool DCMTD::fall(Alarm& alarm)

{

if (limit())

return finish(CAUSE_LIMIT);

if (_alarm.micros() - _run_start + _clock >= _run_width)

return finish(CAUSE_COMPLETE);

if (_run_clock < _clock)

output(_port, _polarity ^ LOW);

alarm.handler(rise0).interval(_clock - _run_clock, false);

return true;

}

bool DCMTD::rise0(Alarm& alarm)

{

return ((DCMTD *)alarm.param(0))->rise(alarm);

}

bool DCMTD::fall0(Alarm& alarm)

{

return ((DCMTD *)alarm.param(0))->fall(alarm);

}

bool DCMTD::finish([[maybe_unused]] CAUSE cause)

{

output(_port, _polarity ^ LOW);

_run_width = 0;

--actives[_group];

return false;

}

void DCMTD::output(uint16_t port, uint8_t value)

{

digitalWrite(port, value);

}

/*
  alarm.h - Alarm 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 not, write to the
  Free Software Foundation, Inc., 59 Temple Place, Suite 330,
  Boston, MA  02111-1307  USA
*/
#pragma once

#include <stddef.h>
#include <stdint.h>
#include <stdbool.h>
#include "hardware.h"

class Alarm : public Hardware
{
  public:

    typedef bool (*handler_t)(Alarm& alarm);

    Alarm(void);
    timer_t alarm(void);
    Alarm& interval(timer_t interval, bool fromnow = true);
    timer_t interval(void);
    Alarm& handler(handler_t handler);
    handler_t handler(void);
    Alarm& param(uint8_t num, void *val);
    void *param(uint8_t num);
    bool expire(void);

  protected:

    friend class AlarmClass;

    Alarm    *_next;
    timer_t   _alarm;
    timer_t   _interval;
    handler_t _handler;
    void     *_params[4];
};

class AlarmClass : public Hardware
{
  public:

    AlarmClass(void);
    bool start(Alarm& alarm);
    bool cancel(Alarm& alarm);
    bool cancel(uint8_t num, void *arg);
    void handle(void);

  protected:

    bool add(Alarm& alarm);

  private:

    inline bool timeup(void);

    friend void AlarmClass_timeup(void);

    Alarm *_alarm_list;
};

extern AlarmClass AlarmTimer;

alarm.h - Alarm 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 not, write to the

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

Boston, MA 02111-1307 USA

#pragma once

#include <stddef.h>

#include <stdint.h>

#include <stdbool.h>

#include "hardware.h"

class Alarm : public Hardware

{

public:

typedef bool (*handler_t)(Alarm& alarm);

Alarm(void);

timer_t alarm(void);

Alarm& interval(timer_t interval, bool fromnow = true);

timer_t interval(void);

Alarm& handler(handler_t handler);

handler_t handler(void);

Alarm& param(uint8_t num, void *val);

void *param(uint8_t num);

bool expire(void);

protected:

friend class AlarmClass;

Alarm *_next;

timer_t _alarm;

timer_t _interval;

handler_t _handler;

void *_params[4];

};

class AlarmClass : public Hardware

{

public:

AlarmClass(void);

bool start(Alarm& alarm);

bool cancel(Alarm& alarm);

bool cancel(uint8_t num, void *arg);

void handle(void);

protected:

bool add(Alarm& alarm);

private:

inline bool timeup(void);

friend void AlarmClass_timeup(void);

Alarm *_alarm_list;

};

extern AlarmClass AlarmTimer;

/*
  alarm.cpp - Alarm 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 not, write to the
  Free Software Foundation, Inc., 59 Temple Place, Suite 330,
  Boston, MA  02111-1307  USA
*/
#include <stddef.h>
#include "alarm.h"

#define lengthof(a) (sizeof(a)/sizeof(a[0]))

AlarmClass AlarmTimer;

Alarm::Alarm(void)
{
  _alarm = micros();
}

Alarm::timer_t Alarm::alarm(void)
{
  return _alarm;
}

Alarm& Alarm::interval(Alarm::timer_t interval, bool fromnow)
{
  _interval = interval;
  if (fromnow)
    _alarm = micros() + interval;
  return *this;
}

Alarm::timer_t Alarm::interval(void)
{
  return _interval;
}

Alarm& Alarm::handler(Alarm::handler_t handler)
{
  _handler = handler;
  return *this;
}

Alarm::handler_t Alarm::handler(void)
{
  return _handler;
}

Alarm& Alarm::param(uint8_t num, void *val)
{
  if (num < lengthof(_params))
    _params[num] = val;
  return *this;
}

void *Alarm::param(uint8_t num)
{
  return num < lengthof(_params) ? _params[num] : NULL;
}

bool Alarm::expire(void)
{
  if (micros() - _alarm < 0)
    return false;
  _alarm += _interval;
  return true;
}

AlarmClass::AlarmClass(void)
: _alarm_list(nullptr)
{
  setupAlarmTimer();
}

bool AlarmClass::start(Alarm& alarm)
{
  if ((alarm._interval < 1) || !alarm._handler)
    return false;
  size_t save = disableAndSaveInterrupts();
  if (add(alarm))
    startAlarmTimer(_alarm_list->_alarm - micros());
  restoreInterrupts(save);
  return true;
}

bool AlarmClass::cancel(Alarm& alarm)
{
  bool rv = false;
  size_t save = disableAndSaveInterrupts();
  for (Alarm **p = &_alarm_list; *p; p = &(*p)->_next)
  {
    if (*p == &alarm)
    {
      *p = (*p)->_next;
      rv = true;
      break;
    }
  }
  restoreInterrupts(save);
  return rv;
}

bool AlarmClass::cancel(uint8_t num, void *arg)
{
  bool rv = false;
  if ((num < lengthof(Alarm::_params)) && arg)
  {
    size_t save = disableAndSaveInterrupts();
    for (Alarm **p = &_alarm_list; *p; p = &(*p)->_next)
    {
      if ((*p)->_params[num] == arg)
      {
        *p = (*p)->_next;
        rv = true;
        break;
      }
    }
    restoreInterrupts(save);
  }
  return rv;
}

bool AlarmClass::add(Alarm& alarm)
{
  Alarm **p;
  for (p = &_alarm_list; *p && ((*p)->_alarm - alarm._alarm <= 0); p = &(*p)->_next)
    continue;
  alarm._next = *p;
  *p = &alarm;
  return p == &_alarm_list;
}

bool AlarmClass::timeup(void)
{
  Alarm *p = NULL;
  while (_alarm_list)
  {
    timer_t t = _alarm_list->_alarm - micros();
    if (p || (t > 0))
    {
      startAlarmTimer(t);
      return true;
    }
    _alarm_list = (p = _alarm_list)->_next;
    if (p->_handler(*p))
    {
      p->_alarm += p->_interval;
      if (!add(*p))
        p = NULL;
    }
    else
      p = NULL;
  }
  return false;
}

void AlarmClass::handle(void)
{
  size_t save = disableAndSaveInterrupts();
  timeup();
  restoreInterrupts(save);
}

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

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

alarm.cpp - Alarm 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 not, write to the

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

Boston, MA 02111-1307 USA

#include <stddef.h>

#include "alarm.h"

#define lengthof(a) (sizeof(a)/sizeof(a[0]))

AlarmClass AlarmTimer;

Alarm::Alarm(void)

{

_alarm = micros();

}

Alarm::timer_t Alarm::alarm(void)

{

return _alarm;

}

Alarm& Alarm::interval(Alarm::timer_t interval, bool fromnow)

{

_interval = interval;

if (fromnow)

_alarm = micros() + interval;

return *this;

}

Alarm::timer_t Alarm::interval(void)

{

return _interval;

}

Alarm& Alarm::handler(Alarm::handler_t handler)

{

_handler = handler;

return *this;

}

Alarm::handler_t Alarm::handler(void)

{

return _handler;

}

Alarm& Alarm::param(uint8_t num, void *val)

{

if (num < lengthof(_params))

_params[num] = val;

return *this;

}

void *Alarm::param(uint8_t num)

{

return num < lengthof(_params) ? _params[num] : NULL;

}

bool Alarm::expire(void)

{

if (micros() - _alarm < 0)

return false;

_alarm += _interval;

return true;

}

AlarmClass::AlarmClass(void)

: _alarm_list(nullptr)

{

setupAlarmTimer();

}

bool AlarmClass::start(Alarm& alarm)

{

if ((alarm._interval < 1) || !alarm._handler)

return false;

size_t save = disableAndSaveInterrupts();

if (add(alarm))

startAlarmTimer(_alarm_list->_alarm - micros());

restoreInterrupts(save);

return true;

}

bool AlarmClass::cancel(Alarm& alarm)

{

bool rv = false;

size_t save = disableAndSaveInterrupts();

for (Alarm **p = &_alarm_list; *p; p = &(*p)->_next)

{

if (*p == &alarm)

{

*p = (*p)->_next;

rv = true;

break;

}

restoreInterrupts(save);

return rv;

}

bool AlarmClass::cancel(uint8_t num, void *arg)

{

bool rv = false;

if ((num < lengthof(Alarm::_params)) && arg)

{

size_t save = disableAndSaveInterrupts();

for (Alarm **p = &_alarm_list; *p; p = &(*p)->_next)

{

if ((*p)->_params[num] == arg)

{

*p = (*p)->_next;

rv = true;

break;

}

restoreInterrupts(save);

}

return rv;

}

bool AlarmClass::add(Alarm& alarm)

{

Alarm **p;

for (p = &_alarm_list; *p && ((*p)->_alarm - alarm._alarm <= 0); p = &(*p)->_next)

continue;

alarm._next = *p;

*p = &alarm;

return p == &_alarm_list;

}

bool AlarmClass::timeup(void)

{

Alarm *p = NULL;

while (_alarm_list)

{

timer_t t = _alarm_list->_alarm - micros();

if (p || (t > 0))

{

startAlarmTimer(t);

return true;

}

_alarm_list = (p = _alarm_list)->_next;

if (p->_handler(*p))

{

p->_alarm += p->_interval;

if (!add(*p))

p = NULL;

}

else

p = NULL;

}

return false;

}

void AlarmClass::handle(void)

{

size_t save = disableAndSaveInterrupts();

timeup();

restoreInterrupts(save);

}

/*
  hardware.h - Hardware 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 not, write to the
  Free Software Foundation, Inc., 59 Temple Place, Suite 330,
  Boston, MA  02111-1307  USA
*/
#pragma once

#include <stddef.h>
#include <stdint.h>
#include <stdbool.h>
#include "Arduino.h"

class Hardware
{
  public:

    typedef int32_t timer_t;

    Hardware(void);
    static timer_t micros(void);
    static size_t disableAndSaveInterrupts(void);
    static void restoreInterrupts(size_t save);

  protected:

    /* For Alarm Class */
    static void setupAlarmTimer(void);
    static void startAlarmTimer(timer_t interval);
};

hardware.h - Hardware 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 not, write to the

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

Boston, MA 02111-1307 USA

#pragma once

#include <stddef.h>

#include <stdint.h>

#include <stdbool.h>

#include "Arduino.h"

class Hardware

{

public:

typedef int32_t timer_t;

Hardware(void);

static timer_t micros(void);

static size_t disableAndSaveInterrupts(void);

static void restoreInterrupts(size_t save);

protected:

/* For Alarm Class */

static void setupAlarmTimer(void);

static void startAlarmTimer(timer_t interval);

};

/*
  hardware.cpp - Hardware 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 not, write to the
  Free Software Foundation, Inc., 59 Temple Place, Suite 330,
  Boston, MA  02111-1307  USA
*/
#include "hardware.h"

/*
 * ESP32
 */
#if defined(ESP32)

Hardware::Hardware(void)
{
}

Hardware::timer_t Hardware::micros(void)
{
  return ::micros();
}

size_t Hardware::disableAndSaveInterrupts(void)
{
  return portSET_INTERRUPT_MASK_FROM_ISR();
}

void Hardware::restoreInterrupts(size_t save)
{
  portCLEAR_INTERRUPT_MASK_FROM_ISR(save);
}

void Hardware::setupAlarmTimer(void)
{
  /* Implement timer interrupt processing. */
}

void Hardware::startAlarmTimer([[maybe_unused]] Hardware::timer_t interval)
{
  /* Implement timer interrupt processing. */
}

/*
 * Generic Arduino
 */
#else

static unsigned char nest;

Hardware::Hardware(void)
{
}

Hardware::timer_t Hardware::micros(void)
{
  return ::micros();
}

size_t Hardware::disableAndSaveInterrupts(void)
{
  noInterrupts();
  return nest++;
}

void Hardware::restoreInterrupts(size_t save)
{
  if ((nest = save) == 0)
    interrupts();
}

void Hardware::setupAlarmTimer(void)
{
}

void Hardware::startAlarmTimer([[maybe_unused]] Hardware::timer_t interval)
{
}

#endif

hardware.cpp - Hardware 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 not, write to the

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

Boston, MA 02111-1307 USA

#include "hardware.h"

* ESP32

#if defined(ESP32)

Hardware::Hardware(void)

{

}

Hardware::timer_t Hardware::micros(void)

{

return ::micros();

}

size_t Hardware::disableAndSaveInterrupts(void)

{

return portSET_INTERRUPT_MASK_FROM_ISR();

}

void Hardware::restoreInterrupts(size_t save)

{

portCLEAR_INTERRUPT_MASK_FROM_ISR(save);

}

void Hardware::setupAlarmTimer(void)

{

/* Implement timer interrupt processing. */

}

void Hardware::startAlarmTimer([[maybe_unused]] Hardware::timer_t interval)

{

/* Implement timer interrupt processing. */

}

* Generic Arduino

#else

static unsigned char nest;

Hardware::Hardware(void)

{

}

Hardware::timer_t Hardware::micros(void)

{

return ::micros();

}

size_t Hardware::disableAndSaveInterrupts(void)

{

noInterrupts();

return nest++;

}

void Hardware::restoreInterrupts(size_t save)

{

if ((nest = save) == 0)

interrupts();

}

void Hardware::setupAlarmTimer(void)

{

}

void Hardware::startAlarmTimer([[maybe_unused]] Hardware::timer_t interval)

{

}

#endif

月: 2023年8月

ステッピング・モーター＆ＤＣモーターの台形駆動ライブラリ