Page MenuHomec4science
Files F60083387

controller.c
No OneTemporary
Actions

File Metadata

Created
Sat, Apr 27, 08:58

controller.c
View Options

#include "controller.h"
#include "communication.h"
#include "drivers/adc.h"
#include "drivers/dac.h"
#include "drivers/debug_gpio.h"
#include "drivers/incr_encoder.h"
#include "drivers/hall.h"
#include "drivers/h_bridge.h"
#include "drivers/callback_timers.h"
#include "lib/basic_filter.h"
#include "lib/pid.h"
#include "lib/utils.h"
volatile uint32_t ctrl_timestamp; // [us].
volatile float32_t ctrl_hallVoltage; // [V].
volatile float32_t ctrl_motorPosCod; // Motor position measured by the incremental encoder [deg].
volatile float32_t ctrl_motorTorque_Nm_c; // Desired motor torque [N.m].
volatile pid_Pid ctrl_currentPid;
bool ctrl_regulateCurrent;
void ctrl_RegulateCurrent(void);
void ctrl_RegulatePosition(void);
/**
* @brief Initialize the position and current controllers.
*/
void ctrl_Init(void)
{
ctrl_timestamp = 0.0f;
ctrl_motorTorque_Nm_c = 0.0f;
// By default the current regulator is off, to allow the calibration of the
// current sensor.
ctrl_regulateCurrent = false;
// Setup the PIDs.
pid_Init((pid_Pid*)&ctrl_currentPid, KP_CURRENT_DEFAULT_VAL,
KI_CURRENT_DEFAULT_VAL, KD_CURRENT_DEFAULT_VAL,
CURRENT_INTEGRATOR_SAT_DEFAULT_VAL, FF_CURRENT_DEFAULT_VAL);
// Make the timers call the regulation functions periodically.
cbt_SetCurrentLoopTimer(ctrl_RegulateCurrent, TE_CURRENT_LOOP_DEFAULT_VAL);
cbt_SetPositionLoopTimer(ctrl_RegulatePosition, TE_CONTROL_LOOP_DEFAULT_VAL);
// Share some variables with the computer.
comm_monitorFloat("actual_current [A]", (float32_t*)&ctrl_currentPid.current, READONLY);
comm_monitorFloat("target_current [A]", (float32_t*)&ctrl_currentPid.target, READONLY);
comm_monitorFloat("actual_position [deg]", (float32_t*)&ctrl_motorPosCod, READONLY);
comm_monitorFloat("motor_torque [N.m]", (float32_t*)&ctrl_motorTorque_Nm_c, READWRITE);
comm_monitorFloat("encoder_pos [deg]", (float32_t*)&ctrl_motorPosCod, READONLY);
comm_monitorFloat("hall_voltage [V]", (float32_t*)&ctrl_hallVoltage, READONLY);
}
/**
* @brief Start the current regulation.
*/
void ctrl_StartCurrentLoop(void)
{
ctrl_regulateCurrent = true;
}
/**
* @brief Current regulation "loop" function.
*/
void ctrl_RegulateCurrent()
{
float32_t motorVoltage, // Motor command voltage [V].
pwmNormalizedDutyCycle, // Motor normalized PWM duty (-1 ot 1).
targetCurrent; // [A].
float32_t motorCurrentCurrent; // [A].
// Get the current.
motorCurrentCurrent = adc_GetCurrent();
// Convert the target torque to current.
targetCurrent = ctrl_motorTorque_Nm_c / MOTOR_TORQUE_CONST;
// Regulate.
motorVoltage = -pid_Step((pid_Pid*)&ctrl_currentPid,
motorCurrentCurrent,
targetCurrent,
(float32_t)cbt_GetCurrentLoopPeriod()*MICROSECOND_TO_SECOND);
// Normalize to get a signed PWM duty (between -1 and 1).
pwmNormalizedDutyCycle = motorVoltage / H_BRIDGE_SUPPLY_VOLTAGE;
utils_SaturateF(&pwmNormalizedDutyCycle, -CURRENT_LOOP_PWM_MAX_DUTY_CYCLE, CURRENT_LOOP_PWM_MAX_DUTY_CYCLE);
// Apply the computed PWM duty, if the current regulation is enabled.
if(ctrl_regulateCurrent)
hb_SetPWM(pwmNormalizedDutyCycle);
else
hb_SetPWM(0.0f);
}
/**
* @brief Position regulation "loop" function.
* @note As an example, a basic position regulator that uses the encoder is
* implemented. Feel free to remove all the content of the function to add your
* own code.
*/
void ctrl_RegulatePosition()
{
// Increment the timestamp.
ctrl_timestamp += cbt_GetPositionLoopPeriod();
// Get the Hall sensor position.
ctrl_hallVoltage = hall_Get();
// Get the encoder position.
ctrl_motorPosCod = enc_GetPosition();
// Compute the motor torque.
//ctrl_motorTorque_Nm_c = 0.0f;
// Saturate the torque to the motor nominal.
utils_SaturateF((float32_t*)&ctrl_motorTorque_Nm_c, -MOTOR_NOMINAL_TORQUE,
MOTOR_NOMINAL_TORQUE);
}

Event Timeline

c4science · Help