台形駆動機能がないステッピング・モーター・ドライバーなどのために台形駆動ライブラリを作ってみた。
arm/48MHzなら100KHzでも動作可能。駆動ステップ数が加速ステップ数＋減速ステップ数以上で台形駆動、それ未満では三角駆動となる。
汎用的にするため信号出力のみとしているが、output()/handle()をオーバーライドすることでより複雑な制御を行うこともできる。
【台形駆動の波形(10KHz-100KHz)】
【サンプル・スケッチ】
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 |
#include "stmtd.h" #define PIN_MOTOR 0 STMTD stepper(PIN_MOTOR); void setup() { /* 100Hz-1KHzで台形駆動(加速期間:100ステップ, 減速期間:100ステップ)する設定 */ stepper.begin(1000, 10000, 100, 100); } void loop() { /* 1000ステップ駆動する */ stepper.run(1000); } |
【ライブラリ】
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 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 |
/* 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 class STMTD { public: STMTD(uint16_t port, uint8_t polarity = 0) : _port(port) , _polarity(polarity) , _maxspd(0) , _minspd(0) , _acstep(0) , _destep(0) , _status(0) { } virtual ~STMTD(void) { } void begin(uint32_t maxspd, uint32_t minspd, uint32_t acstep, uint32_t destep) { _maxspd = maxspd; _minspd = minspd; _acstep = acstep; _destep = destep; pinMode(_port, OUTPUT); } void end(void) { pinMode(_port, INPUT); output(_port, _polarity); } uint8_t run(uint32_t steps) { return run(steps, _maxspd); } uint8_t run(uint32_t steps, uint32_t maxspd) { _status = 0; uint32_t minspd = _minspd; uint32_t acstep = minspd > _maxspd ? _acstep * (minspd - maxspd) / (minspd - _maxspd) : 0; uint32_t destep = minspd > _maxspd ? _destep * (minspd - maxspd) / (minspd - _maxspd) : 0; if (minspd <= maxspd) acstep = destep = 0; uint32_t acdlt = acstep ? (minspd - maxspd) * 256 / acstep : 0; uint32_t dedlt = destep ? (minspd - maxspd) * 256 / destep : 0; if (steps < acstep + destep) destep = destep * steps / (acstep + destep); uint32_t coord = 0; uint32_t clock = micros(); for (uint32_t t, i = 0; handle(), (_status == 0) && (i < steps); ++i) { output(_port, _polarity ^ HIGH); if (i + destep >= steps) { if (coord < dedlt) coord = 0; else coord -= dedlt; t = minspd - coord / 256; } else if (i < acstep) { t = minspd - coord / 256; coord += acdlt; } else t = maxspd; while (micros() - clock < t >> 1) continue; output(_port, _polarity ^ LOW); while (micros() - clock < t) continue; clock += t; } 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 _maxspd; uint32_t _minspd; uint32_t _acstep; uint32_t _destep; volatile uint8_t _status; }; |