Diffusion HHRI-software (hri-gen3) Browse

controller.c
0f02b70374cbhri-gen3

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);
			}

R2671/Firmware/src/controller.c0f02b70374cbhri-gen3

controller.cView Options

R2671/Firmware/src/controller.c
0f02b70374cbhri-gen3

controller.c
View Options