Page MenuHomec4science
Files F91061091

controller.c
No OneTemporary
Actions

File Metadata

Created
Thu, Nov 7, 11:13

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_motorPosHall;
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;
bool ctrl_regulateCurrent;
void ctrl_RegulateCurrent(void);
void ctrl_RegulatePosition(void);
/**
* @brief Initialize the position and current controllers.
*/
void ctrl_Init(void)
{
// 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, 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);
// Share the key variables with the computer.
comm_monitorFloat("current [A]", (float32_t*)&ctrl_currentPid.current, 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_raw_pos [deg]", (float32_t*)&ctrl_motorPosHall, 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].
// 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, if the 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_motorPosHall = hall_Get();
// 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;*/
// 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