diff --git a/src/conf/datatypes.h b/src/conf/datatypes.h index 14a39bf..51275a3 100644 --- a/src/conf/datatypes.h +++ b/src/conf/datatypes.h @@ -148,6 +148,19 @@ typedef struct { CfgHwLeds leds; } CfgHardware; +typedef struct { + uint16_t frequency; + float voltage; +} CfgHapticTone; + +typedef struct { + float duty_solid_threshold; + CfgHapticTone duty; + CfgHapticTone error; + CfgHapticTone vibrate; + uint16_t tone_length; +} CfgHapticFeedback; + typedef struct { float version; bool disabled; @@ -250,6 +263,8 @@ typedef struct { float surge_duty_start; float surge_angle; + CfgHapticFeedback haptic; + CfgLeds leds; CfgHardware hardware; } RefloatConfig; diff --git a/src/conf/settings.xml b/src/conf/settings.xml index 5ea47bb..af5b3c4 100644 --- a/src/conf/settings.xml +++ b/src/conf/settings.xml @@ -956,6 +956,182 @@ p, li { white-space: pre-wrap; } ERPM 3 + + Duty Cycle Frequency + 2 + 1 + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Audible frequency of the Duty Cycle haptic feedback.</p></body></html> + CFG_DFLT_HAPTIC_DUTY_FREQUENCY + 1 + 0 + 1000 + 200 + 0 + 10 + 495 + Hz + 3 + + + Duty Cycle Strength + 1 + 1 + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Strength of the audible Duty Cycle haptic feedback, in Volts.<span style=" font-style:italic;"><br /><br />Recommended Values: 1.0 - 6.0 (Caution with higher values!)</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-style:italic;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-style:italic;">Set to 0.0 to disable.</span></p></body></html> + CFG_DFLT_HAPTIC_DUTY_VOLTAGE + 1 + 1 + 0 + 10 + 0 + 0 + 0.2 + 3 + 10 + V + 7 + + + Duty Cycle Solid Threshold + 1 + 1 + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Duty Cycle threshold to trigger solid tone haptic feedback.</p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-style:italic;">Set to 100% to disable.</span></p></body></html> + CFG_DFLT_HAPTIC_DUTY_SOLID_THRESHOLD + 2 + 1 + 1 + 1 + 0 + 0 + 0.01 + 0.85 + 1000 + + 7 + + + Error Frequency + 2 + 1 + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Audible frequency of the Error haptic feedback.</p></body></html> + CFG_DFLT_HAPTIC_ERROR_FREQUENCY + 1 + 0 + 1000 + 200 + 0 + 10 + 495 + Hz + 3 + + + Error Strength + 1 + 1 + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Strength of the audible Error haptic feedback, in Volts.<span style=" font-style:italic;"><br /><br />Recommended Values: 1.0 - 6.0 (Caution with higher values!)</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-style:italic;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-style:italic;">Set to 0.0 to disable.</span></p></body></html> + CFG_DFLT_HAPTIC_ERROR_VOLTAGE + 1 + 1 + 0 + 10 + 0 + 0 + 0.2 + 3 + 10 + V + 7 + + + Vibrate Frequency + 2 + 1 + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Vibrating frequency of both Duty Cycle and Error haptic feedback.</p></body></html> + CFG_DFLT_HAPTIC_VIBRATE_FREQUENCY + 1 + 0 + 200 + 10 + 0 + 5 + 120 + Hz + 3 + + + Vibrate Strength + 1 + 1 + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Strength of the vibrating haptic feedback for both Duty Cycle and Error, in Volts.<span style=" font-style:italic;"><br /><br />Recommended Values: 1.0 - 6.0 (Caution with higher values!)</span></p> +<p style="-qt-paragraph-type:empty; margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px; font-style:italic;"><br /></p> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;"><span style=" font-style:italic;">Set to 0.0 to disable the vibrating frequency for haptic alerts.</span></p></body></html> + CFG_DFLT_HAPTIC_VIBRATE_VOLTAGE + 1 + 1 + 0 + 10 + 0 + 0 + 0.2 + 0 + 10 + V + 7 + + + Tone Length + 2 + 1 + <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> +<html><head><meta name="qrichtext" content="1" /><style type="text/css"> +p, li { white-space: pre-wrap; } +</style></head><body style=" font-family:'Roboto'; ; font-weight:400; font-style:normal;"> +<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">Length of the haptic feedback beeps in milliseconds.</p></body></html> + CFG_DFLT_HAPTIC_TONE_LENGTH + 1 + 0 + 2000 + 50 + 0 + 50 + 200 + ms + 3 + Nose Angling Speed 1 @@ -3564,6 +3740,14 @@ p, li { white-space: pre-wrap; } dark_pitch_offset is_beeper_enabled disabled + haptic.duty.voltage + haptic.duty.frequency + haptic.duty_solid_threshold + haptic.error.voltage + haptic.error.frequency + haptic.vibrate.voltage + haptic.vibrate.frequency + haptic.tone_length @@ -3703,6 +3887,22 @@ p, li { white-space: pre-wrap; } tiltback_lv_speed + + Haptic Feedback (6.05+) + + ::sep::Duty + haptic.duty.voltage + haptic.duty.frequency + haptic.duty_solid_threshold + ::sep::Error + haptic.error.voltage + haptic.error.frequency + ::sep::General + haptic.vibrate.voltage + haptic.vibrate.frequency + haptic.tone_length + + Remote diff --git a/src/haptic_feedback.c b/src/haptic_feedback.c new file mode 100644 index 0000000..6335ead --- /dev/null +++ b/src/haptic_feedback.c @@ -0,0 +1,127 @@ +// Copyright 2024 Lukas Hrazky +// +// This file is part of the Refloat VESC package. +// +// Refloat VESC package is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by the +// Free Software Foundation, either version 3 of the License, or (at your +// option) any later version. +// +// Refloat VESC package 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 General Public License for +// more details. +// +// You should have received a copy of the GNU General Public License along with +// this program. If not, see . + +#include "haptic_feedback.h" + +#include "vesc_c_if.h" + +#include + +void haptic_feedback_init(HapticFeedback *hf) { + hf->type_playing = HAPTIC_FEEDBACK_NONE; + hf->start_time = 0.0f; + hf->is_playing = false; +} + +void haptic_feedback_configure(HapticFeedback *hf, const CfgHapticFeedback *cfg) { + hf->cfg = cfg; +} + +static HapticFeedbackType state_to_haptic_type(const State *state) { + if (state->state != STATE_RUNNING) { + return HAPTIC_FEEDBACK_NONE; + } + + switch (state->sat) { + case SAT_PB_DUTY: + return HAPTIC_FEEDBACK_DUTY; + case SAT_PB_TEMPERATURE: + return HAPTIC_FEEDBACK_ERROR_TEMPERATURE; + case SAT_PB_LOW_VOLTAGE: + case SAT_PB_HIGH_VOLTAGE: + return HAPTIC_FEEDBACK_ERROR_VOLTAGE; + default: + return HAPTIC_FEEDBACK_NONE; + } +} + +// Returns the number of "beats" per period of a given tone. Tones are played +// on even beats and if there are more than two beats, the last beat is +// skipped, giving a certain number of "beeps" followed by a pause. +static uint8_t get_beats(HapticFeedbackType type) { + switch (type) { + case HAPTIC_FEEDBACK_DUTY: + return 2; + case HAPTIC_FEEDBACK_DUTY_CONTINUOUS: + return 0; + case HAPTIC_FEEDBACK_ERROR_TEMPERATURE: + return 6; + case HAPTIC_FEEDBACK_ERROR_VOLTAGE: + return 8; + case HAPTIC_FEEDBACK_NONE: + break; + } + + return 0; +} + +static const CfgHapticTone *get_haptic_tone(const HapticFeedback *hf) { + switch (hf->type_playing) { + case HAPTIC_FEEDBACK_DUTY: + case HAPTIC_FEEDBACK_DUTY_CONTINUOUS: + return &hf->cfg->duty; + case HAPTIC_FEEDBACK_ERROR_TEMPERATURE: + case HAPTIC_FEEDBACK_ERROR_VOLTAGE: + return &hf->cfg->error; + case HAPTIC_FEEDBACK_NONE: + break; + } + + return 0; +} + +void haptic_feedback_update( + HapticFeedback *hf, const State *state, float duty_cycle, float current_time +) { + if (!VESC_IF->foc_play_tone) { + return; + } + + HapticFeedbackType type_to_play = state_to_haptic_type(state); + if (type_to_play == HAPTIC_FEEDBACK_DUTY && duty_cycle > hf->cfg->duty_solid_threshold) { + type_to_play = HAPTIC_FEEDBACK_DUTY_CONTINUOUS; + } + + float tone_length = hf->cfg->tone_length * 0.001f; + + if (type_to_play != hf->type_playing && current_time - hf->start_time > tone_length) { + hf->type_playing = type_to_play; + hf->start_time = current_time; + } + + bool should_be_playing = false; + if (hf->type_playing != HAPTIC_FEEDBACK_NONE) { + uint8_t beats = get_beats(hf->type_playing); + float period = tone_length * beats; + float time = fmodf(current_time - hf->start_time, period); + uint8_t beat = floorf(time / tone_length); + uint8_t off_beat = beats > 2 ? beats - 2 : 0; + + should_be_playing = beats == 0 || (beat % 2 == 0 && (off_beat == 0 || beat != off_beat)); + } + + if (hf->is_playing && !should_be_playing) { + VESC_IF->foc_play_tone(0, 1, 0.0f); + VESC_IF->foc_play_tone(1, 1, 0.0f); + hf->is_playing = false; + } else if (!hf->is_playing && should_be_playing) { + const CfgHapticTone *tone = get_haptic_tone(hf); + VESC_IF->foc_play_tone(0, tone->frequency, tone->voltage); + VESC_IF->foc_play_tone(1, hf->cfg->vibrate.frequency, hf->cfg->vibrate.voltage); + hf->is_playing = true; + } +} diff --git a/src/haptic_feedback.h b/src/haptic_feedback.h new file mode 100644 index 0000000..ff9b877 --- /dev/null +++ b/src/haptic_feedback.h @@ -0,0 +1,46 @@ +// Copyright 2024 Lukas Hrazky +// +// This file is part of the Refloat VESC package. +// +// Refloat VESC package is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by the +// Free Software Foundation, either version 3 of the License, or (at your +// option) any later version. +// +// Refloat VESC package 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 General Public License for +// more details. +// +// You should have received a copy of the GNU General Public License along with +// this program. If not, see . + +#pragma once + +#include "conf/datatypes.h" +#include "footpad_sensor.h" +#include "state.h" + +typedef enum { + HAPTIC_FEEDBACK_NONE = 0, + HAPTIC_FEEDBACK_DUTY, + HAPTIC_FEEDBACK_DUTY_CONTINUOUS, + HAPTIC_FEEDBACK_ERROR_TEMPERATURE, + HAPTIC_FEEDBACK_ERROR_VOLTAGE, +} HapticFeedbackType; + +typedef struct { + const CfgHapticFeedback *cfg; + + HapticFeedbackType type_playing; + float start_time; + bool is_playing; +} HapticFeedback; + +void haptic_feedback_init(HapticFeedback *hf); + +void haptic_feedback_configure(HapticFeedback *hf, const CfgHapticFeedback *cfg); + +void haptic_feedback_update( + HapticFeedback *hf, const State *state, float duty_cycle, float current_time +); diff --git a/src/main.c b/src/main.c index f0273f7..e5ff842 100644 --- a/src/main.c +++ b/src/main.c @@ -24,6 +24,7 @@ #include "atr.h" #include "charging.h" #include "footpad_sensor.h" +#include "haptic_feedback.h" #include "lcm.h" #include "leds.h" #include "motor_data.h" @@ -184,6 +185,8 @@ typedef struct { float rc_current_target; float rc_current; + HapticFeedback haptic_feedback; + Konami flywheel_konami; } data; @@ -256,6 +259,7 @@ static void reconfigure(data *d) { balance_filter_configure(&d->balance_filter, &d->float_conf); torque_tilt_configure(&d->torque_tilt, &d->float_conf); atr_configure(&d->atr, &d->float_conf); + haptic_feedback_configure(&d->haptic_feedback, &d->float_conf.haptic); } static void configure(data *d) { @@ -1195,6 +1199,10 @@ static void refloat_thd(void *arg) { float new_pid_value = 0; + haptic_feedback_update( + &d->haptic_feedback, &d->state, d->motor.duty_cycle, d->current_time + ); + // Control Loop State Logic switch (d->state.state) { case (STATE_STARTUP): @@ -1598,6 +1606,7 @@ static void data_init(data *d) { d->odometer = VESC_IF->mc_get_odometer(); + haptic_feedback_init(&d->haptic_feedback); lcm_init(&d->lcm, &d->float_conf.hardware.leds); charging_init(&d->charging); }