Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F63036376
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
Fri, May 17, 08:22
Size
4 KB
Mime Type
text/x-c
Expires
Sun, May 19, 08:22 (2 d)
Engine
blob
Format
Raw Data
Handle
17723762
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/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"
extern uint32_t statusReg;
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
Log In to Comment