void TIM4_IRQHandler(void)
{
static u16 Drop_Capture = 0;
static u16 Rise_Capture = 0;
static u16 Rise1 = 0;
static u16 Rise2 = 0;
static u16 Distance1 = 0;
static u16 Distance2 = 0;
static u16 Delta_Distance = 0;
static u8 i = 0;
static u8 j = 0;
if (TIM_GetITStatus(TIM4, TIM_IT_CC1) != RESET) //捕获1发生捕获事件
{
TIM_ClearITPendingBit(TIM4, TIM_IT_CC1); //清除中断标志位
if(GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_6) == 1)
{
TIM_OC1PolarityConfig(TIM4,TIM_ICPolarity_Falling); //CC1P=1 设置为下降沿捕获
Rise_Capture=TIM_GetCapture1(TIM4);
if(i == 0)
{
Rise1 = Rise_Capture;
i = 1;
}
else
{
Rise2 = Rise_Capture;
i = 0;
}
pulse.period = (Rise1>Rise2)?(Rise1-Rise2):(Rise2-Rise1);//us
//Delta.dis =
//Speed = Delta.dis/pulse.period
Speed = Delta_Distance*1000/5069;// um*1000/us == m/s*1000 == mm/s
}
else
{
TIM_OC1PolarityConfig(TIM4,TIM_ICPolarity_Rising); //CC1P=0 设置为上升沿捕获
Drop_Capture=TIM_GetCapture1(TIM4);
if(Rise_Capture>Drop_Capture) pulse.width = 65535-Rise_Capture + Drop_Capture;
else pulse.width = Drop_Capture - Rise_Capture;
if(j == 0)
{
Distance1 = pulse.width;
j = 1;
}
else
{
Distance2 = pulse.width;
j = 0;
}
Delta_Distance = ((Distance1>Distance2)?(Distance1-Distance2):(Distance2-Distance1))*170;//um
}
}
}
static void Holder_PWM_Init()
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
RCC_APB1PeriphClockCmd(RCC_APB_HOLDER_TIM, ENABLE);
/* time base --> all pwm's period and prescaler */
TIM_TimeBaseStructure.TIM_Period = HOLDER_PERIOD;
TIM_TimeBaseStructure.TIM_Prescaler = HOLDER_PRESCALER - 1;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Down;
TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
TIM_TimeBaseInit(HOLDER_TIM, &TIM_TimeBaseStructure);
TIM_ARRPreloadConfig(HOLDER_TIM,ENABLE);
/* output channel --> every pwm's pulse */
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
/* channel 1 --> holder vertical */
TIM_OCInitStructure.TIM_Pulse = HOLDER_V_PULSE_INIT;
TIM_OC1Init(HOLDER_TIM, &TIM_OCInitStructure);
TIM_OC1PolarityConfig(HOLDER_TIM, TIM_OCPreload_Enable);
/* channel 2 --> holder horizontal */
TIM_OCInitStructure.TIM_Pulse = HOLDER_H_PULSE_INIT;
TIM_OC2Init(HOLDER_TIM, &TIM_OCInitStructure);
TIM_OC2PolarityConfig(HOLDER_TIM, TIM_OCPreload_Enable);
/* enable holder's pwm module */
TIM_Cmd(HOLDER_TIM, ENABLE);
}
//定时器5中断服务程序
void TIM5_IRQHandler(void)
{
if((TIM5CH1_CAPTURE_STA&0X80)==0)//还未成功捕获
{
if (TIM_GetITStatus(TIM5, TIM_IT_Update) != RESET)
{
if(TIM5CH1_CAPTURE_STA&0X40)//已经捕获到高电平了
{
if((TIM5CH1_CAPTURE_STA&0X3F)==0X3F)//高电平太长了
{
TIM5CH1_CAPTURE_STA|=0X80;//标记成功捕获了一次
TIM5CH1_CAPTURE_VAL=0XFFFF;
}else TIM5CH1_CAPTURE_STA++;
}
}
if (TIM_GetITStatus(TIM5, TIM_IT_CC1) != RESET)//捕获1发生捕获事件
{
if(TIM5CH1_CAPTURE_STA&0X40) //捕获到一个下降沿
{
TIM5CH1_CAPTURE_STA|=0X80; //标记成功捕获到一次上升沿
TIM5CH1_CAPTURE_VAL=TIM_GetCapture1(TIM5);
TIM_OC1PolarityConfig(TIM5,TIM_ICPolarity_Rising); //CC1P=0 设置为上升沿捕获
}else //还未开始,第一次捕获上升沿
{
TIM5CH1_CAPTURE_STA=0; //清空
TIM5CH1_CAPTURE_VAL=0;
TIM_SetCounter(TIM5,0);
TIM5CH1_CAPTURE_STA|=0X40; //标记捕获到了上升沿
TIM_OC1PolarityConfig(TIM5,TIM_ICPolarity_Falling); //CC1P=1 设置为下降沿捕获
}
}
}
TIM_ClearITPendingBit(TIM5, TIM_IT_CC1|TIM_IT_Update);
}
//配置中断处理函数
void TIM5_IRQHandler(void)
{
if((TIM5CH1_CAPTURE_STA&0x80)==0)//if capture not done
{
if(TIM_GetITStatus(TIM5,TIM_IT_Update)!=RESET)//if overflow occured
{
if(TIM5CH1_CAPTURE_STA&0x40)//rising has been captured
{
if((TIM5CH1_CAPTURE_STA&0x3f)==0x3f)//if overflow times FULL,end capture anyway
{
TIM5CH1_CAPTURE_STA|=0x80;//end capture
TIM5CH1_CAPTURE_VAL=0xffff;//full
}
else TIM5CH1_CAPTURE_STA++;//if overflow times not full,overflow times++
}
}
if(TIM_GetITStatus(TIM5,TIM_IT_CC1)!=RESET)//if captured rising/falling
{
if(TIM5CH1_CAPTURE_STA&0x40)//if rising has been captured,capture下降沿
{
TIM5CH1_CAPTURE_STA|=0x80;//done capture
TIM5CH1_CAPTURE_VAL=TIM_GetCapture1(TIM5);//get value
TIM_OC1PolarityConfig(TIM5,TIM_ICPolarity_Rising);//reset to original mode
}
else//if rising not captured,set all to init value,and set rising captured
{
TIM5CH1_CAPTURE_STA=0;
TIM5CH1_CAPTURE_VAL=0;
TIM_SetCounter(TIM5,0);
TIM5CH1_CAPTURE_STA|=0x40;//set rising captured
TIM_OC1PolarityConfig(TIM5,TIM_ICPolarity_Falling);//then capture falling
}
}
}
TIM_ClearITPendingBit(TIM5,TIM_IT_CC1|TIM_IT_Update);
}
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);
}
//定时器3中断服务程序
void TIM3_IRQHandler(void)
{
if((TIM3CH1_CAPTURE_STA&0X80)==0)//CH1 还未成功捕获
{
if (TIM_GetITStatus(TIM3, TIM_IT_CC1) != RESET)//捕获1发生捕获事件
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC1);
if(TIM3CH1_CAPTURE_STA&0X40) //捕获到一个下降沿为真 发生上升沿中断后再次发生中断为下降沿
{
TIM3CH1_CAPTURE_DOWNVAL=TIM_GetCapture1(TIM3); //获取当前的捕获值
if(TIM3CH1_CAPTURE_DOWNVAL<TIM3CH1_CAPTURE_UPVAL)
{
tim3_T=65535;
}
else tim3_T=0;
tempup1=TIM3CH1_CAPTURE_DOWNVAL-TIM3CH1_CAPTURE_UPVAL+tim3_T;//得到总的高电平时间
pwmout1=tempup1;
TIM3CH1_CAPTURE_STA=0;
TIM_OC1PolarityConfig(TIM3,TIM_ICPolarity_Rising); //CC1P=0 设置为上升沿捕获
}
else //发生捕获事件但不是下降沿，第一次捕获上升沿，清零，定时器开始计数
{
TIM3CH1_CAPTURE_UPVAL=TIM_GetCapture1(TIM3); //获取上升沿的数据
TIM3CH1_CAPTURE_STA|=0X40; //标记以捕获到了上升沿
TIM_OC1PolarityConfig(TIM3,TIM_ICPolarity_Falling); //CC1P=1 设置为下降沿捕获
}
}
}
if((TIM3CH2_CAPTURE_STA&0X80)==0)//CH1 还未成功捕获
{
if (TIM_GetITStatus(TIM3, TIM_IT_CC2) != RESET)//捕获1发生捕获事件
{
TIM_ClearITPendingBit(TIM3,TIM_IT_CC2);
if(TIM3CH2_CAPTURE_STA&0X40) //捕获到一个下降沿为真 发生上升沿中断后再次发生中断为下降沿
{
TIM3CH2_CAPTURE_DOWNVAL=TIM_GetCapture2(TIM3);
if(TIM3CH2_CAPTURE_DOWNVAL<TIM3CH2_CAPTURE_UPVAL)
{
tim3_T=65535;
}
else tim3_T=0;
tempup2=TIM3CH2_CAPTURE_DOWNVAL-TIM3CH2_CAPTURE_UPVAL+tim3_T;//得到总的高电平时间
pwmout2=tempup2;
TIM3CH2_CAPTURE_STA=0;
TIM_OC2PolarityConfig(TIM3,TIM_ICPolarity_Rising); //CC1P=0 设置为上升沿捕获
}
else //发生捕获事件但不是下降沿，第一次捕获上升沿，清零，定时器开始计数
{
TIM3CH2_CAPTURE_UPVAL=TIM_GetCapture2(TIM3); //获取上升沿的数据
TIM3CH2_CAPTURE_STA|=0X40; //标记以捕获到了上升沿
TIM_OC2PolarityConfig(TIM3,TIM_ICPolarity_Falling); //CC1P=1 设置为下降沿捕获
}
}
}
if((TIM3CH3_CAPTURE_STA&0X80)==0)//CH3 还未成功捕获
{
if (TIM_GetITStatus(TIM3, TIM_IT_CC3) != RESET)//捕获3发生捕获事件
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC3);
if(TIM3CH3_CAPTURE_STA&0X40) //捕获到一个下降沿为真 发生上升沿中断后再次发生中断为下降沿
{
TIM3CH3_CAPTURE_DOWNVAL=TIM_GetCapture3(TIM3);
if(TIM3CH3_CAPTURE_DOWNVAL<TIM3CH3_CAPTURE_UPVAL)
{
tim3_T=65535;
}
else tim3_T=0;
tempup3=TIM3CH3_CAPTURE_DOWNVAL-TIM3CH3_CAPTURE_UPVAL+tim3_T;//得到总的高电平时间
pwmout3=tempup3;
TIM3CH3_CAPTURE_STA=0;
TIM_OC3PolarityConfig(TIM3,TIM_ICPolarity_Rising); //CC3P=0 设置为上升沿捕获
}
else //发生捕获事件但不是下降沿，第一次捕获上升沿，清零，定时器开始计数
{
TIM3CH3_CAPTURE_UPVAL=TIM_GetCapture3(TIM3); //获取上升沿的数据
TIM3CH3_CAPTURE_STA|=0X40; //标记以捕获到了上升沿
TIM_OC3PolarityConfig(TIM3,TIM_ICPolarity_Falling); //CC1P=1 设置为下降沿捕获
}
}
}
if((TIM3CH4_CAPTURE_STA&0X80)==0)//CH4 还未成功捕获
{
if (TIM_GetITStatus(TIM3, TIM_IT_CC4) != RESET)//捕获1发生捕获事件
{
TIM_ClearITPendingBit(TIM3,TIM_IT_CC4);
if(TIM3CH4_CAPTURE_STA&0X40) //捕获到一个下降沿为真 发生上升沿中断后再次发生中断为下降沿
{
TIM3CH4_CAPTURE_DOWNVAL=TIM_GetCapture4(TIM3);
if(TIM3CH4_CAPTURE_DOWNVAL<TIM3CH4_CAPTURE_UPVAL)
{
tim3_T=65535;
}
else tim3_T=0;
tempup4=TIM3CH4_CAPTURE_DOWNVAL-TIM3CH4_CAPTURE_UPVAL+tim3_T;//得到总的高电平时间
pwmout4=tempup4;
TIM3CH4_CAPTURE_STA=0;
TIM_OC4PolarityConfig(TIM3,TIM_ICPolarity_Rising); //CC1P=0 设置为上升沿捕获
}
else //发生捕获事件但不是下降沿，第一次捕获上升沿，清零，定时器开始计数
{
TIM3CH4_CAPTURE_UPVAL=TIM_GetCapture4(TIM3); //获取上升沿的数据
TIM3CH4_CAPTURE_STA|=0X40; //标记以捕获到了上升沿
TIM_OC4PolarityConfig(TIM3,TIM_ICPolarity_Falling); //CC4P=1 设置为下降沿捕获
}
}
}
}
//定时器4中断服务程序,用CH1作为超声波转换的通道 即PB6端口
void TIM4_IRQHandler(void)
{
if ((TIM4CH1_CAPTURE_STA & 0X80) == 0) //还未成功捕获
{
if (TIM_GetITStatus(TIM4, TIM_IT_CC1) != RESET) //捕获1发生捕获事件
{
TIM_ClearITPendingBit(TIM4, TIM_IT_CC1); //清除中断标志位
if (TIM4CH1_CAPTURE_STA & 0X40) //捕获到一个下降沿
{
TIM4CH1_CAPTURE_DOWNVAL = TIM_GetCapture1(TIM4);//记录下此时的定时器计数值
if (TIM4CH1_CAPTURE_DOWNVAL < TIM4CH1_CAPTURE_UPVAL)
{/* 如果计数器初始值大于末尾值，说明计数器有溢出 */
tim4_T1 = 65535;
}
else
tim4_T1 = 0;
tempup1 = TIM4CH1_CAPTURE_DOWNVAL - TIM4CH1_CAPTURE_UPVAL
+ tim4_T1; //得到总的高电平的时间
//pwmout1 = tempup1; //总的高电平的时间
tempup1 =tempup1 *17/1000;//计算距离&&UltrasonicWave_Distance<85
TIM4CH1_CAPTURE_STA = 0; //捕获标志位清零，这一步很重要！
TIM_OC1PolarityConfig(TIM4, TIM_ICPolarity_Rising); //设置为上升沿捕获
}
else //发生捕获时间但不是下降沿，第一次捕获到上升沿，记录此时的定时器计数值
{
TIM4CH1_CAPTURE_UPVAL = TIM_GetCapture1(TIM4); //获取上升沿数据
TIM4CH1_CAPTURE_STA |= 0X40; //标记已捕获到上升沿
TIM_OC1PolarityConfig(TIM4, TIM_ICPolarity_Falling);//设置为下降沿捕获
}
}
}
if ((TIM4CH2_CAPTURE_STA & 0X80) == 0) //还未成功捕获
{
if (TIM_GetITStatus(TIM4, TIM_IT_CC2) != RESET) //捕获2发生捕获事件
{
TIM_ClearITPendingBit(TIM4, TIM_IT_CC2); //清除中断标志位
if (TIM4CH2_CAPTURE_STA & 0X40) //捕获到一个下降沿
{
TIM4CH2_CAPTURE_DOWNVAL = TIM_GetCapture2(TIM4);//记录下此时的定时器计数值
if (TIM4CH2_CAPTURE_DOWNVAL < TIM4CH2_CAPTURE_UPVAL)
{
tim4_T2 = 65535;
}
else
tim4_T2 = 0;
tempup2 = TIM4CH2_CAPTURE_DOWNVAL - TIM4CH2_CAPTURE_UPVAL
+ tim4_T2; //得到总的高电平的时间
pwmout2 = tempup2; //总的高电平的时间
TIM4CH2_CAPTURE_STA = 0; //捕获标志位清零
TIM_OC2PolarityConfig(TIM4, TIM_ICPolarity_Rising); //设置为上升沿捕获
}
else //发生捕获时间但不是下降沿，第一次捕获到上升沿，记录此时的定时器计数值
{
TIM4CH2_CAPTURE_UPVAL = TIM_GetCapture2(TIM4); //获取上升沿数据
TIM4CH2_CAPTURE_STA |= 0X40; //标记已捕获到上升沿
TIM_OC2PolarityConfig(TIM4, TIM_ICPolarity_Falling);//设置为下降沿捕获
}
}
}
if ((TIM4CH3_CAPTURE_STA & 0X80) == 0) //还未成功捕获
{
if (TIM_GetITStatus(TIM4, TIM_IT_CC3) != RESET) //捕获3发生捕获事件
{
TIM_ClearITPendingBit(TIM4, TIM_IT_CC3); //清除中断标志位
if (TIM4CH3_CAPTURE_STA & 0X40) //捕获到一个下降沿
{
TIM4CH3_CAPTURE_DOWNVAL = TIM_GetCapture3(TIM4);//记录下此时的定时器计数值
if (TIM4CH3_CAPTURE_DOWNVAL < TIM4CH3_CAPTURE_UPVAL)
{
tim4_T3 = 65535;
}
else
tim4_T3 = 0;
tempup3 = TIM4CH3_CAPTURE_DOWNVAL - TIM4CH3_CAPTURE_UPVAL
+ tim4_T3; //得到总的高电平的时间
pwmout3 = tempup3; //总的高电平的时间
TIM4CH3_CAPTURE_STA = 0; //捕获标志位清零
TIM_OC3PolarityConfig(TIM4, TIM_ICPolarity_Rising); //设置为上升沿捕获
}
else //发生捕获时间但不是下降沿，第一次捕获到上升沿，记录此时的定时器计数值
{
TIM4CH3_CAPTURE_UPVAL = TIM_GetCapture3(TIM4); //获取上升沿数据
TIM4CH3_CAPTURE_STA |= 0X40; //标记已捕获到上升沿
TIM_OC3PolarityConfig(TIM4, TIM_ICPolarity_Falling);//设置为下降沿捕获
}
}
}
if ((TIM4CH4_CAPTURE_STA & 0X80) == 0) //还未成功捕获
{
if (TIM_GetITStatus(TIM4, TIM_IT_CC4) != RESET) //捕获4发生捕获事件
{
TIM_ClearITPendingBit(TIM4, TIM_IT_CC4); //清除中断标志位
if (TIM4CH4_CAPTURE_STA & 0X40) //捕获到一个下降沿
{
TIM4CH4_CAPTURE_DOWNVAL = TIM_GetCapture4(TIM4);//记录下此时的定时器计数值
if (TIM4CH4_CAPTURE_DOWNVAL < TIM4CH4_CAPTURE_UPVAL)
{
tim4_T4 = 65535;
}
else
tim4_T4 = 0;
tempup4 = TIM4CH4_CAPTURE_DOWNVAL - TIM4CH4_CAPTURE_UPVAL
+ tim4_T4; //得到总的高电平的时间
pwmout4 = tempup4; //总的高电平的时间
TIM4CH4_CAPTURE_STA = 0; //捕获标志位清零
TIM_OC4PolarityConfig(TIM4, TIM_ICPolarity_Rising); //设置为上升沿捕获
}
else //发生捕获时间但不是下降沿，第一次捕获到上升沿，记录此时的定时器计数值
{
TIM4CH4_CAPTURE_UPVAL = TIM_GetCapture4(TIM4); //获取上升沿数据
TIM4CH4_CAPTURE_STA |= 0X40; //标记已捕获到上升沿
TIM_OC4PolarityConfig(TIM4, TIM_ICPolarity_Falling);//设置为下降沿捕获
}
}
}
}
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;
}
void TIM3_IRQHandler(void)
{
if (TIM_GetITStatus(TIM3, TIM_IT_CC1) != RESET) //捕获1发生捕获事件
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC1);
if (GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_6) == 1)
{
TIM_OC1PolarityConfig(TIM3, TIM_ICPolarity_Falling);
pwm_rise[4] = TIM_GetCapture1(TIM3);
}
else
{
TIM_OC1PolarityConfig(TIM3, TIM_ICPolarity_Rising);
pwm_drop[4] = TIM_GetCapture1(TIM3);
if (pwm_rise[4] > pwm_drop[4])
{
pwm_in[4] = 65535 - pwm_rise[4] + pwm_drop[4];
}
else
{
pwm_in[4] = pwm_drop[4] - pwm_rise[4];
}
}
}
if (TIM_GetITStatus(TIM3, TIM_IT_CC2) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC2);
if (GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_7) == 1)
{
TIM_OC2PolarityConfig(TIM3, TIM_ICPolarity_Falling);
pwm_rise[5] = TIM_GetCapture2(TIM3);
}
else
{
TIM_OC2PolarityConfig(TIM3, TIM_ICPolarity_Rising);
pwm_drop[5] = TIM_GetCapture2(TIM3);
if (pwm_rise[5] > pwm_drop[5])
{
pwm_in[5] = 65535 - pwm_rise[5] + pwm_drop[5];
}
else
{
pwm_in[5] = pwm_drop[5] - pwm_rise[5];
}
}
}
if (TIM_GetITStatus(TIM3, TIM_IT_CC3) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC3);
if (GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_0) == 1)
{
TIM_OC3PolarityConfig(TIM3, TIM_ICPolarity_Falling);
pwm_rise[6] = TIM_GetCapture3(TIM3);
}
else
{
TIM_OC3PolarityConfig(TIM3, TIM_ICPolarity_Rising);
pwm_drop[6] = TIM_GetCapture3(TIM3);
if (pwm_rise[6] > pwm_drop[6])
{
pwm_in[6] = 65535 - pwm_rise[6] + pwm_drop[6];
}
else
{
pwm_in[6] = pwm_drop[6] - pwm_rise[6];
}
}
}
if (TIM_GetITStatus(TIM3, TIM_IT_CC4) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC4);
if (GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_1) == 1)
{
TIM_OC4PolarityConfig(TIM3, TIM_ICPolarity_Falling);
pwm_rise[7] = TIM_GetCapture4(TIM3);
}
else
{
TIM_OC4PolarityConfig(TIM3, TIM_ICPolarity_Rising);
pwm_drop[7] = TIM_GetCapture4(TIM3);
if (pwm_rise[7] > pwm_drop[7])
{
pwm_in[7] = 65535 - pwm_rise[7] + pwm_drop[7];
}
else
{
pwm_in[7] = pwm_drop[7] - pwm_rise[7];
}
}
}
}
void TIM2_IRQHandler(void)
{
if (TIM_GetITStatus(TIM2, TIM_IT_CC1) != RESET)
{
TIM_ClearITPendingBit(TIM2, TIM_IT_CC1);
if (GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_0) == 1)
{
//CC1P=2 设置为下降沿捕获
TIM_OC1PolarityConfig(TIM2, TIM_ICPolarity_Falling);
pwm_rise[0] = TIM_GetCapture1(TIM2);
}
else
{
//CC1P=0 设置为上升沿捕获
TIM_OC1PolarityConfig(TIM2, TIM_ICPolarity_Rising);
pwm_drop[0] = TIM_GetCapture1(TIM2);
if (pwm_rise[0] > pwm_drop[0])
{
pwm_in[0] = 65535 - pwm_rise[0] + pwm_drop[0];
}
else
{
pwm_in[0] = pwm_drop[0] - pwm_rise[0];
}
}
}
if (TIM_GetITStatus(TIM2, TIM_IT_CC2) != RESET)
{
TIM_ClearITPendingBit(TIM2, TIM_IT_CC2);
if (GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_1) == 1)
{
TIM_OC2PolarityConfig(TIM2, TIM_ICPolarity_Falling);
pwm_rise[1] = TIM_GetCapture2(TIM2);
}
else
{
TIM_OC2PolarityConfig(TIM2, TIM_ICPolarity_Rising);
pwm_drop[1] = TIM_GetCapture2(TIM2);
if (pwm_rise[1] > pwm_drop[1])
{
pwm_in[1] = 65535 - pwm_rise[1] + pwm_drop[1];
}
else
{
pwm_in[1] = pwm_drop[1] - pwm_rise[1];
}
}
}
if (TIM_GetITStatus(TIM2, TIM_IT_CC3) != RESET)
{
TIM_ClearITPendingBit(TIM2, TIM_IT_CC3);
if (GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_2) == 1)
{
TIM_OC3PolarityConfig(TIM2, TIM_ICPolarity_Falling);
pwm_rise[2] = TIM_GetCapture3(TIM2);
}
else
{
TIM_OC3PolarityConfig(TIM2, TIM_ICPolarity_Rising);
pwm_drop[2] = TIM_GetCapture3(TIM2);
if (pwm_rise[2] > pwm_drop[2])
{
pwm_in[2] = 65535 - pwm_rise[2] + pwm_drop[2];
}
else
{
pwm_in[2] = pwm_drop[2] - pwm_rise[2];
}
}
}
if (TIM_GetITStatus(TIM2, TIM_IT_CC4) != RESET)
{
TIM_ClearITPendingBit(TIM2, TIM_IT_CC4);
if (GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_3) == 1)
{
TIM_OC4PolarityConfig(TIM2, TIM_ICPolarity_Falling);
pwm_rise[3] = TIM_GetCapture4(TIM2);
}
else
{
TIM_OC4PolarityConfig(TIM2, TIM_ICPolarity_Rising);
pwm_drop[3] = TIM_GetCapture4(TIM2);
if (pwm_rise[3] > pwm_drop[3])
{
pwm_in[3] = 65535 - pwm_rise[3] + pwm_drop[3];
}
else
{
pwm_in[3] = pwm_drop[3] - pwm_rise[3];
}
pwm_in_error_count = 0;
}
}
}
