-
Notifications
You must be signed in to change notification settings - Fork 4
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
7 changed files
with
181 additions
and
58 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,74 @@ | ||
| /* | ||
| * Copyright (C) 2022-2023 Dmitry Ponomarev <[email protected]> | ||
| * This Source Code Form is subject to the terms of the Mozilla Public | ||
| * License, v. 2.0. If a copy of the MPL was not distributed with this | ||
| * file, You can obtain one at https://mozilla.org/MPL/2.0/. | ||
| */ | ||
|
|
||
| #include "algorithms.hpp" | ||
|
|
||
|
|
||
| static const RawCommand RAW_COMMAND_MIN = 0; | ||
| static const RawCommand RAW_COMMAND_MAX = 8191; | ||
|
|
||
|
|
||
| static const ActuatorCommandValue ACT_COMMAND_MIN = -1.0f; | ||
| static const ActuatorCommandValue ACT_COMMAND_MAX = 1.0f; | ||
|
|
||
| PwmDurationUs mapRawCommandToPwm(RawCommand command, | ||
| PwmDurationUs min_pwm, | ||
| PwmDurationUs max_pwm, | ||
| PwmDurationUs def_pwm) { | ||
| PwmDurationUs pwm; | ||
| if (command < RAW_COMMAND_MIN || command > RAW_COMMAND_MAX) { | ||
| pwm = def_pwm; | ||
| } else { | ||
| pwm = (PwmDurationUs)mapFloat(command, RAW_COMMAND_MIN, RAW_COMMAND_MAX, min_pwm, max_pwm); | ||
| } | ||
| return pwm; | ||
| } | ||
|
|
||
|
|
||
| PwmDurationUs mapActuatorCommandToPwm(ActuatorCommandValue command, | ||
| PwmDurationUs min_pwm, | ||
| PwmDurationUs max_pwm, | ||
| PwmDurationUs def_pwm) { | ||
| PwmDurationUs pwm; | ||
| if (command < ACT_COMMAND_MIN || command > ACT_COMMAND_MAX) { | ||
| pwm = def_pwm; | ||
| } else { | ||
| pwm = (PwmDurationUs)mapFloat(command, ACT_COMMAND_MIN, ACT_COMMAND_MAX, min_pwm, max_pwm); | ||
| } | ||
| return pwm; | ||
| } | ||
|
|
||
| float mapPwmToPct(uint16_t pwm_val, int16_t pwm_min, int16_t pwm_max) { | ||
| auto max = pwm_max; | ||
| auto min = pwm_min; | ||
| if (pwm_min > pwm_max) { | ||
| max = pwm_min; | ||
| min = pwm_max; | ||
| } | ||
| auto scaled_val = (pwm_val - min) * 100.0f / (max - min); | ||
| auto val = std::clamp(scaled_val, 0.f, 100.f); | ||
| return val; | ||
| } | ||
|
|
||
| float mapFloat(float value, float in_min, float in_max, float out_min, float out_max) { | ||
| float output; | ||
| if (value <= in_min && in_min <= in_max) { | ||
| output = out_min; | ||
| } else if (value >= in_max && in_min <= in_max) { | ||
| output = out_max; | ||
| } else if (out_min == out_max) { | ||
| output = out_min; | ||
| } else { | ||
| output = out_min + (value - in_min) / (in_max - in_min) * (out_max - out_min); | ||
| if (out_min <= out_max) { | ||
| output = std::clamp(output, out_min, out_max); | ||
| } else { | ||
| output = std::clamp(output, out_max, out_min); | ||
| } | ||
| } | ||
| return output; | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,59 @@ | ||
| /* | ||
| * Copyright (C) 2022-2023 Dmitry Ponomarev <[email protected]> | ||
| * This Source Code Form is subject to the terms of the Mozilla Public | ||
| * License, v. 2.0. If a copy of the MPL was not distributed with this | ||
| * file, You can obtain one at https://mozilla.org/MPL/2.0/. | ||
| */ | ||
|
|
||
| #ifndef DEVICES_SERVOS_COMMON_H_ | ||
| #define DEVICES_SERVOS_COMMON_H_ | ||
|
|
||
| #include <stdbool.h> | ||
| #include <stdint.h> | ||
|
|
||
| #include <algorithm> | ||
|
|
||
| #ifdef __cplusplus | ||
| extern "C" { | ||
| #endif | ||
|
|
||
| typedef uint16_t PwmDurationUs; | ||
| typedef int16_t RawCommand; | ||
| typedef float ActuatorCommandValue; | ||
|
|
||
| /** | ||
| * @brief Map raw command value (in interval from 0 to 8191) | ||
| * to PWM duration in us (in interval from min to max) | ||
| * @return pwm_duration if input is correct, | ||
| * def_pwm if raw_command value is less than min or higher than max | ||
| */ | ||
|
|
||
| PwmDurationUs mapRawCommandToPwm(RawCommand rc_value, PwmDurationUs min_pwm, | ||
| PwmDurationUs max_pwm, PwmDurationUs def_pwm); | ||
|
|
||
| /** | ||
| * @brief Map array command value (in interval from -1.0 to 1.0) | ||
| * to PWM duration in us (in interval from min to max) | ||
| * @return pwm_duration if input is correct, | ||
| * def_pwm if raw_command value is less than min or higher than max | ||
| */ | ||
|
|
||
| PwmDurationUs mapActuatorCommandToPwm(ActuatorCommandValue ac_value, | ||
| PwmDurationUs min_pwm, PwmDurationUs max_pwm, | ||
| PwmDurationUs def_pwm); | ||
|
|
||
| /** | ||
| * @brief Map PWM duration in us (in interval from min to max) to pct | ||
| * @return pwm_duration in pct | ||
| */ | ||
|
|
||
| float mapPwmToPct(uint16_t pwm_val, int16_t pwm_min, int16_t pwm_max); | ||
|
|
||
| float mapFloat(float value, float in_min, float in_max, float out_min, | ||
| float out_max); | ||
|
|
||
| #ifdef __cplusplus | ||
| } | ||
| #endif | ||
|
|
||
| #endif // DEVICES_SERVOS_COMMON_H_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.