ＤＣモーター／ソレノイドの台形駆動ライブラリを作ってみた。前回投稿のSoftwarePWMの精度は高いが計算負荷が高すぎて周波数が出せなかったので、今度は可能な限り高速化してみた。
arm/48MHzなら100KHzでも動作可能。駆動時間が加速時間＋減速時間以上で台形駆動、それ未満では三角駆動となる。
汎用的にするため信号出力のみとしているが、output()/handle()をオーバーライドすることでより複雑な制御を行うこともできる。
【台形駆動の波形】
【サンプル・スケッチ】
|
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 |
#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); } |
【ライブラリ】
|
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 130 131 132 133 134 135 136 137 138 139 140 141 142 143 |
/* 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; }; |