void COMP1Interrupt(void)
{
printf("c1i\n");
NVIC_DisableIRQ(COMP1_2_3_IRQn);
// TODO: ここでカウンタの値を操作して速度を制御する
TIM_CounterModeConfig(TIM4, TIM_CounterMode_Down);
NVIC_EnableIRQ(TIM4_IRQn);
}
void TIM4Interrupt(void)
{
printf("t4i\n");
NVIC_DisableIRQ(TIM4_IRQn);
// ここでDrivePhaseが更新される
Commutate();
switch (DrivePhase)
{
case 0:
PeripheralInit::InitCOMP1(PeripheralInit::Comp1InvInp::PA5, PeripheralInit::CompOutPol::NonInverted);
break;
case 1:
PeripheralInit::InitCOMP1(PeripheralInit::Comp1InvInp::PA4, PeripheralInit::CompOutPol::Inverted);
break;
case 2:
PeripheralInit::InitCOMP1(PeripheralInit::Comp1InvInp::PA0, PeripheralInit::CompOutPol::NonInverted);
break;
case 3:
PeripheralInit::InitCOMP1(PeripheralInit::Comp1InvInp::PA5, PeripheralInit::CompOutPol::Inverted);
break;
case 4:
PeripheralInit::InitCOMP1(PeripheralInit::Comp1InvInp::PA4, PeripheralInit::CompOutPol::NonInverted);
break;
case 5:
PeripheralInit::InitCOMP1(PeripheralInit::Comp1InvInp::PA0, PeripheralInit::CompOutPol::Inverted);
break;
default:
printf("drivephase outta range");
// 例外を送出してもよい．
break;
}
/*
* Phase0,2,4においては，各相の立ち下がりエッジを期待する．よって，出力極性は非反転．
*/
//PeripheralInit::InitCOMP1(PeripheralInit::Comp1InvInp::PA5, PeripheralInit::CompOutPol::NonInverted);
TIM_CounterModeConfig(TIM4, TIM_CounterMode_Up);
TIM_SetCounter(TIM4, 0);
NVIC_EnableIRQ(COMP1_2_3_IRQn);
}
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 vMotorsInit(unsigned portBASE_TYPE motors_daemon_priority,
unsigned portBASE_TYPE beeping_daemon_priority)
{
// Enable GPIOA & GPIOC clock
gpio_clock_init(GPIOA);
gpio_clock_init(GPIOC);
timer_clock_init(TIM5);
// 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_3, // MOTORS_ENABLE
.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 or down
TIM_CounterModeConfig(TIM5, TIM_CounterMode_CenterAligned3);
// robots.freehostia.com/SpeedControl/SpeedControllersBody.html#2.2
// f = 72MHz / 960 / 4 =~ 18kHz (P=5%, R=0.6ohms, L=100uH)
TIM_TimeBaseInitTypeDef Timer_InitStructure =
{
.TIM_ClockDivision = TIM_CKD_DIV1,
.TIM_Prescaler = DEFAULT_PSC,
.TIM_Period = PERIOD,
.TIM_CounterMode = TIM_CounterMode_Up
};
TIM_TimeBaseInit(TIM5, &Timer_InitStructure);
// Output Compare Init :
TIM_OCInitTypeDef OC_InitStructure;
TIM_OCStructInit(&OC_InitStructure);
OC_InitStructure.TIM_OCMode = TIM_OCMode_PWM2;
// Channel 1 & 2, left motor
TIM_OC1Init(TIM5, &OC_InitStructure);
TIM_OC1PreloadConfig(TIM5, TIM_OCPreload_Enable);
TIM_OC1PolarityConfig(TIM5, TIM_OCPolarity_High); // pos pwm
TIM_OC2Init(TIM5, &OC_InitStructure);
TIM_OC2PreloadConfig(TIM5, TIM_OCPreload_Enable);
TIM_OC2PolarityConfig(TIM5, TIM_OCPolarity_Low); // neg pwm
// Channel 3 & 4, right motor
TIM_OC3Init(TIM5, &OC_InitStructure);
TIM_OC3PreloadConfig(TIM5, TIM_OCPreload_Enable);
TIM_OC3PolarityConfig(TIM5, TIM_OCPolarity_High); // pos pwm
TIM_OC4Init(TIM5, &OC_InitStructure);
TIM_OC4PreloadConfig(TIM5, TIM_OCPreload_Enable);
TIM_OC4PolarityConfig(TIM5, TIM_OCPolarity_Low); // neg pwm
// Enables the TIM Capture Compare Channels
TIM_CCxCmd(TIM5, TIM_Channel_1, TIM_CCx_Enable);
TIM_CCxCmd(TIM5, TIM_Channel_2, TIM_CCx_Enable);
TIM_CCxCmd(TIM5, TIM_Channel_3, TIM_CCx_Enable);
TIM_CCxCmd(TIM5, TIM_Channel_4, TIM_CCx_Enable);
// Set default value: motor stopped
vMotorsDisable();
// Enables TIM5 peripheral Preload register on ARR
TIM_ARRPreloadConfig(TIM5, ENABLE);
TIM_Cmd(TIM5, ENABLE); // enable timer 5
// Create the beep mutex
xBeepMutex = xSemaphoreCreateMutex();
// Create the daemon
xTaskCreate(vMotorsTask, (const signed char * const)"motorsd",
configMINIMAL_STACK_SIZE, NULL, motors_daemon_priority, NULL);
xTaskCreate(beeping_daemon, (const signed char * const)"motorsd",
configMINIMAL_STACK_SIZE, NULL, beeping_daemon_priority, NULL);
}
static void vMotorsReset()
{
motors_command_t command;
command.motors = 0;
vMotorsApplyCommands(command);
previous_command.motors = 0;
}
