Homec4science
Diffusion HHRI-software (hri-gen2) Browse

R2671/Firmware/src/controller.c
2a0e1d639660hri-gen2

Last Change
Raw File

controller.c
View Options

#include "controller.h"
#include "communication.h"
#include "drivers/adc.h"
#include "drivers/dac.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"
extern uint32_t statusReg;
volatile uint32_t ctrl_timestamp = 0; // [us].
volatile float32_t ctrl_motorPosCod;
volatile float32_t ctrl_motorPosCod_c;
volatile float32_t ctrl_motorPosCodFilt = 0.0f;
volatile float32_t ctrl_motorTorque_Nm_c = 0.0f;
volatile uint32_t ctrl_currentFilterType = FILTER_TYPE_NONE;
volatile pid_Pid ctrl_currentPid, ctrl_positionPid;
volatile bfilt_BasicFilter ctrl_encoderFilter;
void ctrl_RegulateCurrent(void);
void ctrl_RegulatePosition(void);
/**
* @brief Initialize the position and current controllers.
*/
void ctrl_Init(void)
{
// Setup the PIDs.
pid_Init((pid_Pid*)&ctrl_currentPid, KP_CURRENT_DEFAULT_VAL, KI_CURRENT_DEFAULT_VAL, 0.0f, CURRENT_INTEGRATOR_SAT, FF_CURRENT_DEFAULT_VAL);
#ifdef REGULATION_TYPE_SYMMETRIC
pid_Init((pid_Pid*)&ctrl_positionPid, KP_POSITION_DEFAULT_VAL, KI_POSITION_DEFAULT_VAL, KD_POSITION_DEFAULT_VAL, POSITION_INTEGRATOR_SAT_DEFAULT_VAL, 0.0f);
#else
pid_Init((pid_Pid*)&ctrl_positionPid, KP_POSITION_DEFAULT_VAL, 0.0f, KD_POSITION_DEFAULT_VAL, 0.0f, 0.0f);
#endif
// Setup the increment encoder filter.
bfilt_Init((bfilt_BasicFilter*)&ctrl_encoderFilter, ENCODER_FILT_TAU_DEFAULT_VAL, 0.0f);
// 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);
}
/**
* @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].
// Get the current.
float32_t motorCurrrentCurrent = adc_GetCurrent();
// Convert the target torque to current.
targetCurrent = ctrl_motorTorque_Nm_c / MOTOR_TORQUE_CONST;
// Regulate.
motorVoltage = -pid_Step((pid_Pid*)&ctrl_currentPid,
motorCurrrentCurrent,
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.
hb_SetPWM(pwmNormalizedDutyCycle);
}
/**
* @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 encoder position.
ctrl_motorPosCod = enc_GetPosition();
// Filter the position given by the encoder.
if(ctrl_currentFilterType == FILTER_TYPE_FIRST_ORDER)
{
ctrl_motorPosCodFilt = bfilt_Step((bfilt_BasicFilter*)&ctrl_encoderFilter, ctrl_motorPosCod);
}
else if(ctrl_currentFilterType == FILTER_TYPE_RUNNING_MEAN)
{
// TODO.
ctrl_motorPosCodFilt = ctrl_motorPosCod;
}
else // No filter.
ctrl_motorPosCodFilt = ctrl_motorPosCod;
// Regulate.
#ifdef REGULATION_TYPE_SYMMETRIC // No wall, just regulation on a point.
ctrl_motorTorque_Nm_c = -pid_Step((pid_Pid*)&ctrl_positionPid,
ctrl_motorPosCodFilt,
ctrl_motorPosCod_c,
(float32_t)(cbt_GetPositionLoopPeriod())*MICROSECOND_TO_SECOND);
#else // Wall, regulate only if not touching the wall.
if(ctrl_motorPosCodFilt < ctrl_motorPosCod_c) // Touching the wall.
{
ctrl_motorTorque_Nm_c = -pid_Step((pid_Pid*)&ctrl_positionPid,
ctrl_motorPosCodFilt,
(float32_t)ctrl_motorPosCod_c,
(float32_t)(cbt_GetPositionLoopPeriod())*MICROSECOND_TO_SECOND);
}
else
ctrl_motorTorque_Nm_c = 0.0f;
#endif*/
ctrl_motorTorque_Nm_c = 0.0f;
}
c4science · Help