Merge pull request #2 from MDHSweden/pwmDriver
Pwmdriver Accepted by Lennart
This commit is contained in:
commit
b8ea6487c2
67
UAV-ControlSystem/inc/drivers/motors.h
Normal file
67
UAV-ControlSystem/inc/drivers/motors.h
Normal file
@ -0,0 +1,67 @@
|
||||
/*
|
||||
* motors.h
|
||||
*
|
||||
* Created on: 15 sep. 2016
|
||||
* Author: jgr12001
|
||||
*/
|
||||
|
||||
#ifndef DRIVERS_MOTORS_H_
|
||||
#define DRIVERS_MOTORS_H_
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: Initializing a motor (maps a pwm signal to a certain pin) *
|
||||
* INFORMATION: The motor becomes active, Each motor configuration is changed in the "stm32f4xx_revo.h" file *
|
||||
* Example - pwmEnableMotor(MOTOR_1) *
|
||||
**************************************************************************/
|
||||
void pwmEnableMotor(uint8_t motor);
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: Enables all motors (maps a pwm signal to all pins which will be connected to a motor) *
|
||||
* INFORMATION: All motors become active
|
||||
**************************************************************************/
|
||||
void pwmEnableAllMotors(void);
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: Deactivates a motor (Disables the pwm signal for the motor pin )
|
||||
* INFORMATION:
|
||||
* Example - pwmDeactivateMotor(MOTOR_1)
|
||||
**************************************************************************/
|
||||
void pwmDeactivateMotor(uint8_t motor);
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: If a motor is Deactivated (no pwm signal), this function will activate the motor to it's last state (the pwm signal will go back to is's last state) *
|
||||
* INFORMATION:
|
||||
* Example - pwmActivateMotor(MOTOR_1)
|
||||
**************************************************************************/
|
||||
void pwmActivateMotor(uint8_t motor);
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: Deactivates all motors (Deactivate a pwm signal to all motor pins) *
|
||||
* INFORMATION: *
|
||||
* Example - pwmDeactivateAllMotors() *
|
||||
**************************************************************************/
|
||||
void pwmDeactivateAllMotors(void);
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: Activates all motors (Activates a pwm signal to all motor pins to it's last state)* *
|
||||
* INFORMATION: *
|
||||
* Example - pwmActivateAllMotors() *
|
||||
**************************************************************************/
|
||||
void pwmActivateAllMotors(void);
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: Change the speed of a certain motor if it's active *
|
||||
* INFORMATION: The speed is changes by switching the the pulse of a pwm signal *
|
||||
* Example - pwmAdjustSpeedOfMotor(MOTOR_2, 200) *
|
||||
**************************************************************************/
|
||||
void pwmAdjustSpeedOfMotor(uint8_t motor, uint16_t pulse);
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: Change the speed of a certain motor if it's active *
|
||||
* INFORMATION: The speed is change by switching the the Duty Cycle of a pwm signal (The Duty Cycle may only take values between 0 - 100 %) *
|
||||
* Example - pwmAdjustSpeedOfMotor(MOTOR_2, 50); *
|
||||
**************************************************************************/
|
||||
void pwmAdjustSpeedOfMotorDutyCycle(uint8_t motor, uint16_t DutyCycle);
|
||||
|
||||
|
||||
#endif /* DRIVERS_MOTORS_H_ */
|
48
UAV-ControlSystem/inc/drivers/pwm.h
Normal file
48
UAV-ControlSystem/inc/drivers/pwm.h
Normal file
@ -0,0 +1,48 @@
|
||||
/**************************************************************************
|
||||
* NAME: Johan Gärtner *
|
||||
* PURPOSE: Initialize driver for the PWM signal to certain outputs *
|
||||
* INFORMATION: *
|
||||
* GLOBAL VARIABLES: *
|
||||
* Variable Type Description *
|
||||
* -------- ---- ----------- *
|
||||
* **************************************************************************/
|
||||
|
||||
#ifndef DRIVERS_PWM_H_
|
||||
#define DRIVERS_PWM_H_
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: pwmInit initializes a pwm signal to a certain pin output *
|
||||
* INFORMATION:
|
||||
* Example - pwmInit(GPIOB, GPIO_PIN_0, TIM3, TIM_CHANNEL_3, 2000, 100);
|
||||
* GPIO = selects GPIO output on processor
|
||||
* pin = selects pin output on processor
|
||||
* tim = Timer configuration
|
||||
* Channel = Selects a channel for a certain timer, each timer as 4 channels
|
||||
* period = Period of PWM signal
|
||||
* pulse = the "duty cycle" of the pwm signal *
|
||||
**************************************************************************/
|
||||
void pwmInit(GPIO_TypeDef * GPIO, uint16_t pin, TIM_TypeDef * tim, uint32_t Channel, uint16_t period, uint16_t pulse);
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: setPwmPulse changes a certain pulse for an initialized pwm signal *
|
||||
* INFORMATION: It's only possible to change the pwm output for a certain timer
|
||||
* on a certain channel, not a pin output
|
||||
* Example - setPwmPulse(TIM_CHANNEL_4, TIM3, 500); *
|
||||
**************************************************************************/
|
||||
void setPwmPulse(uint32_t Channel, TIM_TypeDef * tim, uint16_t newPulse);
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: startPwm Activates a pwm signal for a certain timer on a certain channel
|
||||
* INFORMATION:
|
||||
* Example - startPwm(TIM_CHANNEL_4, TIM3);
|
||||
**************************************************************************/
|
||||
void startPwm(uint32_t Channel, TIM_TypeDef * tim);
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: stopPwm Deactivates a pwm signal for a certain timer on a certain channel
|
||||
* INFORMATION:
|
||||
* Example - stopPwm(TIM_CHANNEL_4, TIM3);
|
||||
**************************************************************************/
|
||||
void stopPwm(uint32_t Channel, TIM_TypeDef * tim);
|
||||
|
||||
#endif /* DRIVERS_PWM_H_ */
|
@ -101,28 +101,65 @@
|
||||
|
||||
|
||||
/* Define all the moter of the system, servos + extra */
|
||||
#define MOTOR_1 1
|
||||
#define MOTOR_1_PIN SERVO_OUT_1_PIN
|
||||
#define MOTOR_1_PORT SERVO_OUT_1_GPIO_PORT
|
||||
#define MOTOR_1_TIM TIM3
|
||||
#define MOTOR_1_CHANNEL TIM_CHANNEL_3
|
||||
|
||||
#define MOTOR_2 2
|
||||
#define MOTOR_2_PIN SERVO_OUT_2_PIN
|
||||
#define MOTOR_2_PORT SERVO_OUT_2_GPIO_PORT
|
||||
#define MOTOR_2_TIM TIM3
|
||||
#define MOTOR_2_CHANNEL TIM_CHANNEL_4
|
||||
|
||||
#define MOTOR_3 3
|
||||
#define MOTOR_3_PIN SERVO_OUT_3_PIN
|
||||
#define MOTOR_3_PORT SERVO_OUT_3_GPIO_PORT
|
||||
#define MOTOR_3_TIM TIM9
|
||||
#define MOTOR_3_CHANNEL TIM_CHANNEL_2
|
||||
|
||||
#define MOTOR_4 4
|
||||
#define MOTOR_4_PIN SERVO_OUT_4_PIN
|
||||
#define MOTOR_4_PORT SERVO_OUT_4_GPIO_PORT
|
||||
#define MOTOR_4_TIM TIM2
|
||||
#define MOTOR_4_CHANNEL TIM_CHANNEL_3
|
||||
|
||||
#define MOTOR_5 5
|
||||
#define MOTOR_5_PIN SERVO_OUT_5_PIN
|
||||
#define MOTOR_5_PORT SERVO_OUT_5_GPIO_PORT
|
||||
#define MOTOR_5_TIM TIM5
|
||||
#define MOTOR_5_CHANNEL TIM_CHANNEL_2
|
||||
|
||||
#define MOTOR_6 6
|
||||
#define MOTOR_6_PIN SERVO_OUT_6_PIN
|
||||
#define MOTOR_6_PORT SERVO_OUT_6_GPIO_PORT
|
||||
#define MOTOR_7_PIN
|
||||
#define MOTOR_7_PORT
|
||||
#define MOTOR_8_PIN
|
||||
#define MOTOR_8_PORT
|
||||
#define MOTOR_9_PIN
|
||||
#define MOTOR_9_PORT
|
||||
#define MOTOR_10_PIN
|
||||
#define MOTOR_10_PORT
|
||||
#define MOTOR_6_TIM TIM5
|
||||
#define MOTOR_6_CHANNEL TIM_CHANNEL_1
|
||||
|
||||
#define MOTOR_7 7
|
||||
#define MOTOR_7_PIN GPIO_PIN_6
|
||||
#define MOTOR_7_PORT GPIOC
|
||||
#define MOTOR_7_TIM TIM8
|
||||
#define MOTOR_7_CHANNEL TIM_CHANNEL_1
|
||||
|
||||
#define MOTOR_8 8
|
||||
#define MOTOR_8_PIN GPIO_PIN_7
|
||||
#define MOTOR_8_PORT GPIOC
|
||||
#define MOTOR_8_TIM TIM8
|
||||
#define MOTOR_8_CHANNEL TIM_CHANNEL_2
|
||||
|
||||
#define MOTOR_9 9
|
||||
#define MOTOR_9_PIN GPIO_PIN_8
|
||||
#define MOTOR_9_PORT GPIOC
|
||||
#define MOTOR_9_TIM TIM8
|
||||
#define MOTOR_9_CHANNEL TIM_CHANNEL_3
|
||||
|
||||
#define MOTOR_10 10
|
||||
#define MOTOR_10_PIN GPIO_PIN_9
|
||||
#define MOTOR_10_PORT GPIOC
|
||||
#define MOTOR_10_TIM TIM8
|
||||
#define MOTOR_10_CHANNEL TIM_CHANNEL_4
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
|
195
UAV-ControlSystem/src/drivers/motors.c
Normal file
195
UAV-ControlSystem/src/drivers/motors.c
Normal file
@ -0,0 +1,195 @@
|
||||
/**************************************************************************
|
||||
* NAME: Johan Gärtner *
|
||||
* PURPOSE: Driver for the motor signal to certain outputs *
|
||||
* INFORMATION: *
|
||||
* GLOBAL VARIABLES: *
|
||||
* Variable Type Description *
|
||||
* MOTOR_PWM_INIT_PERIODE define The initializing periode of the pwm signal to each motor *
|
||||
* MOTOR_PWM_INIT_PULSE define The initializing pulse of the pwm signal to each motor
|
||||
* **************************************************************************/
|
||||
|
||||
#include "stm32f4xx.h"
|
||||
#include "stm32f4xx_revo.h"
|
||||
#include "drivers/pwm.h"
|
||||
#include "drivers/motors.h"
|
||||
|
||||
#define MOTOR_PWM_INIT_PERIODE 65535
|
||||
#define MOTOR_PWM_INIT_PULSE MOTOR_PWM_INIT_PERIODE/2
|
||||
|
||||
/* A struct of a pwm motor profile */
|
||||
typedef struct
|
||||
{
|
||||
GPIO_TypeDef * port; //GPIOA/B/C
|
||||
uint16_t pin; //GPIO_PIN_0/1/2/3/..
|
||||
TIM_TypeDef * tim; //TIM1/2/3/..
|
||||
uint32_t channel; //TIM_CHANNEL_1/TIM_CHANNEL_1/..
|
||||
}motorProfile;
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: Returns a profile of a certain motor
|
||||
* INFORMATION: Each Motor has a certain profile which includes a pin, port, timer and a timer channel
|
||||
* Each motor configuration is changed in the "stm32f4xx_revo.h" file
|
||||
* Example - getMotorProfile(MOTOR_1)
|
||||
**************************************************************************/
|
||||
motorProfile getMotorProfile(uint8_t motor)
|
||||
{
|
||||
motorProfile profile;
|
||||
|
||||
switch(motor)
|
||||
{
|
||||
case MOTOR_1:
|
||||
profile.port = MOTOR_1_PORT;
|
||||
profile.pin = MOTOR_1_PIN;
|
||||
profile.tim = MOTOR_1_TIM;
|
||||
profile.channel = MOTOR_1_CHANNEL;
|
||||
break;
|
||||
case MOTOR_2:
|
||||
profile.port = MOTOR_2_PORT;
|
||||
profile.pin = MOTOR_2_PIN;
|
||||
profile.tim = MOTOR_2_TIM;
|
||||
profile.channel = MOTOR_2_CHANNEL;
|
||||
break;
|
||||
case MOTOR_3:
|
||||
profile.port = MOTOR_3_PORT;
|
||||
profile.pin = MOTOR_3_PIN;
|
||||
profile.tim = MOTOR_3_TIM;
|
||||
profile.channel = MOTOR_3_CHANNEL;
|
||||
break;
|
||||
case MOTOR_4:
|
||||
profile.port = MOTOR_4_PORT;
|
||||
profile.pin = MOTOR_4_PIN;
|
||||
profile.tim = MOTOR_4_TIM;
|
||||
profile.channel = MOTOR_4_CHANNEL;
|
||||
break;
|
||||
case MOTOR_5:
|
||||
profile.port = MOTOR_5_PORT;
|
||||
profile.pin = MOTOR_5_PIN;
|
||||
profile.tim = MOTOR_5_TIM;
|
||||
profile.channel = MOTOR_5_CHANNEL;
|
||||
break;
|
||||
case MOTOR_6:
|
||||
profile.port = MOTOR_6_PORT;
|
||||
profile.pin = MOTOR_6_PIN;
|
||||
profile.tim = MOTOR_6_TIM;
|
||||
profile.channel = MOTOR_6_CHANNEL;
|
||||
break;
|
||||
case MOTOR_7:
|
||||
profile.port = MOTOR_7_PORT;
|
||||
profile.pin = MOTOR_7_PIN;
|
||||
profile.tim = MOTOR_7_TIM;
|
||||
profile.channel = MOTOR_7_CHANNEL;
|
||||
break;
|
||||
case MOTOR_8:
|
||||
profile.port = MOTOR_8_PORT;
|
||||
profile.pin = MOTOR_8_PIN;
|
||||
profile.tim = MOTOR_8_TIM;
|
||||
profile.channel = MOTOR_8_CHANNEL;
|
||||
break;
|
||||
case MOTOR_9:
|
||||
profile.port = MOTOR_9_PORT;
|
||||
profile.pin = MOTOR_9_PIN;
|
||||
profile.tim = MOTOR_9_TIM;
|
||||
profile.channel = MOTOR_9_CHANNEL;
|
||||
break;
|
||||
case MOTOR_10:
|
||||
profile.port = MOTOR_10_PORT;
|
||||
profile.pin = MOTOR_10_PIN;
|
||||
profile.tim = MOTOR_10_TIM;
|
||||
profile.channel = MOTOR_10_CHANNEL;;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
return profile;
|
||||
}
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: Initializing a motor (maps a pwm signal to a certain pin) *
|
||||
* INFORMATION: The motor becomes active, Each motor configuration is changed in the "stm32f4xx_revo.h" file *
|
||||
* Example - pwmEnableMotor(MOTOR_1) *
|
||||
**************************************************************************/
|
||||
void pwmEnableMotor(uint8_t motor)
|
||||
{
|
||||
motorProfile profile = getMotorProfile(motor);
|
||||
|
||||
pwmInit(profile.port, profile.pin, profile.tim, profile.channel, MOTOR_PWM_INIT_PERIODE, MOTOR_PWM_INIT_PULSE);
|
||||
}
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: Enables all motors (maps a pwm signal to all pins which will be connected to a motor) *
|
||||
* INFORMATION: All motors become active
|
||||
**************************************************************************/
|
||||
void pwmEnableAllMotors(void)
|
||||
{
|
||||
for (uint8_t i = 1; i < 11; i++ ) pwmEnableMotor(i);
|
||||
}
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: If a motor is Deactivated (no pwm signal), this function will activate the motor to it's last state (the pwm signal will go back to is's last state) *
|
||||
* INFORMATION:
|
||||
* Example - pwmActivateMotor(MOTOR_1)
|
||||
**************************************************************************/
|
||||
void pwmActivateMotor(uint8_t motor)
|
||||
{
|
||||
motorProfile profile = getMotorProfile(motor);
|
||||
|
||||
startPwm(profile.channel, profile.tim);
|
||||
}
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: Deactivates a motor (Disables the pwm signal for the motor pin )
|
||||
* INFORMATION:
|
||||
* Example - pwmDeactivateMotor(MOTOR_1)
|
||||
**************************************************************************/
|
||||
void pwmDeactivateMotor(uint8_t motor)
|
||||
{
|
||||
motorProfile profile = getMotorProfile(motor);
|
||||
|
||||
stopPwm(profile.channel, profile.tim);
|
||||
}
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: Activates all motors (Activates a pwm signal to all motor pins to it's last state)* *
|
||||
* INFORMATION: *
|
||||
* Example - pwmActivateAllMotors() *
|
||||
**************************************************************************/
|
||||
void pwmActivateAllMotors(void)
|
||||
{
|
||||
for (uint8_t i = 1; i < 11; i++ ) pwmDeactivateMotor(i);
|
||||
}
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: Deactivates all motors (Deactivate a pwm signal to all motor pins) *
|
||||
* INFORMATION: *
|
||||
* Example - pwmDeactivateAllMotors() *
|
||||
**************************************************************************/
|
||||
void pwmDeactivateAllMotors(void)
|
||||
{
|
||||
for (uint8_t i = 1; i < 11; i++ ) pwmActivateMotor(i);
|
||||
}
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: Change the speed of a certain motor if it's active *
|
||||
* INFORMATION: The speed is changes by switching the the pulse of a pwm signal *
|
||||
* Example - pwmAdjustSpeedOfMotor(MOTOR_2, 200) *
|
||||
**************************************************************************/
|
||||
void pwmAdjustSpeedOfMotor(uint8_t motor, uint16_t pulse)
|
||||
{
|
||||
motorProfile profile = getMotorProfile(motor);
|
||||
|
||||
setPwmPulse(profile.channel, profile.tim, pulse);
|
||||
}
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: Change the speed of a certain motor if it's active *
|
||||
* INFORMATION: The speed is change by switching the the Duty Cycle of a pwm signal (The Duty Cycle may only take values between 0 - 100 %) *
|
||||
* Example - pwmAdjustSpeedOfMotor(MOTOR_2, 50); *
|
||||
**************************************************************************/
|
||||
void pwmAdjustSpeedOfMotorDutyCycle(uint8_t motor, uint16_t DutyCycle)
|
||||
{
|
||||
uint16_t pulse = (DutyCycle*MOTOR_PWM_INIT_PERIODE)/100; /* Converts the DutyCycle to a pulse */
|
||||
motorProfile profile = getMotorProfile(motor);
|
||||
|
||||
setPwmPulse(profile.channel, profile.tim, pulse);
|
||||
}
|
246
UAV-ControlSystem/src/drivers/pwm.c
Normal file
246
UAV-ControlSystem/src/drivers/pwm.c
Normal file
@ -0,0 +1,246 @@
|
||||
/**************************************************************************
|
||||
* NAME: Johan Gärtner *
|
||||
* PURPOSE: Initialize driver for the PWM signal to certain outputs *
|
||||
* INFORMATION: *
|
||||
* GLOBAL VARIABLES: *
|
||||
* Variable Type Description *
|
||||
* -------- ---- ----------- *
|
||||
* **************************************************************************/
|
||||
|
||||
#include "stm32f4xx.h"
|
||||
#include "drivers/pwm.h"
|
||||
|
||||
typedef enum { false, true } bool;
|
||||
|
||||
bool pwmOk = true;
|
||||
|
||||
/* Struct for a certain pwm profile */
|
||||
typedef struct
|
||||
{
|
||||
GPIO_TypeDef * GPIO; //GPIOA/B/C
|
||||
uint16_t pin; //GPIO_PIN_0/1/2/3/..
|
||||
TIM_TypeDef * tim; //TIM1/2/3/..
|
||||
uint32_t Alternate; //GPIO_AF1_TIM1/GPIO_AF2_TIM2/..
|
||||
uint32_t Channel; //TIM_CHANNEL_1/TIM_CHANNEL_1/..
|
||||
}pwmProfile;
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: Error handler which stops the program if an error occurs *
|
||||
* INFORMATION: *
|
||||
**************************************************************************/
|
||||
static void Error_Handler(void);
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: get_Pwm_Profile() Gets a variable of the struct "pwmProfile" *
|
||||
* INFORMATION:
|
||||
* Example - get_Pwm_Profile(GPIOA, GPIO_PIN_0, TIM_CHANNEL_3, TIM3); *
|
||||
**************************************************************************/
|
||||
pwmProfile get_Pwm_Profile(GPIO_TypeDef * GPIO, uint16_t pin, uint32_t Channel, TIM_TypeDef * tim);
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: pwmInit initializes a pwm signal to a certain pin output *
|
||||
* INFORMATION:
|
||||
* Example - pwmInit(GPIOB, GPIO_PIN_0, TIM3, TIM_CHANNEL_3, 2000, 100);
|
||||
* GPIO = selects GPIO output on processor
|
||||
* pin = selects pin output on processor
|
||||
* tim = Timer configuration
|
||||
* Channel = Selects a channel for a certain timer, each timer as 4 channels
|
||||
* period = Period of PWM signal
|
||||
* pulse = the "duty cycle" of the pwm signal *
|
||||
**************************************************************************/
|
||||
void pwmInit(GPIO_TypeDef * GPIO, uint16_t pin, TIM_TypeDef * tim, uint32_t Channel, uint16_t period, uint16_t pulse)
|
||||
{
|
||||
pwmProfile profile;
|
||||
|
||||
profile = get_Pwm_Profile(GPIO, pin, Channel, tim); /* Get a current profile for a pwm to a certain pin */
|
||||
|
||||
HAL_Init();
|
||||
|
||||
/* Enables clock for a certain GPIO and timer */
|
||||
if (GPIO == GPIOA) __HAL_RCC_GPIOA_CLK_ENABLE();
|
||||
if (GPIO == GPIOB) __HAL_RCC_GPIOB_CLK_ENABLE();
|
||||
if (GPIO == GPIOC) __HAL_RCC_GPIOC_CLK_ENABLE();
|
||||
|
||||
if (tim == TIM1) __HAL_RCC_TIM1_CLK_ENABLE();
|
||||
if (tim == TIM2) __HAL_RCC_TIM2_CLK_ENABLE();
|
||||
if (tim == TIM3) __HAL_RCC_TIM3_CLK_ENABLE();
|
||||
if (tim == TIM4) __HAL_RCC_TIM4_CLK_ENABLE();
|
||||
if (tim == TIM5) __HAL_RCC_TIM5_CLK_ENABLE();
|
||||
if (tim == TIM8) __HAL_RCC_TIM8_CLK_ENABLE();
|
||||
if (tim == TIM9) __HAL_RCC_TIM9_CLK_ENABLE();
|
||||
if (tim == TIM12) __HAL_RCC_TIM12_CLK_ENABLE();
|
||||
/* ------------------------------------------ */
|
||||
|
||||
GPIO_InitTypeDef GPIO_InitStructure;
|
||||
|
||||
GPIO_InitStructure.Pin = profile.pin; /* Sets the pwm pin */
|
||||
GPIO_InitStructure.Mode = GPIO_MODE_AF_PP;
|
||||
GPIO_InitStructure.Speed = GPIO_SPEED_HIGH;
|
||||
GPIO_InitStructure.Alternate = profile.Alternate; /* Enables the Alternate function for a pin to the correct pwm function */
|
||||
GPIO_InitStructure.Pull = GPIO_NOPULL;
|
||||
HAL_GPIO_Init(profile.GPIO, &GPIO_InitStructure); /* Sets the pwm GPIO */
|
||||
|
||||
TIM_HandleTypeDef TimHandle;
|
||||
|
||||
uint32_t uwPrescalerValue =2;
|
||||
TimHandle.Instance = profile.tim; /* Sets timer */
|
||||
TimHandle.Init.Period = period; /* Sets period of pwm */
|
||||
TimHandle.Init.Prescaler = uwPrescalerValue;
|
||||
TimHandle.Init.ClockDivision = 0;
|
||||
TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP;
|
||||
|
||||
TIM_OC_InitTypeDef PWMConfig;
|
||||
|
||||
//PWMConfig.Pulse = 840;
|
||||
PWMConfig.Pulse = 840;
|
||||
PWMConfig.OCMode = TIM_OCMODE_PWM1;
|
||||
PWMConfig.OCPolarity = TIM_OCPOLARITY_HIGH;
|
||||
PWMConfig.OCNPolarity = TIM_OCNPOLARITY_HIGH;
|
||||
PWMConfig.OCIdleState = TIM_OCIDLESTATE_SET;
|
||||
PWMConfig.OCNIdleState= TIM_OCNIDLESTATE_RESET;
|
||||
PWMConfig.OCFastMode = TIM_OCFAST_DISABLE;
|
||||
|
||||
TIM_BreakDeadTimeConfigTypeDef DeadConfig;
|
||||
|
||||
DeadConfig.AutomaticOutput=TIM_AUTOMATICOUTPUT_ENABLE;
|
||||
DeadConfig.BreakPolarity=0;
|
||||
DeadConfig.BreakState=0;
|
||||
DeadConfig.DeadTime=100;
|
||||
DeadConfig.LockLevel=0;
|
||||
DeadConfig.OffStateIDLEMode=1;
|
||||
DeadConfig.OffStateRunMode=1;
|
||||
|
||||
if(HAL_TIM_PWM_Init(&TimHandle) != HAL_OK) Error_Handler(); /* Init pwm */
|
||||
|
||||
if(HAL_TIM_PWM_ConfigChannel(&TimHandle,&PWMConfig,profile.Channel) != HAL_OK) Error_Handler(); /* pwm configure channel and select channel */
|
||||
|
||||
if(HAL_TIMEx_ConfigBreakDeadTime(&TimHandle,&DeadConfig) != HAL_OK) Error_Handler(); /* ConfigBreakDeadTime and sets channel */
|
||||
|
||||
if(HAL_TIM_PWM_Start(&TimHandle, profile.Channel) != HAL_OK) Error_Handler(); /* Starts pwm and sets channel */
|
||||
|
||||
if(HAL_TIMEx_PWMN_Start(&TimHandle,profile.Channel) != HAL_OK) Error_Handler(); /* Starts pwm and sets channel */
|
||||
|
||||
|
||||
/* Sets a pulse for a certain channel for a certain timer (CCRx = channel x) */
|
||||
switch(profile.Channel)
|
||||
{
|
||||
case TIM_CHANNEL_1:
|
||||
profile.tim->CCR1 = pulse;
|
||||
break;
|
||||
case TIM_CHANNEL_2:
|
||||
profile.tim->CCR2 = pulse;
|
||||
break;
|
||||
case TIM_CHANNEL_3:
|
||||
profile.tim->CCR3 = pulse;
|
||||
break;
|
||||
case TIM_CHANNEL_4:
|
||||
profile.tim->CCR4 = pulse;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: setPwmPulse changes a certain pulse for an initialized pwm signal
|
||||
* INFORMATION: It's only possible to change the pwm output for a certain timer
|
||||
* on a certain channel, not a pin output. This function will only run is a pwm is active.
|
||||
* The pwm is activated and deactivated at startPwm/stopPwm
|
||||
* Example - setPwmPulse(TIM_CHANNEL_4, TIM3, 500);
|
||||
**************************************************************************/
|
||||
void setPwmPulse(uint32_t Channel, TIM_TypeDef * tim, uint16_t newPulse)
|
||||
{
|
||||
if(pwmOk)
|
||||
{
|
||||
TIM_HandleTypeDef TimHandle;
|
||||
|
||||
TimHandle.Instance = tim;
|
||||
HAL_TIM_PWM_Start(&TimHandle, Channel);
|
||||
|
||||
switch(Channel)
|
||||
{
|
||||
case TIM_CHANNEL_1:
|
||||
tim->CCR1 = newPulse;
|
||||
break;
|
||||
case TIM_CHANNEL_2:
|
||||
tim->CCR2 = newPulse;
|
||||
break;
|
||||
case TIM_CHANNEL_3:
|
||||
tim->CCR3 = newPulse;
|
||||
break;
|
||||
case TIM_CHANNEL_4:
|
||||
tim->CCR4 = newPulse;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: startPwm Activates a pwm signal for a certain timer on a certain channel
|
||||
* INFORMATION:
|
||||
* Example - startPwm(TIM_CHANNEL_4, TIM3);
|
||||
**************************************************************************/
|
||||
void startPwm(uint32_t Channel, TIM_TypeDef * tim)
|
||||
{
|
||||
pwmOk = true;
|
||||
TIM_HandleTypeDef TimHandle;
|
||||
|
||||
TimHandle.Instance = tim;
|
||||
HAL_TIM_PWM_Start(&TimHandle, Channel);
|
||||
}
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: stopPwm Deactivates a pwm signal for a certain timer on a certain channel
|
||||
* INFORMATION:
|
||||
* Example - stopPwm(TIM_CHANNEL_4, TIM3);
|
||||
**************************************************************************/
|
||||
void stopPwm(uint32_t Channel, TIM_TypeDef * tim)
|
||||
{
|
||||
pwmOk = false;
|
||||
TIM_HandleTypeDef TimHandle;
|
||||
|
||||
TimHandle.Instance = tim;
|
||||
HAL_TIM_PWM_Stop(&TimHandle, Channel);
|
||||
}
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: Error handler which stops the program if an error occurs *
|
||||
* INFORMATION: *
|
||||
**************************************************************************/
|
||||
static void Error_Handler(void)
|
||||
{
|
||||
while(1)
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
/**************************************************************************
|
||||
* BRIEF: get_Pwm_Profile() Gets a variable of the struct "pwmProfile" *
|
||||
* INFORMATION:
|
||||
* Example - get_Pwm_Profile(GPIOA, GPIO_PIN_0, TIM_CHANNEL_3, TIM3); *
|
||||
**************************************************************************/
|
||||
pwmProfile get_Pwm_Profile(GPIO_TypeDef * GPIO, uint16_t pin, uint32_t Channel, TIM_TypeDef * tim)
|
||||
{
|
||||
pwmProfile profile;
|
||||
|
||||
/* Sets a Alternate function for a certain timer */
|
||||
if(tim == TIM1) profile.Alternate = GPIO_AF1_TIM1;
|
||||
else if(tim == TIM2) profile.Alternate = GPIO_AF1_TIM2;
|
||||
else if(tim == TIM3) profile.Alternate = GPIO_AF2_TIM3;
|
||||
else if(tim == TIM4) profile.Alternate = GPIO_AF2_TIM4;
|
||||
else if(tim == TIM5) profile.Alternate = GPIO_AF2_TIM5;
|
||||
else if(tim == TIM8) profile.Alternate = GPIO_AF3_TIM8;
|
||||
else if(tim == TIM9) profile.Alternate = GPIO_AF3_TIM9;
|
||||
else if(tim == TIM12) profile.Alternate = GPIO_AF9_TIM12;
|
||||
|
||||
profile.Channel = Channel;
|
||||
profile.GPIO = GPIO;
|
||||
profile.pin = pin;
|
||||
profile.tim = tim;
|
||||
|
||||
return profile;
|
||||
}
|
||||
|
||||
|
Reference in New Issue
Block a user