Skip to content

Commit c30f7cb

Browse files
vegano1vegano1
committed
add SetPressureState gcode
add SetPumpState gcode
1 parent 94dca5f commit c30f7cb

File tree

5 files changed

+200
-17
lines changed

5 files changed

+200
-17
lines changed

‎stm32-modules/include/vacuum-module/vacuum-module/gcodes.hpp‎

Lines changed: 125 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -294,13 +294,80 @@ struct SetStatusBarState {
294294
}
295295
};
296296

297+
struct SetPressureState {
298+
/*
299+
* M120- SetPressureState set state of pressure control (target pressure,
300+
* current pressure)
301+
* */
302+
double pressure = 0;
303+
uint32_t duration_s = 0;
304+
double ramp_rate = 0;
305+
bool vent_after = false;
306+
bool start_pump = false;
307+
308+
using ParseResult = std::optional<SetPressureState>;
309+
static constexpr auto prefix = std::array{'M', '1', '2', '0', ' '};
310+
static constexpr const char* response = "M120 OK\n";
311+
312+
using StartArg = Arg<uint8_t, 'S'>;
313+
using PressureArg = Arg<uint16_t, 'P'>;
314+
using DurationArg = Arg<uint32_t, 'D'>;
315+
using RampArg = Arg<uint16_t, 'R'>;
316+
using VentArg = Arg<uint8_t, 'V'>;
317+
318+
template <typename InputIt, typename Limit>
319+
requires std::forward_iterator<InputIt> &&
320+
std::sized_sentinel_for<Limit, InputIt>
321+
static auto parse(const InputIt& input, Limit limit)
322+
-> std::pair<ParseResult, InputIt> {
323+
auto res =
324+
gcode::SingleParser<StartArg, PressureArg, DurationArg, RampArg,
325+
VentArg>::parse_gcode(input, limit, prefix);
326+
if (!res.first.has_value()) {
327+
return std::make_pair(ParseResult(), input);
328+
}
329+
330+
auto ret = SetPressureState{.pressure = 0.0,
331+
.duration_s = 0,
332+
.ramp_rate = 0.0,
333+
.vent_after = false,
334+
.start_pump = false};
335+
336+
auto arguments = res.first.value();
337+
if (std::get<0>(arguments).present) {
338+
ret.start_pump = static_cast<bool>(std::get<0>(arguments).value);
339+
}
340+
if (std::get<1>(arguments).present) {
341+
ret.pressure = static_cast<double>(std::get<1>(arguments).value);
342+
}
343+
if (std::get<2>(arguments).present) {
344+
ret.duration_s =
345+
static_cast<uint32_t>(std::get<2>(arguments).value);
346+
}
347+
if (std::get<3>(arguments).present) {
348+
ret.ramp_rate = static_cast<double>(std::get<3>(arguments).value);
349+
}
350+
if (std::get<4>(arguments).present) {
351+
ret.vent_after = static_cast<bool>(std::get<4>(arguments).value);
352+
}
353+
return std::make_pair(ret, res.second);
354+
}
355+
356+
template <typename InputIt, typename InLimit>
357+
requires std::forward_iterator<InputIt> &&
358+
std::sized_sentinel_for<InputIt, InLimit>
359+
static auto write_response_into(InputIt buf, InLimit limit) -> InputIt {
360+
return write_string_to_iterpair(buf, limit, response);
361+
}
362+
};
363+
297364
struct GetPressureState {
298365
/*
299-
* M120- GetPressureState get state of pressure control (target pressure,
366+
* M121- GetPressureState get state of pressure control (target pressure,
300367
* current pressure)
301368
* */
302369
using ParseResult = std::optional<GetPressureState>;
303-
static constexpr auto prefix = std::array{'M', '1', '2', '0'};
370+
static constexpr auto prefix = std::array{'M', '1', '2', '1'};
304371

305372
template <typename InputIt, typename InLimit>
306373
requires std::forward_iterator<InputIt> &&
@@ -309,7 +376,7 @@ struct GetPressureState {
309376
double target_pressure,
310377
double current_pressure) -> InputIt {
311378
int res = 0;
312-
res = snprintf(&*buf, (limit - buf), "M120 T:%d C:%d OK\n",
379+
res = snprintf(&*buf, (limit - buf), "M121 T:%d C:%d OK\n",
313380
target_pressure, current_pressure);
314381
if (res <= 0) {
315382
return buf;
@@ -332,12 +399,64 @@ struct GetPressureState {
332399
}
333400
};
334401

402+
struct SetPumpState {
403+
/*
404+
* M122- SetPumpState set state of pump control (start pump, target rpm,
405+
* on/off)
406+
* */
407+
std::optional<double> target_rpm = 0;
408+
std::optional<uint8_t> duty_cycle = 0;
409+
bool start_pump = false;
410+
411+
using ParseResult = std::optional<SetPumpState>;
412+
static constexpr auto prefix = std::array{'M', '1', '2', '2', ' '};
413+
static constexpr const char* response = "M122 OK\n";
414+
415+
using StartArg = Arg<float, 'S'>;
416+
using RPMArg = Arg<uint16_t, 'R'>;
417+
using DutyArg = Arg<uint8_t, 'D'>;
418+
419+
template <typename InputIt, typename Limit>
420+
requires std::forward_iterator<InputIt> &&
421+
std::sized_sentinel_for<Limit, InputIt>
422+
static auto parse(const InputIt& input, Limit limit)
423+
-> std::pair<ParseResult, InputIt> {
424+
auto res = gcode::SingleParser<StartArg, RPMArg, DutyArg>::parse_gcode(
425+
input, limit, prefix);
426+
if (!res.first.has_value()) {
427+
return std::make_pair(ParseResult(), input);
428+
}
429+
430+
auto ret =
431+
SetPumpState{.target_rpm = 0, .duty_cycle = 0, .start_pump = false};
432+
433+
auto arguments = res.first.value();
434+
if (std::get<0>(arguments).present) {
435+
ret.start_pump = static_cast<bool>(std::get<0>(arguments).value);
436+
}
437+
if (std::get<1>(arguments).present) {
438+
ret.target_rpm = static_cast<double>(std::get<1>(arguments).value);
439+
}
440+
if (std::get<2>(arguments).present) {
441+
ret.duty_cycle = static_cast<uint8_t>(std::get<2>(arguments).value);
442+
}
443+
return std::make_pair(ret, res.second);
444+
}
445+
446+
template <typename InputIt, typename InLimit>
447+
requires std::forward_iterator<InputIt> &&
448+
std::sized_sentinel_for<InputIt, InLimit>
449+
static auto write_response_into(InputIt buf, InLimit limit) -> InputIt {
450+
return write_string_to_iterpair(buf, limit, response);
451+
}
452+
};
453+
335454
struct GetPumpState {
336455
/*
337-
* M121- GetPumpState state of the pump (target rpm, current rpm, etc)
456+
* M123- GetPumpState state of the pump (target rpm, current rpm, etc)
338457
* */
339458
using ParseResult = std::optional<GetPumpState>;
340-
static constexpr auto prefix = std::array{'M', '1', '2', '1'};
459+
static constexpr auto prefix = std::array{'M', '1', '2', '3'};
341460

342461
template <typename InputIt, typename InLimit>
343462
requires std::forward_iterator<InputIt> &&
@@ -346,7 +465,7 @@ struct GetPumpState {
346465
double target_rpm, double current_rpm)
347466
-> InputIt {
348467
int res = 0;
349-
res = snprintf(&*buf, (limit - buf), "M121 T:%d C:%d OK\n", target_rpm,
468+
res = snprintf(&*buf, (limit - buf), "M123 T:%d C:%d OK\n", target_rpm,
350469
current_rpm);
351470
if (res <= 0) {
352471
return buf;

‎stm32-modules/include/vacuum-module/vacuum-module/host_comms_task.hpp‎

Lines changed: 57 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -41,11 +41,11 @@ class HostCommsTask {
4141
gcode::GroupParser<gcode::EnterBootloader, gcode::SetSerialNumber,
4242
gcode::GetSystemInfo, gcode::GetResetReason,
4343
gcode::SetStatusBarState, gcode::GetPumpState,
44-
gcode::GetPressureState>;
44+
gcode::GetPressureState, gcode::SetPressureState>;
4545

4646
using AckOnlyCache =
4747
AckCache<8, gcode::EnterBootloader, gcode::SetSerialNumber,
48-
gcode::SetStatusBarState>;
48+
gcode::SetStatusBarState, gcode::SetPressureState>;
4949
using GetSystemInfoCache = AckCache<8, gcode::GetSystemInfo>;
5050
using GetResetReasonCache = AckCache<8, gcode::GetResetReason>;
5151
using GetPumpStateCache = AckCache<8, gcode::GetPumpState>;
@@ -450,6 +450,34 @@ class HostCommsTask {
450450
return std::make_pair(true, tx_into);
451451
}
452452

453+
template <typename InputIt, typename InputLimit>
454+
requires std::forward_iterator<InputIt> &&
455+
std::sized_sentinel_for<InputLimit, InputIt>
456+
auto visit_gcode(const gcode::SetPressureState& gcode, InputIt tx_into,
457+
InputLimit tx_limit) -> std::pair<bool, InputIt> {
458+
auto id = ack_only_cache.add(gcode);
459+
if (id == 0) {
460+
return std::make_pair(
461+
false, errors::write_into(tx_into, tx_limit,
462+
errors::ErrorCode::GCODE_CACHE_FULL));
463+
}
464+
auto message = messages::SetPressureStateMessage{
465+
.id = id,
466+
.pressure_setpoint = gcode.pressure,
467+
.duration_s = gcode.duration_s,
468+
.ramp_rate = gcode.ramp_rate,
469+
.start_pump = gcode.start_pump,
470+
.vent_after = gcode.vent_after,
471+
};
472+
if (!task_registry->send(message, TICKS_TO_WAIT_ON_SEND)) {
473+
auto wrote_to = errors::write_into(
474+
tx_into, tx_limit, errors::ErrorCode::INTERNAL_QUEUE_FULL);
475+
ack_only_cache.remove_if_present(id);
476+
return std::make_pair(false, wrote_to);
477+
}
478+
return std::make_pair(true, tx_into);
479+
}
480+
453481
template <typename InputIt, typename InputLimit>
454482
requires std::forward_iterator<InputIt> &&
455483
std::sized_sentinel_for<InputLimit, InputIt>
@@ -472,6 +500,33 @@ class HostCommsTask {
472500
return std::make_pair(true, tx_into);
473501
}
474502

503+
template <typename InputIt, typename InputLimit>
504+
requires std::forward_iterator<InputIt> &&
505+
std::sized_sentinel_for<InputLimit, InputIt>
506+
auto visit_gcode(const gcode::SetPumpState& gcode, InputIt tx_into,
507+
InputLimit tx_limit) -> std::pair<bool, InputIt> {
508+
auto id = ack_only_cache.add(gcode);
509+
if (id == 0) {
510+
return std::make_pair(
511+
false, errors::write_into(tx_into, tx_limit,
512+
errors::ErrorCode::GCODE_CACHE_FULL));
513+
}
514+
auto message = messages::SetPumpStateMessage{
515+
.id = id,
516+
.from_host = true,
517+
.rpm_setpoint = gcode.target_rpm,
518+
.duty_cycle = gcode.duty_cycle,
519+
.run_pump = gcode.start_pump,
520+
};
521+
if (!task_registry->send(message, TICKS_TO_WAIT_ON_SEND)) {
522+
auto wrote_to = errors::write_into(
523+
tx_into, tx_limit, errors::ErrorCode::INTERNAL_QUEUE_FULL);
524+
ack_only_cache.remove_if_present(id);
525+
return std::make_pair(false, wrote_to);
526+
}
527+
return std::make_pair(true, tx_into);
528+
}
529+
475530
template <typename InputIt, typename InputLimit>
476531
requires std::forward_iterator<InputIt> &&
477532
std::sized_sentinel_for<InputLimit, InputIt>

‎stm32-modules/include/vacuum-module/vacuum-module/messages.hpp‎

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -114,11 +114,11 @@ struct SetStatusBarStateMessage {
114114
struct PressureControlMessage {};
115115
struct PumpControlMessage {};
116116

117-
struct SetTargetPressureMessage {
117+
struct SetPressureStateMessage {
118118
uint32_t id = 0;
119119
double pressure_setpoint = 0;
120-
double ramp_rate = 0;
121120
uint32_t duration_s = 0;
121+
double ramp_rate = 0;
122122
bool start_pump = false;
123123
bool vent_after = false;
124124
};
@@ -129,6 +129,7 @@ struct SetPumpStateMessage {
129129
// This is from inner pressureTask, can we use this from host?
130130
// or do we need something like pwm/duty cycle?
131131
double rpm_setpoint = 0;
132+
uint8_t duty_cycle = 0; // for external use
132133
bool run_pump = false;
133134
};
134135

@@ -167,7 +168,7 @@ using UIMessage = ::std::variant<std::monostate, SetStatusBarStateMessage>;
167168

168169
using PressureMessage =
169170
::std::variant<std::monostate, PressureControlMessage,
170-
SetTargetPressureMessage, GetPressureStateMessage>;
171+
SetPressureStateMessage, GetPressureStateMessage>;
171172

172173
using PumpMessage = ::std::variant<std::monostate, PumpControlMessage,
173174
SetPumpStateMessage, GetPumpStateMessage>;

‎stm32-modules/include/vacuum-module/vacuum-module/pressure_task.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -221,7 +221,7 @@ class PressureTask {
221221
}
222222

223223
template <PressureControlPolicy Policy>
224-
auto visit_message(const messages::SetTargetPressureMessage& m,
224+
auto visit_message(const messages::SetPressureStateMessage& m,
225225
Policy& policy) -> void {
226226
// TODO: Validate incoming values
227227
_pressure_control.target_pressure = m.pressure_setpoint;

‎stm32-modules/include/vacuum-module/vacuum-module/pump_task.hpp‎

Lines changed: 13 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -20,11 +20,16 @@ static constexpr const uint32_t CONTROL_PERIOD_HZ = 100;
2020
static constexpr const uint32_t CONTROL_PERIOD_MS =
2121
(1 / CONTROL_PERIOD_HZ) * 1000;
2222
static constexpr const double MS_TO_SECONDS = 0.001F;
23+
static constexpr const uint8_t MAX_PWM = 100;
24+
static constexpr const double MAX_RPM = 3000;
2325

2426
struct PumpControl {
2527
// NOLINTNEXTLINE(misc-non-private-member-variables-in-classes)
28+
uint8_t current_pwm = 0;
29+
uint8_t target_pwm = 0;
2630
double target_rpm = 0.0F;
2731
double current_rpm = 0.0F;
32+
bool manual_control = false;
2833

2934
// NOLINTNEXTLINE(misc-non-private-member-variables-in-classes)
3035
PID pid; // Current PID loop
@@ -34,6 +39,7 @@ struct PumpControl {
3439
};
3540

3641
const PumpControl pump_control = {
42+
.target_pwm = 0,
3743
.target_rpm = 0.0f,
3844
.pid = PID{.kp = 1,
3945
.ki = 0.5,
@@ -128,11 +134,13 @@ class PumpTask {
128134
auto delta_s = (timestamp - pump_control.last_tick) * MS_TO_SECONDS;
129135
_pump_control.last_tick = timestamp;
130136

131-
// TODO: Do we want ramp gen here for smooth interpolation?
137+
// TODO: Do we want rampgen here for smooth interpolation?
132138
// Compute the new duty cycle
133139
auto current_rpm = policy.get_pump_rpm();
134140
auto difference = _pump_control.target_rpm - current_rpm;
135141
auto duty = _pump_control.pid.compute(difference, delta_s);
142+
duty = std::clamp<uint8_t>(duty, 0, MAX_PWM);
143+
_pump_control.current_pwm = duty;
136144

137145
// set the motor duty cycle
138146
policy.set_pump_duty_cycle(duty);
@@ -143,12 +151,12 @@ class PumpTask {
143151
-> void {
144152
static_cast<void>(m);
145153
static_cast<void>(policy);
146-
147154
// TODO: validate incoming values
148-
_pump_control.target_rpm = m.rpm_setpoint;
155+
_pump_control.target_pwm =
156+
std::clamp<uint8_t>(m.duty_cycle, 0, MAX_PWM);
157+
_pump_control.target_rpm =
158+
std::clamp<double>(m.rpm_setpoint, 0, MAX_RPM);
149159
_pump_control.enable_pump = m.run_pump;
150-
auto timestamp = policy.get_time_ms();
151-
(void)timestamp;
152160

153161
if (!m.run_pump) {
154162
policy.enable_pump_control(false);

0 commit comments

Comments
 (0)