Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F86216408
controller.c
No One
Temporary
Actions
Download File
Edit File
Delete File
View Transforms
Subscribe
Mute Notifications
Award Token
Subscribers
None
File Metadata
Details
File Info
Storage
Attached
Created
Sat, Oct 5, 00:12
Size
5 KB
Mime Type
text/x-c
Expires
Mon, Oct 7, 00:12 (2 d)
Engine
blob
Format
Raw Data
Handle
21372435
Attached To
R2671 HHRI-software
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
Log In to Comment