-
Notifications
You must be signed in to change notification settings - Fork 0
/
motor_interval.h
292 lines (265 loc) · 11 KB
/
motor_interval.h
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
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
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
// motor_interval.h
#pragma once
#include "scheduler.h"
#include "switch.h"
#include "mup_util.h"
#ifndef MQTT_MAX_PACKET_SIZE
#define MAX_MESSAGE_SIZE 512
#else
#define MAX_MESSAGE_SIZE MQTT_MAX_PACKET_SIZE
#endif
namespace ustd {
class MotorInterval {
public:
/*! MotorInterval implements a simple dc motor whose advance is controlled
* by a sensor switch triggered by a notch wheel.
*
* The connection requires one digital ouput GPIO pin for the motor driver
* and one digital input pin for the sensor switch.
*
* It is possible to start the motor and optionally specify how many
* intervals shall the motor advance before automatically stopping. It is
* also possible to explicitely stop the motor before the requested number
* of intervals will be reached. The current interval will be completed
* before stopping. During motion, it is also possible to change the number
* of intervals requested.
*
* A security mechanism performs a hard emergency stop if the sensor switch
* does not report any activity during a specified timeout.
*/
String MOTORINTERVAL_VERSION = "0.3.0";
Scheduler *pSched;
int tID;
String name;
uint8_t motorPort;
uint8_t sensorPort;
unsigned long sensorTimeout;
unsigned maxIntervals = UINT_MAX;
unsigned defaultIntervals = UINT_MAX;
bool motorActiveLogic = false;
bool sensorActiveLogic = false;
bool state;
unsigned long startTime = millis();
unsigned long stopTime = 0;
unsigned long lastTime = 0;
unsigned target_interval = 0;
unsigned current_interval = 0;
String lastResult = "Not initialized";
Switch intervalSensor;
MotorInterval(String name, uint8_t motorPort, uint8_t sensorPort, unsigned long sensorTimeout,
bool motorActiveLogic = false, bool sensorActiveLogic = false)
: name(name), motorPort(motorPort), sensorPort(sensorPort), sensorTimeout(sensorTimeout),
motorActiveLogic(motorActiveLogic), sensorActiveLogic(sensorActiveLogic),
intervalSensor(name + ".sensor", sensorPort, ustd::Switch::Mode::Falling,
sensorActiveLogic) {
/*! Instantiate an Interval Motor
* @param name The name of the entity
* @param motorPort The pin identifier for the digital output GPIO port that
* controls the motor
* @param sensorPort The pin identifier for the digital input GPIO port connected to
* the swicth triggered at each interval
* @param sensorTimeout The timeout in milliseconds for the motor to advance for one
* interval. If no sensor input is received, the motor will
* perform an emergecy stop and report an error
* @param motorActiveLogic If true, the motor logic is inverted (motor on on LOW).
* Default is false
* @param sensorActiveLogic If true, the sensor logic is inverted. Default is false
*/
}
~MotorInterval() {
}
void setSensorTimeout(unsigned long ms) {
/*! Change the sensor timeout
* @param ms The timeout in milliseconds for the motor to advance for one interval. If no
* sensor input is received, the motor will perform an emergecy stop and report
* an error
*/
sensorTimeout = ms;
}
void setDefaultIntervals(unsigned intervals) {
/*! Change the number of default intervals to advance when a start command is issued without
* specifying a number of intervals
* @param intervals The default number of intervals that the motor will advance on a start
* command without a specified number of intervals. If the speicifed value
* is higher than the value of maxIntervals, the value new value is capped
* to maxIntervals.
*/
defaultIntervals = intervals > maxIntervals ? maxIntervals : intervals;
}
void setMaxIntervals(unsigned intervals) {
/*! Change the number of maximum intervals that the motor can advance
* @param intervals Set the limit of the maximum number of intervals that the motor can
* advance. If the value is lower as defaultIntervals, defaultIntervals
* will be capped to the new value.
*/
maxIntervals = intervals;
if (defaultIntervals > maxIntervals) {
defaultIntervals = maxIntervals;
}
}
void begin(Scheduler *_pSched) {
/*! Initialize the interval motor mupplet
*
* @param _pSched Pointer to the application's scheduler
*/
pSched = _pSched;
lastResult = "OK";
pinMode(motorPort, OUTPUT);
pinMode(sensorPort, INPUT);
stopMotor();
tID = pSched->add([this]() { this->loop(); }, name, 50000);
pSched->subscribe(tID, name + "/switch/#",
[this](String topic, String msg, String originator) {
this->commandMsg(topic, msg, originator);
});
pSched->subscribe(tID, name + ".sensor/switch/#",
[this](String topic, String msg, String originator) {
this->sensorMsg(topic, msg, originator);
});
intervalSensor.begin(_pSched);
pSched->publish(name + "/switch/result", lastResult.c_str());
}
void start(unsigned intervals = UINT_MAX) {
/*! Starts the motor
* @param intervals Number of intervals before automatically stopping the motor. Default is
* the defaultInterval setting.
*
* If the motor is already running, the number of intervals to advance is changed to the
* specified value. By specifing a value equal or lesser than the value of the already
* advanced intervals, the motor will stop as soon as the current interval has been
* completed.
*/
set(intervals);
}
void stop(bool hardstop = false) {
/*! Stops the motor when the current interval is finished
*
* @param hardstop If true, the motor is stopped immediately without waiting for the
* current interval to complete. In this case the reported number of
* intervals will only include the last fully completed interval. If the
* motor is restarted the next interval will be shorter since the motor
* starts at a intermediate position. Default is false
*/
if (state && hardstop) {
// stop immediately
stopMotor();
publishState();
} else if (state) {
// stop at next finished interval
target_interval = current_interval + 1;
publishf(name + "/switch/requestedintervals", "%u", target_interval);
}
}
void set(unsigned intervals) {
/*! Controls the motor
* @param intervals Number of intervals before automatically stopping the motor.
*
* If the motor is already running, the number of intervals to advance is changed to the
* specified value. By specifing a value equal or lesser than the vallue of the already
* advanced intervals, the motor will stop as soon as the current interval has been
* completed.
*/
if (state) {
if (intervals) {
target_interval = intervals == UINT_MAX ? defaultIntervals : intervals;
publishf(name + "/switch/requestedintervals", "%u", target_interval);
} else {
stop();
}
} else {
if (intervals) {
target_interval = intervals == UINT_MAX ? defaultIntervals : intervals;
current_interval = 0;
lastResult = "OK";
startMotor();
pSched->publish(name + "/switch/state", "on");
publishf(name + "/switch/requestedintervals", "%u", target_interval);
}
}
}
void publishState() {
pSched->publish(name + "/switch/state", state ? "on" : "off");
pSched->publish(name + "/switch/result", lastResult.c_str());
publishf(name + "/switch/requestedintervals", "%u", target_interval);
publishf(name + "/switch/intervals", "%u", current_interval);
publishf(name + "/switch/duration", "%lu", timeDiff(startTime, stopTime));
}
virtual void loop() {
if (state && timeDiff(lastTime, millis()) > sensorTimeout) {
// sensor broken or movement stuck
lastResult = "No feedback from motor sensor. Please check device.";
stop(true);
}
}
protected:
void publishf(String topic, String format, ...) {
va_list args;
va_start(args, format);
vpublishf(topic, format.c_str(), args);
va_end(args);
}
void publishf(String topic, const char *format, ...) {
va_list args;
va_start(args, format);
vpublishf(topic, format, args);
va_end(args);
}
void vpublishf(String topic, const char *format, va_list args) {
char buffer[MAX_MESSAGE_SIZE];
vsnprintf(buffer, MAX_MESSAGE_SIZE - 1, format, args);
buffer[MAX_MESSAGE_SIZE - 1] = 0;
pSched->publish(topic, buffer);
}
void commandMsg(String topic, String msg, String originator) {
msg.toLowerCase();
if (topic == name + "/switch/set") {
if (msg == "on") {
start();
} else if (msg == "off") {
stop();
} else {
int value = atoi(msg.c_str());
if (value > 0) {
start(value);
} else if (value == 0) {
stop();
} else {
// negative value -> hard stop!
stop(true);
}
}
}
};
void sensorMsg(String topic, String msg, String originator) {
if (originator != "mqtt" && topic == name + ".sensor/switch/state") {
if (msg == "trigger") {
lastTime = millis();
++current_interval;
if (current_interval >= target_interval) {
stopMotor();
publishState();
}
}
}
}
void startMotor() {
state = true;
startTime = millis();
stopTime = lastTime = startTime;
if (motorActiveLogic) {
digitalWrite(motorPort, true);
} else {
digitalWrite(motorPort, false);
}
}
void stopMotor() {
state = false;
stopTime = millis();
if (!motorActiveLogic) {
digitalWrite(motorPort, true);
} else {
digitalWrite(motorPort, false);
}
}
};
} // namespace ustd