diff --git a/lib/forecastnucleoframework/include/forecast/IHardware.hpp b/lib/forecastnucleoframework/include/forecast/IHardware.hpp index e7bf645..a790697 100644 --- a/lib/forecastnucleoframework/include/forecast/IHardware.hpp +++ b/lib/forecastnucleoframework/include/forecast/IHardware.hpp @@ -83,6 +83,15 @@ class IHardware { app.fatal_error("Hardware setStartT() function not implemented"); } + /** + * @brief Set the experiment duration. + * + * @param duration The experiment duration (seconds) + */ + virtual inline void set_duration(float duration) { + app.fatal_error("Hardware set_duration() function not implemented"); + } + /** * @brief Return the start time of the experiment. * @@ -101,6 +110,15 @@ class IHardware { app.fatal_error("Hardware getStartT() function not implemented"); } + /** + * @brief Return the hw duration time. + * + * @return duration_t + */ + virtual inline float get_duration() const { + app.fatal_error("Hardware get_duration() function not implemented"); + } + /** * @brief Return the torque applied by the motor (current feedback) * diff --git a/lib/forecastnucleoframework/include/forecast/platforms/workbench/Hardware.hpp b/lib/forecastnucleoframework/include/forecast/platforms/workbench/Hardware.hpp index 23c883e..d666490 100644 --- a/lib/forecastnucleoframework/include/forecast/platforms/workbench/Hardware.hpp +++ b/lib/forecastnucleoframework/include/forecast/platforms/workbench/Hardware.hpp @@ -43,13 +43,13 @@ class Hardware : public IHardware { logs["dtauS"] = &dtauS; logs["ddtauS"] = &ddtauS; - logs["tauSensor"] = &tauSensor; - logs["dtauSensor"] = &dtauSensor; - logs["ddtauSensor"] = &ddtauSensor; + logs["F"] = &tauSensor; + logs["dF"] = &dtauSensor; + logs["ddF"] = &ddtauSensor; - logs["thetaM"] = &thetaM; - logs["dthetaM"] = &dthetaM; - logs["ddthetaM"] = &ddthetaM; + logs["x"] = &thetaM; + logs["dx"] = &dthetaM; + logs["ddx"] = &ddthetaM; logs["thetaE"] = &thetaE; logs["dthetaE"] = &dthetaE; @@ -110,6 +110,13 @@ class Hardware : public IHardware { */ inline void set_start_time(float time) override { start_t = time; } + /** + * @brief Set the experiment duration. + * + * @param duration The experiment duration (seconds). + */ + inline void set_duration(float duration) override { duration_t = duration; } + /** * @brief Return the start time of the experiment. * @@ -124,7 +131,14 @@ class Hardware : public IHardware { */ virtual inline float get_current_time() const override { return t - start_t; } - /**std::make_unique() + /** + * @brief Return the hw duration time. + * + * @return duration_t + */ + virtual inline float get_duration() const override { return duration_t; } + + /** * @brief Return the torque applied by the motor (current feedback) * * @return motorTorqueFeedback @@ -296,11 +310,14 @@ class Hardware : public IHardware { EsconMotor *env_motor = nullptr; ///< Motor used for the env. simulation AnalogInput *torque_sensor = nullptr; ///< Torque sensor + AnalogInput *load_cell2_sensor = nullptr; ///< Torque sensor utility::AnalogFilter* lowPassTauSensor; + utility::AnalogFilter* lowPassLoacCell2; float t, dt, current_time; float start_t; + float duration_t; float tauM; float dtauM; diff --git a/lib/forecastnucleoframework/include/forecast/platforms/workbench/config/escon_motor.h b/lib/forecastnucleoframework/include/forecast/platforms/workbench/config/escon_motor.h index 4324aa4..fc5e3d6 100644 --- a/lib/forecastnucleoframework/include/forecast/platforms/workbench/config/escon_motor.h +++ b/lib/forecastnucleoframework/include/forecast/platforms/workbench/config/escon_motor.h @@ -13,7 +13,7 @@ namespace motorControl { constexpr float LINMOT_FORCE_MIN = -255.00; constexpr float KT = (LINMOT_FORCE_MAX - LINMOT_FORCE_MIN); constexpr float JM = 1.0000f; - constexpr float offset_bias = -0.015; // LinMot: 0.000 | Moog E024: 0.0042 | 0.000 + constexpr float offset_bias = 0.00; // LinMot: 0.000 | Moog E024: -0.015 constexpr float amp_scale = 1.00; #elif constexpr float MAX_CURR = 3.33f; diff --git a/lib/forecastnucleoframework/src/App.cpp b/lib/forecastnucleoframework/src/App.cpp index 143cffd..540a800 100644 --- a/lib/forecastnucleoframework/src/App.cpp +++ b/lib/forecastnucleoframework/src/App.cpp @@ -566,6 +566,7 @@ bool App::exec_control_loop(unsigned long freq, float duration) { DEBUG_INFO("Executing the control loop at frequency %luHz\n", freq); + hw->set_duration(duration); hw->safety_off(); // putting the hardware in a ready state for the // experiment size_t log_size = hw_logs.size(); diff --git a/lib/forecastnucleoframework/src/platforms/workbench/Hardware.cpp b/lib/forecastnucleoframework/src/platforms/workbench/Hardware.cpp index d011eb9..3fdc5d0 100644 --- a/lib/forecastnucleoframework/src/platforms/workbench/Hardware.cpp +++ b/lib/forecastnucleoframework/src/platforms/workbench/Hardware.cpp @@ -34,6 +34,8 @@ forecast::Status forecast::Hardware::init() { if (not torqueSensorInit()) return Status::TORQUE_SENSOR_INIT_ERR; + load_cell2_sensor = new AnalogInput(PC_1); + lowPassTauSensor = utility::AnalogFilter::getLowPassFilterHz(2.0f); lowPassTauSensor->clean(); @@ -164,23 +166,32 @@ void forecast::Hardware::update(float dt) { float amplitude_voltage(1); - //float center_voltage(LOADCELL_250_OFFSET); - float center_voltage(LOADCELL_5K_OFFSET); + float center_voltage(LOADCELL_250_OFFSET); + //float center_voltage(LOADCELL_5K_OFFSET); - // Board voltage from USB: 3.324 V + // Read Load Cell 1 float signed_voltage = torque_sensor->read_average_float() * 3.324f - center_voltage; if (signed_voltage >= 0.00f) { amplitude_voltage = 3.324 - center_voltage; - tauSensor = signed_voltage/amplitude_voltage * LOADCELL_5K_RANGE; + tauSensor = signed_voltage/amplitude_voltage * LOADCELL_250_RANGE; } else{ amplitude_voltage = center_voltage - 0.00; - tauSensor = signed_voltage/amplitude_voltage * LOADCELL_5K_RANGE; + tauSensor = signed_voltage/amplitude_voltage * LOADCELL_250_RANGE; } +// Read Load Cell 2 + float lc2_signed_voltage = load_cell2_sensor->read_average_float() * 3.324f - 1.749; + if (lc2_signed_voltage >= 0.00f) { + amplitude_voltage = 3.324 -1.749; + tauS = lc2_signed_voltage/amplitude_voltage * LOADCELL_5K_RANGE; + } else{ + amplitude_voltage = 1.749 - 0.00; + tauS = lc2_signed_voltage/amplitude_voltage * LOADCELL_5K_RANGE; + } + tauS = lowPassLoacCell2->process(-tauS, dt); float tauSensor_filt = lowPassTauSensor->process(tauSensor, dt); - tauSensor = tauSensor_filt + 11.0f; // Bias in Newton, hydraulic tests 2023-07-19 - - + tauSensor = tauSensor_filt - 7.0f; // Bias in Newton, hydraulic tests 2023-07-19 + dtauSensor = (tauSensor - prev_tauSensor) / dt; ddtauSensor = (dtauSensor - prev_dtauSensor) / dt; prev_tauSensor = tauSensor; diff --git a/lib/forecastnucleoframework/src/reference_generators/ConstantRefGen.cpp b/lib/forecastnucleoframework/src/reference_generators/ConstantRefGen.cpp index 7b1f048..aacbb4e 100644 --- a/lib/forecastnucleoframework/src/reference_generators/ConstantRefGen.cpp +++ b/lib/forecastnucleoframework/src/reference_generators/ConstantRefGen.cpp @@ -5,5 +5,13 @@ forecast::ConstantRefGen::ConstantRefGen(std::vector v) : values(std::mov } std::vector forecast::ConstantRefGen::process(const IHardware* hw) { - return values; -} + float time = hw->get_current_time(); + float duration = hw->get_duration(); + +if (time > 2.0f && time < (duration - 2)) { + return values; + } + else { + return {0}; + } +} \ No newline at end of file diff --git a/lib/forecastnucleoframework/src/reference_generators/Sinusoid.cpp b/lib/forecastnucleoframework/src/reference_generators/Sinusoid.cpp index e6f8d10..991a822 100644 --- a/lib/forecastnucleoframework/src/reference_generators/Sinusoid.cpp +++ b/lib/forecastnucleoframework/src/reference_generators/Sinusoid.cpp @@ -5,6 +5,6 @@ forecast::SinusoidRefGen::SinusoidRefGen(float frequency, float amplitude) : fre } std::vector forecast::SinusoidRefGen::process(const IHardware* hw) { - float output = (amplitude * (-1)* cos(2 * M_PI * frequency * hw->get_current_time() )) + amplitude; + float output = (amplitude * sin(2 * M_PI * frequency * hw->get_current_time() )); return {output}; } diff --git a/src/main.cpp b/src/main.cpp index 668cee2..5261ba3 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -21,6 +21,7 @@ #include #include #include +#include /** Refgen Headers */ #include @@ -64,9 +65,7 @@ int main() app.get_controller_factory().add("NoController", make_no_controller_builder()); app.get_controller_factory().add("Admittance", make_admittance_control_builder()); app.get_controller_factory().add("Impedance", make_impedance_control_builder()); - //app.get_controller_factory().add("Bypass", make_Bypass_builder()); - - //app.get_operator_factory().add("Sum", make_sum_op_builder()); + app.get_controller_factory().add("Bypass", make_Bypass_builder()); DEBUG_INFO("finished with app\n");