void vMotorsInit(unsigned portBASE_TYPE motorsDaemonPriority_)
{
// Enable GPIOA & GPIOC clock
vGpioClockInit(GPIOA);
vGpioClockInit(GPIOC);
// Enable TIM2 clock
vTimerClockInit(TIM2);
// Motors PWM: MOTOR1=left, MOTOR2=right ; A and B have opposed polarity
GPIO_InitTypeDef GPIO_InitStructure1 =
{
.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 | // MOTOR2_B, MOTOR2_A
GPIO_Pin_2 | GPIO_Pin_3 , // MOTOR1_B, MOTOR1_A
.GPIO_Mode = GPIO_Mode_AF_PP, // alternate function push pull
.GPIO_Speed = GPIO_Speed_2MHz
};
GPIO_Init(GPIOA, &GPIO_InitStructure1);
// Motors enable pin
GPIO_InitTypeDef GPIO_InitStructure2 =
{
.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1, // MOTOR1_EN, MOTOR2_EN
.GPIO_Mode = GPIO_Mode_Out_PP, // push pull
.GPIO_Speed = GPIO_Speed_2MHz
};
GPIO_Init(GPIOC, &GPIO_InitStructure2);
// Set output compare interrupt flags of channels configured in output
// (CCxS=00 in TIMx_CCMRx register) when counting up and down
TIM_CounterModeConfig(TIM2, TIM_CounterMode_CenterAligned3);
TIM_TimeBaseInitTypeDef Timer_InitStructure =
{
.TIM_ClockDivision = TIM_CKD_DIV1,
.TIM_Prescaler = DEFAULT_PSC,
.TIM_Period = PERIOD,
.TIM_CounterMode = TIM_CounterMode_Up
};
TIM_TimeBaseInit(TIM2, &Timer_InitStructure);
// Output Compare Init :
TIM_OCInitTypeDef OC_InitStructure;
TIM_OCStructInit(&OC_InitStructure);
OC_InitStructure.TIM_OCMode = TIM_OCMode_PWM1;
// Channel 1 & 2, left motor
TIM_OC1Init(TIM2, &OC_InitStructure);
TIM_OC1PreloadConfig(TIM2, TIM_OCPreload_Enable);
TIM_OC1PolarityConfig(TIM2, TIM_OCPolarity_High); // pos pwm
TIM_OC2Init(TIM2, &OC_InitStructure);
TIM_OC2PreloadConfig(TIM2, TIM_OCPreload_Enable);
TIM_OC2PolarityConfig(TIM2, TIM_OCPolarity_Low); // neg pwm
// Channel 3 & 4, right motor
TIM_OC3Init(TIM2, &OC_InitStructure);
TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable);
TIM_OC3PolarityConfig(TIM2, TIM_OCPolarity_High); // pos pwm
TIM_OC4Init(TIM2, &OC_InitStructure);
TIM_OC4PreloadConfig(TIM2, TIM_OCPreload_Enable);
TIM_OC4PolarityConfig(TIM2, TIM_OCPolarity_Low); // neg pwm
// Enables the TIM Capture Compare Channels
TIM_CCxCmd(TIM2, TIM_Channel_1, TIM_CCx_Enable);
TIM_CCxCmd(TIM2, TIM_Channel_2, TIM_CCx_Enable);
TIM_CCxCmd(TIM2, TIM_Channel_3, TIM_CCx_Enable);
TIM_CCxCmd(TIM2, TIM_Channel_4, TIM_CCx_Enable);
// Set default value: motors stopped
vMotorsDisable();
// Enables TIM peripheral Preload register on ARR
TIM_ARRPreloadConfig(TIM2, ENABLE);
TIM_Cmd(TIM2, ENABLE); // enable timer
// Create the daemon
xTaskCreate(vMotorsTask, (const signed char * const)"motorsd",
configMINIMAL_STACK_SIZE, NULL, motorsDaemonPriority_, NULL);
}
static void vMotorsReset()
{
previousCommand.motors = 0;
vMotorsApplyCommands(previousCommand);
}
void vMotorsEnable()
{
// We first stop the motors
vMotorsReset();
GPIO_SetBits(GPIOC, GPIO_Pin_0);
GPIO_SetBits(GPIOC, GPIO_Pin_1);
}
void vMotorsEnable()
{
// We stop motors because speed was zero
vMotorsReset();
GPIO_SetBits(GPIOC, GPIO_Pin_3);
}
