#include "stm32f4xx_hal.h"
#include "arm_math.h"
#include "tim.h"
#include "stdio.h"

Data Structures
struct	Components
	Coordinates components structure. More...

struct	PID_para
	PID parameter structure. More...

struct	Motor
	Motor parameter structure. More...

Macros
#define	pi 3.14159265359f

#define	SQRT_3 1.7320508f

#define	UnitPulse 4096

Typedefs
typedef struct Components	Coordinates
	Coordinates components structure. More...

typedef struct PID_para	PID_para
	PID parameter structure. More...

typedef enum ControlMode	ControlMode
	ControlMode enumerator. More...

typedef enum UserAPP	UserAPP
	UserAPP enumerator. More...

typedef enum DRV_GAIN	DRV_GAIN
	DRV8302 amplification gain enumerator. More...

Enumerations
enum	ControlMode { Idle = 0 , Speed , Position , Torque }
	ControlMode enumerator. More...

enum	UserAPP { Drag = 1 , ForceFeedback , Switch , SpaceWalk }
	UserAPP enumerator. More...

enum	DRV_GAIN { DRV_GAIN_10 = 0 , DRV_GAIN_40 }
	DRV8302 amplification gain enumerator. More...

Functions
void	MotorInit (Motor *mHandle, ADC_HandleTypeDef hadc, TIM_HandleTypeDef hDriver, TIM_HandleTypeDef hEncoder, I2C_HandleTypeDef hIICEncoder, ControlMode mode)
	: Motor initialization function More...

void	ModeSelection (Motor *mHandle, ControlMode mode)
	: Mode selection function More...

void	UserAPPSelection (Motor *mHandle, UserAPP app)
	: user application selection function More...

void	EnableGate (void)
	: Pull up the ENGATE pin of the DRV8302 to activate the gate More...

void	GainSelection (DRV_GAIN gain)
	: Select the gain of the buit-in amplifier of DRV8302 If GAIN = LOW, the internal current shunt amplifiers have a gain of 10V/V. If GAIN = HIGH, the current shunt amplifiers have a gain of 40V/V More...

void	DC_Calibration (Motor *mHandle)
	: Calibrate the DC offset of the current shunt amplifier should be called after EnableGate() More...

void	SetMotorParam (Motor *mHandle, uint8_t DC_supply, uint8_t phase, uint8_t polePairs, float MaxCurrent, float MaxSpeed)
	: Set motor parameters More...

int	calcAngle (Motor *mHandle)
	: [Auxiliary Debug Function] calculate electric angle from Valpha and Vbeta More...

void	MotorCalibration (Motor *mHandle)
	: Calibrate motor when power on Measure the angle offset between encoder 0 deg and motor electrical 0 deg(phase a 100%) Angle_E = (Angle_M - Offset)*nPolePairs More...

void	MotorOpenLoop (Motor *mHandle)
	: openloop drag control More...

void	ADCSample (Motor *mHandle)
	: Current sampling function, once called, retrieve one set of sampled data from DMA Data stored in mHandle->Current called in timer callback function every control period More...

void	ADCFilter (Motor *mHandle)
	: Filter the sampled current More...

void	CurrentReconstruction (Motor *mHandle)
	: Reconstruct the sampled current from 2 phases to three phases More...

void	getEangle (Motor *mHandle)
	: Calculate electric angle from mechanical angle, store in mHandle->E_angle More...

void	getSpeed (Motor *mHandle)
	: Calculate the motor speed in rpm, store in mHandle->speed More...

void	ClarkeTransform (Coordinates *pComponents)
	: Clarke Transformation function, convert abc to alpha beta More...

void	ParkTransform (Coordinates *pComponents, int theta)
	: Park Transformation function, convert alpha, beta to d, q Original park: q leads d axis by 90 deg Modified park: d leads q axis by 90 deg More...

void	InvPark (Coordinates *pComponents, int theta)
	: Inverse Park Transformation function, convert d, q to alpha, beta Original park: q leads d axis by 90 deg Modified park: d leads q axis by 90 deg More...

void	GetSector (Motor *mHandle)
	: Calculate the sector of the target space voltage vector More...

void	ModulationLimit (Motor *mHandle)
	: Overmodulation limiting function, prevent overmodulation More...

void	SetSVPWM (Motor *mHandle)
	: MOSFET dwelling time calculation function, to generate SVPWM More...

void	dq2SVPWM (Motor *mHandle)
	: Generate SVPWM directly from dq voltage More...

void	SetCurrentPID (Motor *mHandle, float kp, float ki, float kd)
	: Set PID parameters of both d,q current loop More...

void	SetSpeedPID (Motor *mHandle, float kp, float ki, float kd, float outMax)
	: Set speed loop PID parameters and the speed limit More...

void	SetCurrentTarget (Motor *mHandle, float idTarget, float iqTarget)
	: Set target id, iq More...

void	SetSpeedTarget (Motor *mHandle, float Target)
	: Set target motor speed, with over speed limit More...

void	IdPID (Motor *mHandle)
	: Current loop Id PID calculation, calculate target Vd from Id with current kalman filter, integral limiter, and over voltage limiter More...

void	IqPID (Motor *mHandle)
	: Current loop Iq PID calculation, calculate target Vq from Iq with current kalman filter, integral limiter, and over voltage limiter More...

void	SpeedPID (Motor *mHandle)
	: Speed loop PID calculation, set target Id = 0, calculate target Iq from speed with integral limiter, and over current limiter More...

void	AnglePID (Motor *mHandle)
	: Angle loop PID calculation, calculate target speed with integral limiter More...

void	SetAnglePID (Motor *mHandle, float kp, float ki, float kd)
	: Set angle loop PID parameters More...

void	SetAngleTarget (Motor *mHandle, float Target)
	: Set absolute target angle, only single-turn (0-360 deg) More...

void	SetIncrementAngle (Motor *mHandle, float Target)
	: Set increment target angle, the incremental angle is also restricted in (0-360 deg) More...

void	FOC (Motor *mHandle)
	: FOC loop funtion, including electric angle conversion, phase current filtering, Clarke/Park transformation, PID calculation, and SVPWM generation Called in ADC_DMA callback every control period More...

void	FOCcheck (Motor *mHandle)
	: [Auxiliary Debug Function] call to print FOC PID parameters More...

void	GetSVPWM (Motor *mHandle)
	: Original SVPWM calculation function using complex numbers, slow More...

Macro Definition Documentation

◆ pi

#define pi 3.14159265359f

Definition at line 29 of file control.h.

◆ SQRT_3

#define SQRT_3 1.7320508f

Definition at line 30 of file control.h.

◆ UnitPulse

#define UnitPulse 4096

Definition at line 31 of file control.h.

Typedef Documentation

◆ ControlMode

typedef enum ControlMode ControlMode

ControlMode enumerator.

to set the operation mode of the FOC driver

◆ Coordinates

typedef struct Components Coordinates

Coordinates components structure.

abc, alpha/beta, d/q

◆ DRV_GAIN

typedef enum DRV_GAIN DRV_GAIN

DRV8302 amplification gain enumerator.

10V/V or 40V/V

◆ PID_para

typedef struct PID_para PID_para

PID parameter structure.

pid parameters and target, error, previous error, and sum of error

◆ UserAPP

typedef enum UserAPP UserAPP

UserAPP enumerator.

to set different user applications of the FOC driver

Enumeration Type Documentation

◆ ControlMode

enum ControlMode

ControlMode enumerator.

to set the operation mode of the FOC driver

Enumerator
Idle
Speed
Position
Torque

Definition at line 59 of file control.h.

◆ DRV_GAIN

enum DRV_GAIN

DRV8302 amplification gain enumerator.

10V/V or 40V/V

Enumerator
DRV_GAIN_10
DRV_GAIN_40

Definition at line 73 of file control.h.

◆ UserAPP

enum UserAPP

UserAPP enumerator.

to set different user applications of the FOC driver

Enumerator
Drag
ForceFeedback
Switch
SpaceWalk

Definition at line 66 of file control.h.

Function Documentation

◆ ADCFilter()

void ADCFilter ( Motor * mHandle )

: Filter the sampled current

Parameters

[in]		mHandle motor instance handler
[out]

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

◆ ADCSample()

void ADCSample ( Motor * mHandle )

: Current sampling function, once called, retrieve one set of sampled data from DMA Data stored in mHandle->Current called in timer callback function every control period

Parameters

[in]		mHandle motor instance handler
[out]

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

◆ AnglePID()

void AnglePID ( Motor * mHandle )

: Angle loop PID calculation, calculate target speed with integral limiter

Parameters

[in]		mHandle motor instance handler
[out]		SetSpeedTarget(mHandle, target)

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022 Bug fixed by Alfred John (Zzx) on 04-Sept-2022 used 'SetSpeedTarget(mHandle, target);' instead of directly assign target speed to prevent over speed.

◆ calcAngle()

int calcAngle ( Motor * mHandle )

: [Auxiliary Debug Function] calculate electric angle from Valpha and Vbeta

Parameters

[in]		mHandle motor instance handler
[out]

Returns: : electric angle [range 0-4096]

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

◆ ClarkeTransform()

void ClarkeTransform ( Coordinates * pComponents )

: Clarke Transformation function, convert abc to alpha beta

Parameters

[in]		pComponents coordinates pointer (pComponents->a, pComponents->b, pComponents->c)
[out]		pComponents coordinates pointer (pComponents->alpha, pComponents->beta)

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

◆ CurrentReconstruction()

void CurrentReconstruction ( Motor * mHandle )

: Reconstruct the sampled current from 2 phases to three phases

Parameters

[in]		mHandle motor instance handler
[out]

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

◆ DC_Calibration()

void DC_Calibration ( Motor * mHandle )

: Calibrate the DC offset of the current shunt amplifier should be called after EnableGate()

Parameters

[in]		mHandle motor instance handler
[out]

Returns: : electric angle [range 0-4096]

Change logs: Created by Alfred John (Zzx) on 04-Sept-2022

◆ dq2SVPWM()

void dq2SVPWM ( Motor * mHandle )

: Generate SVPWM directly from dq voltage

Parameters

[in]		mHandle motor instance handler
[out]

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

◆ EnableGate()

void EnableGate ( void )

: Pull up the ENGATE pin of the DRV8302 to activate the gate

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

◆ FOC()

void FOC ( Motor * mHandle )

: FOC loop funtion, including electric angle conversion, phase current filtering, Clarke/Park transformation, PID calculation, and SVPWM generation Called in ADC_DMA callback every control period

Parameters

[in]		mHandle motor instance handler
[out]

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

◆ FOCcheck()

void FOCcheck ( Motor * mHandle )

: [Auxiliary Debug Function] call to print FOC PID parameters

Parameters

[in]		mHandle motor instance handler
[out]		mHandle->idPID, mHandle->iqPID, mHandle->speedPID, mHandle->anglePID

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

◆ GainSelection()

void GainSelection ( DRV_GAIN gain )

: Select the gain of the buit-in amplifier of DRV8302 If GAIN = LOW, the internal current shunt amplifiers have a gain of 10V/V. If GAIN = HIGH, the current shunt amplifiers have a gain of 40V/V

Parameters

[in] gain DRV_GAIN_10, DRV_GAIN_40

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

◆ getEangle()

void getEangle ( Motor * mHandle )

: Calculate electric angle from mechanical angle, store in mHandle->E_angle

Parameters

[in]		mHandle motor instance handler
[out]		mHandle->E_angle

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

◆ GetSector()

void GetSector ( Motor * mHandle )

: Calculate the sector of the target space voltage vector

Parameters

[in]		mHandle->Voltage.alpha, mHandle->Voltage.beta
[out]		mHandle->sector

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

◆ getSpeed()

void getSpeed ( Motor * mHandle )

: Calculate the motor speed in rpm, store in mHandle->speed

Parameters

[in]		mHandle motor instance handler
[out]		mHandle->speed

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

◆ GetSVPWM()

void GetSVPWM ( Motor * mHandle )

: Original SVPWM calculation function using complex numbers, slow

Parameters

[in]
[out]

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

◆ IdPID()

void IdPID ( Motor * mHandle )

: Current loop Id PID calculation, calculate target Vd from Id with current kalman filter, integral limiter, and over voltage limiter

Parameters

[in]		mHandle motor instance handler
[out]		mHandle->Voltage.d

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

◆ InvPark()

void InvPark	(	Coordinates *	pComponents,
		int	theta
	)

: Inverse Park Transformation function, convert d, q to alpha, beta Original park: q leads d axis by 90 deg Modified park: d leads q axis by 90 deg

Parameters

[in]		pComponents coordinates pointer (pComponents->d, pComponents->q)
[out]		pComponents coordinates pointer (pComponents->alpha, pComponents->beta)

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

◆ IqPID()

void IqPID ( Motor * mHandle )

: Current loop Iq PID calculation, calculate target Vq from Iq with current kalman filter, integral limiter, and over voltage limiter

Parameters

[in]		mHandle motor instance handler
[out]		mHandle->Voltage.q

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

◆ ModeSelection()

void ModeSelection	(	Motor *	mHandle,
		ControlMode	mode
	)

: Mode selection function

Parameters

[in]		mHandle motor instance handler
[in]		mode operation mode

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

◆ ModulationLimit()

void ModulationLimit ( Motor * mHandle )

: Overmodulation limiting function, prevent overmodulation

Parameters

[in]		mHandle->T1, mHandle->T2
[out]		mHandle->T1, mHandle->T2

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

◆ MotorCalibration()

void MotorCalibration ( Motor * mHandle )

: Calibrate motor when power on Measure the angle offset between encoder 0 deg and motor electrical 0 deg(phase a 100%) Angle_E = (Angle_M - Offset)*nPolePairs

Parameters

[in] mHandle motor instance handler

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

◆ MotorInit()

void MotorInit	(	Motor *	mHandle,
		ADC_HandleTypeDef	hadc,
		TIM_HandleTypeDef	hDriver,
		TIM_HandleTypeDef	hEncoder,
		I2C_HandleTypeDef	hIICEncoder,
		ControlMode	mode
	)

: Motor initialization function

Parameters

[in]		mHandle motor instance handler
[in]		hadc current sampling adc handler
[in]		hDriver driver timer handler (set the control frequency)
[in]		hEncoder PWM encoder timer handler
[in]		hIICEncoder I2C encoder handler
[in]		mode initial operation mode

Returns: : void

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

◆ MotorOpenLoop()

void MotorOpenLoop ( Motor * mHandle )

: openloop drag control

Parameters

[in]		mHandle motor instance handler
[out]

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

◆ ParkTransform()

void ParkTransform	(	Coordinates *	pComponents,
		int	theta
	)

: Park Transformation function, convert alpha, beta to d, q Original park: q leads d axis by 90 deg Modified park: d leads q axis by 90 deg

Parameters

[in]		pComponents coordinates pointer (pComponents->alpha, pComponents->beta)
[out]		pComponents coordinates pointer (pComponents->d, pComponents->q)

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

◆ SetAnglePID()

void SetAnglePID	(	Motor *	mHandle,
		float	kp,
		float	ki,
		float	kd
	)

: Set angle loop PID parameters

Parameters

[in]		mHandle motor instance handler
[in]		kp, ki, kd
[out]		mHandle->anglePID

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

◆ SetAngleTarget()

void SetAngleTarget	(	Motor *	mHandle,
		float	Target
	)

: Set absolute target angle, only single-turn (0-360 deg)

Parameters

[in]		mHandle motor instance handler
[in]		Target
[out]		mHandle->anglePID.target

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

◆ SetCurrentPID()

void SetCurrentPID	(	Motor *	mHandle,
		float	kp,
		float	ki,
		float	kd
	)

: Set PID parameters of both d,q current loop

Parameters

[in]		mHandle motor instance handler
[in]		kp, ki, kd
[out]		mHandle->iqPID, mHandle->idPID

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

◆ SetCurrentTarget()

void SetCurrentTarget	(	Motor *	mHandle,
		float	idTarget,
		float	iqTarget
	)

: Set target id, iq

Parameters

[in]		mHandle motor instance handler
[in]		idTarget, iqTarget
[out]		mHandle->idPID.target, mHandle->iqPID.target

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

◆ SetIncrementAngle()

void SetIncrementAngle	(	Motor *	mHandle,
		float	Target
	)

: Set increment target angle, the incremental angle is also restricted in (0-360 deg)

Parameters

[in]		mHandle motor instance handler
[out]		SetAngleTarget(mHandle,Target);

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

◆ SetMotorParam()

void SetMotorParam	(	Motor *	mHandle,
		uint8_t	DC_supply,
		uint8_t	phase,
		uint8_t	polePairs,
		float	MaxCurrent,
		float	MaxSpeed
	)

: Set motor parameters

Parameters

[in]		mHandle motor instance handler
[in]		V DC supply voltage
[in]		phase number of phase
[in]		polePairs number of pole pairs
[in]		MaxCurrent maximum current -> maximum phase current
[in]		MaxSpeed maximum speed

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

◆ SetSpeedPID()

void SetSpeedPID	(	Motor *	mHandle,
		float	kp,
		float	ki,
		float	kd,
		float	outMax
	)

: Set speed loop PID parameters and the speed limit

Parameters

[in]		mHandle motor instance handler
[in]		kp, ki, kd, outMax
[out]		mHandle->speedPID, mHandle->SpeedLimit

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

◆ SetSpeedTarget()

void SetSpeedTarget	(	Motor *	mHandle,
		float	Target
	)

: Set target motor speed, with over speed limit

Parameters

[in]		mHandle motor instance handler
[in]		Target
[out]		mHandle->speedPID.target

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022 Bug fixed by Alfred John (Zzx) on 04-Sept-2022 added negative over speed judgement

◆ SetSVPWM()

void SetSVPWM ( Motor * mHandle )

: MOSFET dwelling time calculation function, to generate SVPWM

Parameters

[in]		mHandle motor instance handler
[out]		mHandle->Ta, mHandle->Tb, mHandle->Tc

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

◆ SpeedPID()

void SpeedPID ( Motor * mHandle )

: Speed loop PID calculation, set target Id = 0, calculate target Iq from speed with integral limiter, and over current limiter

Parameters

[in]		mHandle motor instance handler
[out]		mHandle->iqPID.target

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

◆ UserAPPSelection()

void UserAPPSelection	(	Motor *	mHandle,
		UserAPP	app
	)

: user application selection function

Parameters

[in]		mHandle motor instance handler
[in]		app user application

Returns: :

Change logs: Created by Alfred John (Zzx) on 03-Sept-2022

Data Structures

Macros

Typedefs

Enumerations

Functions

Macro Definition Documentation

◆ pi

◆ SQRT_3

◆ UnitPulse

Typedef Documentation

◆ ControlMode

◆ Coordinates

◆ DRV_GAIN

◆ PID_para

◆ UserAPP

Enumeration Type Documentation

◆ ControlMode

◆ DRV_GAIN

◆ UserAPP

Function Documentation

◆ ADCFilter()

◆ ADCSample()

◆ AnglePID()

◆ calcAngle()

◆ ClarkeTransform()

◆ CurrentReconstruction()

◆ DC_Calibration()

◆ dq2SVPWM()

◆ EnableGate()

◆ FOC()

◆ FOCcheck()

◆ GainSelection()

◆ getEangle()

◆ GetSector()

◆ getSpeed()

◆ GetSVPWM()

◆ IdPID()

◆ InvPark()

◆ IqPID()

◆ ModeSelection()

◆ ModulationLimit()

◆ MotorCalibration()

◆ MotorInit()

◆ MotorOpenLoop()

◆ ParkTransform()

◆ SetAnglePID()

◆ SetAngleTarget()

◆ SetCurrentPID()

◆ SetCurrentTarget()

◆ SetIncrementAngle()

◆ SetMotorParam()

◆ SetSpeedPID()

◆ SetSpeedTarget()

◆ SetSVPWM()

◆ SpeedPID()

◆ UserAPPSelection()