void Motion_Control(void)
{
if(PID_main_Speed.Out > 35) //主机方向控制
{
Moter_main_forward;
TIM_SetCompare1(TIM4,PID_main_Speed.Out);
}
else if(PID_main_Speed.Out < -35)
{
Moter_main_back;
TIM_SetCompare1(TIM4,-PID_main_Speed.Out);
}
else
{
Moter_main_stop;
TIM_SetCompare1(TIM4,0);
}
if(PID_101_Speed.Out > 35) //从机101放向控制
{
Moter_101_forward;
TIM_SetCompare2(TIM4,PID_101_Speed.Out);
}
else if(PID_101_Speed.Out < -35)
{
Moter_101_back;
TIM_SetCompare2(TIM4,-PID_101_Speed.Out);
}
else
{
Moter_101_stop;
TIM_SetCompare2(TIM4,0);
}
}
// @brief : Called from TIM5_IRQHandler, whenever TIM5 CH1 generates an interrupt
// or TIM5 generates an update. This function serves two purposes. Firstly,
// it will be used to debounce the switch after it is pressed or released
// (interrupt). Secondly, it is used to notify the user of the maximum hold
// time (update). This is part 2 of the switch service routine.
// @param : none
// @retval: none
void SWService2 (void) {
if (TIM_GetITStatus(TIM_SWITCH, TIM_IT_CC1) != RESET) {
TIM_ClearITPendingBit(TIM_SWITCH, TIM_IT_CC1);
if (SWstate == SW_DEBOUNCE_HIGH) { // previous state is debounce_high.
if (USER_SW) { // SW debounced and remained high (PRESSED / HELD)
SWstate = SW_HIGH;
SWstatus = SW_PRESS;
TIM_SetCompare2(TIM_SWITCH, sw_hold_time[SWstatus]);
bSWFlag = TRUE;
}
else { // SW debounced and went low (RELEASED)
SWstate = SW_DEBOUNCE_LOW;
TIM_SetCounter(TIM_SWITCH, 0); // TIM5->CNT = 0;
}
}
else if (SWstate == SW_DEBOUNCE_LOW) { // previous state is debounce_low
SWstate = SW_LOW;
TIM_Cmd(TIM_SWITCH, DISABLE); // Disabling timer DOESN'T reset the counter
TIM_SetCounter(TIM_SWITCH, 0);
}
}
else if (TIM_GetITStatus(TIM_SWITCH, TIM_IT_CC2) != RESET) {
TIM_ClearITPendingBit(TIM_SWITCH, TIM_IT_CC2);
SWstatus++;
TIM_SetCompare2(TIM_SWITCH, sw_hold_time[SWstatus]);
DispBlink();
}
}
void loop(void) {
int32_t data[3];
LIS302DL_ReadACC(data);
int32_t x = cround(data[0], 300)/5;
int32_t y = cround(data[1], 300)/5;
TIM_SetCompare1(TIM4, 0);
TIM_SetCompare2(TIM4, 0);
TIM_SetCompare3(TIM4, 0);
TIM_SetCompare4(TIM4, 0);
if ( x > 0 ) {
TIM_SetCompare2(TIM4, x);
}
else {
TIM_SetCompare4(TIM4, -x);
}
if ( y > 0 ) {
TIM_SetCompare1(TIM4, y);
}
else {
TIM_SetCompare3(TIM4, -y);
}
delay(10);
}
void batholithResetPwm(){
TIM_SetCompare2(TIM8,0);
TIM_SetCompare3(TIM8,0);
TIM_SetCompare2(TIM1,0);
TIM_SetCompare1(TIM1,0);
me.isPWMallowed=0;
}
/* ---------------------------------------------------------------------------*/
void setLeftMotor(int32_t m_speed){
if(m_speed >= 0){
TIM_SetCompare1(TIM4, m_speed);
TIM_SetCompare2(TIM4, 0);
}else{
TIM_SetCompare1(TIM4, 0);
TIM_SetCompare2(TIM4, -m_speed);
}
}
void MOTORS_RMS(uint8_t speed) {
if (speed >= 50) {
TIM_SetCompare2(TIM3, (speed-50) * 20);
GPIO_SetBits(GPIOB, GPIO_Pin_8);
GPIO_ResetBits(GPIOB, GPIO_Pin_9);
} else {
TIM_SetCompare2(TIM3, speed * 20);
GPIO_ResetBits(GPIOB, GPIO_Pin_8);
GPIO_SetBits(GPIOB, GPIO_Pin_9);
}
}
void Holder_ChangePulse()
{
switch (holder_state)
{
case HOLDER_STATE_TURNUP:
if (holder_v_pulse + holder_pulse_step > HOLDER_V_PULSE_MAX)
{
holder_v_pulse = HOLDER_V_PULSE_MAX;
}
else
{
holder_v_pulse += holder_pulse_step;
}
TIM_SetCompare1(HOLDER_TIM, holder_v_pulse);
break;
case HOLDER_STATE_TURNDOWN:
if (holder_v_pulse - holder_pulse_step < HOLDER_V_PULSE_MIN)
{
holder_v_pulse = HOLDER_V_PULSE_MIN;
}
else
{
holder_v_pulse -= holder_pulse_step;
}
TIM_SetCompare1(HOLDER_TIM, holder_v_pulse);
break;
case HOLDER_STATE_TURNLEFT:
if (holder_h_pulse + holder_pulse_step > HOLDER_H_PULSE_MAX)
{
holder_h_pulse = HOLDER_H_PULSE_MAX;
}
else
{
holder_h_pulse += holder_pulse_step;
}
TIM_SetCompare2(HOLDER_TIM, holder_h_pulse);
break;
case HOLDER_STATE_TURNRIGHT:
if (holder_h_pulse - holder_pulse_step < HOLDER_H_PULSE_MIN)
{
holder_h_pulse = HOLDER_H_PULSE_MIN;
}
else
{
holder_h_pulse -= holder_pulse_step;
}
TIM_SetCompare2(HOLDER_TIM, holder_h_pulse);
break;
default:
break;
}
}
/************************************************************************************************
* 函数名：turn_to_left
* 函数作用：控制小车右转
* 参数：yscs (预设次数) 预设光电传感器输出方波的次数 pwm_x 时分别给左右两个电机输出不同占空比的pwm波
* 库版本 ：ST3.5.0
*************************************************************************************************/
void turn_to_left( u16 pwm_1 , u16 pwm_2,u16 yanshi )
{
//yushe_cishu=yscs;
TIM_SetCompare1(TIM4,pwm_1);//设置电机的初始pwn波 1、2是控制右侧的电机，3、4控制左侧的电机。1、3是前进,2、4是后退。
TIM_SetCompare2(TIM4,0);
TIM_SetCompare3(TIM4,0);
TIM_SetCompare4(TIM4,pwm_2);
delay_ms(yanshi);//根据实际情况更改
TIM_SetCompare1(TIM4,0);//设置电机的初始pwn波 1、2是控制右侧的电机，3、4控制左侧的电机。1、3是前进,2、4是后退。
TIM_SetCompare2(TIM4,0);
TIM_SetCompare3(TIM4,0);
TIM_SetCompare4(TIM4,0);
}
/* set pulse widths from actuator values, assumed to be in us */
void l_actuators_pwm_commit(void) {
TIM_SetCompare1(TIM3, actuators_pwm_values[0]);
TIM_SetCompare2(TIM3, actuators_pwm_values[1]);
TIM_SetCompare3(TIM3, actuators_pwm_values[2]);
TIM_SetCompare4(TIM3, actuators_pwm_values[3]);
TIM_SETCOMPARE(PWM5_OC)(PWM_5AND6_TIMER, actuators_pwm_values[4]);
TIM_SETCOMPARE(PWM6_OC)(PWM_5AND6_TIMER, actuators_pwm_values[5]);
#ifdef USE_SERVOS_7AND8
TIM_SetCompare1(TIM4, actuators_pwm_values[6]);
TIM_SetCompare2(TIM4, actuators_pwm_values[7]);
#endif
}
void PWM_Set(int moto1, int moto2, int moto3, int moto4)
{
PWM_Init(GPIO_Pin_3);
/*output*/
TIM_SetCompare2(TIM8,moto1);
PWM_Init(GPIO_Pin_4);
TIM_SetCompare2(TIM8,moto2);
PWM_Init(GPIO_Pin_5);
TIM_SetCompare2(TIM8,moto3);
PWM_Init(GPIO_Pin_6);
TIM_SetCompare2(TIM8,moto4);
}
/*差速算法，速度合成*/
void Direction_Dispose(void)//Dispose:处理
{
int Direction_Read;
Direction_Read=ADC_ConvertedValue[1];//读取数据
Direction_Difference=(ADC_Direction-Direction_Read)*0.1;//差=原始数据-现在数据
/*pwm电压调节器，电池电压变化pwm不变*/
// （读出数据/4096）*3.3*21（倍）=实际电压 原始pwm*（32/测量电压）=输出pwm
// ADC_ConvertedValueLocal =(float) ADC_ConvertedValue[2]/4096*3.28*21;
// P=32/ADC_ConvertedValueLocal;
if(int_PWM>0)//判断向前还是向后，如果向前
{
if(Direction_Difference>0)//判断转向数据正数还是负数，如果证书
{
PWM_R=int_PWM-(int_PWM*Direction_Difference/100);//向前的PWM数据-转向值=右//读取PWM信号*（读取的转向/100），意思就是差速信号是转向是PWM的百分比
PWM_L=int_PWM+(int_PWM*Direction_Difference/100);//左边转向值
} //机器向右转
else
{
PWM_R=int_PWM-(int_PWM*Direction_Difference/100);//右边转向值
PWM_L=int_PWM+(int_PWM*Direction_Difference/100);//左边转向值
} //机器向左转
TIM_SetCompare1(TIM1,PWM_L);//送入左电机，第一通道，设定向前运动
TIM_SetCompare1(TIM8,PWM_R);//送入右电机，第一通道，设定向前运动
}
else
{
if(Direction_Difference>0)
{
PWM_R=(-int_PWM)-(int_PWM*Direction_Difference/100);//向前的PWM数据-转向值=右边转向值
PWM_L=(-int_PWM)+(int_PWM*Direction_Difference/100);//左边转向值
} //机器向右转
else
{
PWM_R=(-int_PWM)-(int_PWM*Direction_Difference/100);//右边转向值
PWM_L=(-int_PWM)+(int_PWM*Direction_Difference/100);//左边转向值
} //机器向左转
TIM_SetCompare2(TIM1,PWM_L);//送入左电机，第二通道，设定电机向后运动
TIM_SetCompare2(TIM8,PWM_R);//送入左电机，第二通道，设定电机向后运动
}
}
/**
* @brief This function handles TIM4 global interrupt request.
* @param None
* @retval None
*/
void TIM4_IRQHandler(void)
{
uint16_t capture = 0;
/* TIM4_CH1 toggling */
if (TIM_GetITStatus(TIM4, TIM_IT_CC1) != RESET)
{
TIM_ClearITPendingBit(TIM4, TIM_IT_CC1 );
capture = TIM_GetCapture1(TIM4);
TIM_SetCompare1(TIM4, capture + Channel1Pulse );
}
/* TIM4_CH2 toggling */
if (TIM_GetITStatus(TIM4, TIM_IT_CC2) != RESET)
{
TIM_ClearITPendingBit(TIM4, TIM_IT_CC2);
capture = TIM_GetCapture2(TIM4);
TIM_SetCompare2(TIM4, capture + Channel2Pulse);
}
/* TIM4_CH3 toggling */
if (TIM_GetITStatus(TIM4, TIM_IT_CC3) != RESET)
{
TIM_ClearITPendingBit(TIM4, TIM_IT_CC3);
capture = TIM_GetCapture3(TIM4);
TIM_SetCompare3(TIM4, capture + Channel3Pulse);
}
/* TIM4_CH4 toggling */
if (TIM_GetITStatus(TIM4, TIM_IT_CC4) != RESET)
{
TIM_ClearITPendingBit(TIM4, TIM_IT_CC4);
capture = TIM_GetCapture4(TIM4);
TIM_SetCompare4(TIM4, capture + Channel4Pulse);
}
/* TIM_Update reset all */
if (TIM_GetITStatus(TIM4, TIM_IT_Update) != RESET)
{
TIM_ClearITPendingBit(TIM4, TIM_IT_Update);
TIM_SetCompare1(TIM4, Channel1Pulse);
TIM_SetCompare2(TIM4, Channel2Pulse);
TIM_SetCompare3(TIM4, Channel3Pulse);
TIM_SetCompare4(TIM4, Channel4Pulse);
}
}
void Move (uint8_t chan, float angle, servo type)
{
switch(chan)
{
case 1:
TIM_SetCompare1(TIM12, AngleToDuty(angle, type));
break;
case 2:
TIM_SetCompare2(TIM12, AngleToDuty(angle, type));
break;
case 3:
TIM_SetCompare1(TIM3, AngleToDuty(angle, type));
break;
case 4:
TIM_SetCompare2(TIM3, AngleToDuty(angle, type));
break;
case 5:
TIM_SetCompare3(TIM3, AngleToDuty(angle, type));
break;
case 6:
TIM_SetCompare1(TIM4, AngleToDuty(angle, type));
break;
case 7:
TIM_SetCompare2(TIM4, AngleToDuty(angle, type));
break;
case 8:
TIM_SetCompare3(TIM4, AngleToDuty(angle, type));
break;
case 9:
TIM_SetCompare4(TIM4, AngleToDuty(angle, type));
break;
case 10:
TIM_SetCompare1(TIM4, AngleToDuty(angle, type));
break;
default:
break;
}
}
//后右电机控制
void motorHY_control(TIM_TypeDef* TMM , uint16_t PWMPulse, u8 Channel, u8 Cmd)
{
//PWM控制
if(Channel==1)
TIM_SetCompare1(TMM, PWMPulse);
else if(Channel==2)
TIM_SetCompare2(TMM, PWMPulse);
else if(Channel==3)
TIM_SetCompare3(TMM, PWMPulse);
else if(Channel==4)
TIM_SetCompare4(TMM, PWMPulse);
//模式控制
if(Cmd==1) //模式0，停止
{
MotorHY_IN1_Reset;
MotorHY_IN2_Reset;
}
if(Cmd==2) //模式1，正转
{
MotorHY_IN1_Set;
MotorHY_IN2_Reset;
}
if(Cmd==3) //模式2，反转
{
MotorHY_IN1_Reset;
MotorHY_IN2_Set;
}
if(Cmd==4) //模式3，刹车
{
MotorHY_IN1_Set;
MotorHY_IN2_Set;
}
}
void pwm_out_set_value()
{
TIM_SetCompare1(TIM4, pwm_out[0]);
TIM_SetCompare2(TIM4, pwm_out[1]);
TIM_SetCompare3(TIM4, pwm_out[2]);
TIM_SetCompare4(TIM4, pwm_out[3]);
}
//电机控制函数，控制输出PWM的占空比。
void motor_control(void) //初始化电机控制
{
TIM_SetCompare1(TIM2, 0);
TIM_SetCompare2(TIM2, 0);
TIM_SetCompare3(TIM2, 0);
TIM_SetCompare4(TIM2, 0);
}
Status StartCompareTimer(int channel, int ticks)
{
Status status = OK;
switch (channel)
{
case TIMER_CHANNEL_1:
TIM_SetCompare1(COMPARE_TIMER, (uint16_t) (TIM_GetCounter(COMPARE_TIMER) + ticks));
break;
case TIMER_CHANNEL_2:
TIM_SetCompare2(COMPARE_TIMER, (uint16_t) (TIM_GetCounter(COMPARE_TIMER) + ticks));
break;
case TIMER_CHANNEL_3:
TIM_SetCompare3(COMPARE_TIMER, (uint16_t) (TIM_GetCounter(COMPARE_TIMER) + ticks));
break;
case TIMER_CHANNEL_4:
TIM_SetCompare4(COMPARE_TIMER, (uint16_t) (TIM_GetCounter(COMPARE_TIMER) + ticks));
break;
default:
status = "InvalidComparatorId";
break;
}
if (status == OK)
{
TIM_ClearITPendingBit(COMPARE_TIMER, channel);
TIM_ITConfig(COMPARE_TIMER, channel, ENABLE);
}
return status;
}
void TIMER2_CH2_PWM_Init(int prescaler,int autoreload){
//USER LED / PB3 / TIM2_CH2 / AF2
RCC_AHBPeriphClockCmd(RCC_AHBENR_GPIOBEN ,ENABLE);
GPIO_InitTypeDef myGPIO;
GPIO_StructInit(&myGPIO);
myGPIO.GPIO_Pin=GPIO_Pin_3;
myGPIO.GPIO_Mode=GPIO_Mode_AF;
GPIO_Init(GPIOB,&myGPIO);
GPIO_PinAFConfig(GPIOB,GPIO_PinSource3,GPIO_AF_2);
//select the output mode by writing the CCS bits in the CCMRx register
//Timer time base configuration
RCC_APB1PeriphClockCmd(RCC_APB1ENR_TIM2EN,ENABLE);
TIM_TimeBaseInitTypeDef myTimeBase;
TIM_TimeBaseStructInit(&myTimeBase);
myTimeBase.TIM_CounterMode=TIM_CounterMode_Up;
myTimeBase.TIM_Period=autoreload;
myTimeBase.TIM_Prescaler=prescaler;
myTimeBase.TIM_ClockDivision= TIM_CKD_DIV1;
TIM_TimeBaseInit(TIM2,&myTimeBase);
//Timer capture compare configuration
TIM_OCInitTypeDef myTimerOC;
TIM_OCStructInit(&myTimerOC);
myTimerOC.TIM_OCMode=TIM_OCMode_PWM1;
myTimerOC.TIM_OCPolarity=TIM_OCPolarity_High;
myTimerOC.TIM_OutputState=TIM_OutputState_Enable;
myTimerOC.TIM_Pulse=autoreload;//0 Duty cycle at start
TIM_OC2Init(TIM2,&myTimerOC);
TIM_CCxCmd(TIM2,TIM_Channel_2,TIM_CCx_Enable);//enable CCP2
//start Timer
TIM_SetCompare2(TIM2,0);
TIM_Cmd(TIM2,ENABLE);//Counter enabled
}
/**
* @brief This function handles timer 8 capture/compare interrupt request.
* @retval None
*/
void TIM8_CC_IRQHandler(void) {
if (TIM_GetITStatus(TIM8, TIM_IT_CC1) != RESET) {
TIM_ClearITPendingBit(TIM8, TIM_IT_CC1);
if(TIM_Handler[TIM8CH1] != NULL) {
TIM_Handler[TIM8CH1]();
}
TIM_SetCompare1(TIM8, TIM_GetCapture1(TIM8) + TIM_IRQ_period[TIM8CH1]);
}
else if (TIM_GetITStatus(TIM8, TIM_IT_CC2) != RESET) {
TIM_ClearITPendingBit(TIM8, TIM_IT_CC2);
if(TIM_Handler[TIM8CH2] != NULL) {
TIM_Handler[TIM8CH2]();
}
TIM_SetCompare2(TIM8, TIM_GetCapture2(TIM8) + TIM_IRQ_period[TIM8CH2]);
}
else if (TIM_GetITStatus(TIM8, TIM_IT_CC3) != RESET) {
TIM_ClearITPendingBit(TIM8, TIM_IT_CC3);
if(TIM_Handler[TIM8CH3] != NULL) {
TIM_Handler[TIM8CH3]();
}
TIM_SetCompare3(TIM8, TIM_GetCapture3(TIM8) + TIM_IRQ_period[TIM8CH3]);
}
else if (TIM_GetITStatus(TIM8, TIM_IT_CC4) != RESET) {
TIM_ClearITPendingBit(TIM8, TIM_IT_CC4);
if(TIM_Handler[TIM8CH4] != NULL) {
TIM_Handler[TIM8CH4]();
}
TIM_SetCompare4(TIM8, TIM_GetCapture4(TIM8) + TIM_IRQ_period[TIM8CH4]);
}
}
void SweepRobot_CollisionCtrlRightSteerMotorPosMove(enum STEER_MOTOR_POS pos)
{
switch(pos) {
case STEER_MOTOR_IDLE_POS:
TIM_SetCompare2(COLLISION_TEST_STEER_MOTOR_CTRL_TIM, COLLISION_TEST_RIGHT_STEERING_MOTOR_IDLE_POS);
break;
case STEER_MOTOR_FRONT_POS:
TIM_SetCompare2(COLLISION_TEST_STEER_MOTOR_CTRL_TIM, COLLISION_TEST_RIGHT_STEERING_MOTOR_FRONT_POS);
break;
case STEER_MOTOR_SIDE_POS:
TIM_SetCompare2(COLLISION_TEST_STEER_MOTOR_CTRL_TIM, COLLISION_TEST_RIGHT_STEERING_MOTOR_SIDE_POS);
break;
default:
break;
}
}
void Servo::writeMicroseconds(uint16_t pulseWidth) {
if (!this->attached()) {
return;
}
pulseWidth = constrain(pulseWidth, this->minPW, this->maxPW);
//SERVO_TIM_CCR = pulseWidth * (SERVO_TIM_ARR + 1) * SERVO_TIM_PWM_FREQ / 1000000;
uint16_t SERVO_TIM_CCR = pulseWidth;
if(PIN_MAP[this->pin].timer_ch == TIM_Channel_1)
{
TIM_SetCompare1(PIN_MAP[this->pin].timer_peripheral, SERVO_TIM_CCR);
}
else if(PIN_MAP[this->pin].timer_ch == TIM_Channel_2)
{
TIM_SetCompare2(PIN_MAP[this->pin].timer_peripheral, SERVO_TIM_CCR);
}
else if(PIN_MAP[this->pin].timer_ch == TIM_Channel_3)
{
TIM_SetCompare3(PIN_MAP[this->pin].timer_peripheral, SERVO_TIM_CCR);
}
else if(PIN_MAP[this->pin].timer_ch == TIM_Channel_4)
{
TIM_SetCompare4(PIN_MAP[this->pin].timer_peripheral, SERVO_TIM_CCR);
}
}
/**
* @brief This function handles TIM3 global interrupt request.
* @param None
* @retval None
*/
void TIM3_IRQHandler(void)
{
/* TIM3_CH1 toggling with frequency = 256.35 Hz */
if (TIM_GetITStatus(TIM3, TIM_IT_CC1) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC1 );
uhCapture = TIM_GetCapture1(TIM3);
TIM_SetCompare1(TIM3, uhCapture + uhCCR1_Val );
}
/* TIM3_CH2 toggling with frequency = 512.7 Hz */
if (TIM_GetITStatus(TIM3, TIM_IT_CC2) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC2);
uhCapture = TIM_GetCapture2(TIM3);
TIM_SetCompare2(TIM3, uhCapture + uhCCR2_Val);
}
/* TIM3_CH3 toggling with frequency = 1025.4 Hz */
if (TIM_GetITStatus(TIM3, TIM_IT_CC3) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC3);
uhCapture = TIM_GetCapture3(TIM3);
TIM_SetCompare3(TIM3, uhCapture + uhCCR3_Val);
}
/* TIM3_CH4 toggling with frequency = 2050.78 Hz */
if (TIM_GetITStatus(TIM3, TIM_IT_CC4) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC4);
uhCapture = TIM_GetCapture4(TIM3);
TIM_SetCompare4(TIM3, uhCapture + uhCCR4_Val);
}
}
/*******************************************************************************
** Function name: PWM_SetDutyCycle( PWM_typedef * PWM)
** Description : The function settings dutycycle on PWM channel
** Parameter : None
** Return value : None
** Remarks : PWM_FAIL - Init unsuccessful
** PWM_OK - Init successful
*******************************************************************************/
int PWM_SetDutyCycle( PWM_typedef * PWM )
{
int Retval = PWM_FAIL;
if(PWM != NULL)
{
switch(PWM->chid)
{
case PWM_CH1:
TIM_SetCompare1(TIM3,PWM->duty_cycle);
break;
case PWM_CH2:
TIM_SetCompare2(TIM3,PWM->duty_cycle);
break;
case PWM_CH3:
TIM_SetCompare3(TIM3,PWM->duty_cycle);
break;
case PWM_CH4:
TIM_SetCompare4(TIM3,PWM->duty_cycle);
break;
default: break;
}
Retval = PWM_OK;
}
else
{
/* do nothing */
}
return Retval;
}
void Timer::set_duty(float duty, uint8_t CH_No) {
uint16_t C_Val = (uint16_t) (TIMER_CLOCK / (this->freq_) * duty);
if (CH_No==1) TIM_SetCompare1(this->TIM,C_Val); //设置占空比
if (CH_No==2) TIM_SetCompare2(this->TIM,C_Val);
if (CH_No==3) TIM_SetCompare3(this->TIM,C_Val);
if (CH_No==4) TIM_SetCompare4(this->TIM,C_Val);
}
/**
* @brief This function handles TIM3 global interrupt request.
* @param None
* @retval None
*/
void TIM3_IRQHandler(void)
{
/* TIM3_CH1 toggling with frequency = 585.9 Hz */
if (TIM_GetITStatus(TIM3, TIM_IT_CC1) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC1 );
capture = TIM_GetCapture1(TIM3);
TIM_SetCompare1(TIM3, capture + CCR1_Val );
}
/* TIM3_CH2 toggling with frequency = 1171.8 Hz */
if (TIM_GetITStatus(TIM3, TIM_IT_CC2) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC2);
capture = TIM_GetCapture2(TIM3);
TIM_SetCompare2(TIM3, capture + CCR2_Val);
}
/* TIM3_CH3 toggling with frequency = 2343.75 Hz */
if (TIM_GetITStatus(TIM3, TIM_IT_CC3) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC3);
capture = TIM_GetCapture3(TIM3);
TIM_SetCompare3(TIM3, capture + CCR3_Val);
}
/* TIM3_CH4 toggling with frequency = 4687.5 Hz */
if (TIM_GetITStatus(TIM3, TIM_IT_CC4) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC4);
capture = TIM_GetCapture4(TIM3);
TIM_SetCompare4(TIM3, capture + CCR4_Val);
}
}
/**
* @brief This function handles TIM3 global interrupt request.
* @param None
* @retval None
*/
void TIM3_IRQHandler(void)
{
/* TIM3_CH1 toggling with frequency = 183.1 Hz */
if (TIM_GetITStatus(TIM3, TIM_IT_CC1) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC1 );
capture = TIM_GetCapture1(TIM3);
TIM_SetCompare1(TIM3, capture + CCR1_Val );
}
/* TIM3_CH2 toggling with frequency = 366.2 Hz */
if (TIM_GetITStatus(TIM3, TIM_IT_CC2) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC2);
capture = TIM_GetCapture2(TIM3);
TIM_SetCompare2(TIM3, capture + CCR2_Val);
}
/* TIM3_CH3 toggling with frequency = 732.4 Hz */
if (TIM_GetITStatus(TIM3, TIM_IT_CC3) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC3);
capture = TIM_GetCapture3(TIM3);
TIM_SetCompare3(TIM3, capture + CCR3_Val);
}
/* TIM3_CH4 toggling with frequency = 1464.8 Hz */
if (TIM_GetITStatus(TIM3, TIM_IT_CC4) != RESET)
{
TIM_ClearITPendingBit(TIM3, TIM_IT_CC4);
capture = TIM_GetCapture4(TIM3);
TIM_SetCompare4(TIM3, capture + CCR4_Val);
}
}
int timer_set(tim_t dev, int channel, unsigned int timeout)
{
int now = timer_read(dev);
TIM_TypeDef *timer = NULL;
switch (dev) {
case TIMER_0:
timer = TIMER_0_DEV;
break;
case TIMER_1:
timer = TIMER_1_DEV;
break;
default:
return -1;
}
DEBUG("set timer %i to %i + %i\n", channel-1, now, timeout);
switch (channel) {
case 1:
TIM_SetCompare1(timer, now + timeout - 1);
TIM_ITConfig(timer, TIM_IT_CC1, ENABLE);
break;
case 2:
TIM_SetCompare2(timer, now + timeout - 1);
TIM_ITConfig(timer, TIM_IT_CC2, ENABLE);
break;
case 3:
TIM_SetCompare3(timer, now + timeout - 1);
TIM_ITConfig(timer, TIM_IT_CC3, ENABLE);
break;
case 4:
TIM_SetCompare4(timer, now + timeout - 1);
TIM_ITConfig(timer, TIM_IT_CC4, ENABLE);
break;
}
return 0;
}
/**
* @brief This function handles TIM4 global interrupt request.
* @param None
* @retval None
*/
void TIM4_IRQHandler(void)
{
static __IO uint8_t servo1TurningRight = 1;
static __IO uint8_t servo2TurningRight = 1;
int16_t incrementX = 0;
int16_t incrementY = 0;
/* TIM_Update */
if (TIM_GetITStatus(TIM4, TIM_IT_Update) != RESET)
{
TIM_ClearITPendingBit(TIM4, TIM_IT_Update);
/* Read MEMS values */
Buffer[0] = 0;
Buffer[2] = 0;
LIS302DL_Read(Buffer, LIS302DL_OUT_X_ADDR, 4);
/* Remove the offsets values from data */
Buffer[0] -= XOffset;
Buffer[2] -= YOffset;
incrementX = Buffer[0] * LIS302DL_SENSITIVITY_2_3G;
servo1Pulse = incrementX + 1500;
incrementY = Buffer[2] * LIS302DL_SENSITIVITY_2_3G;
servo2Pulse = incrementY + 1500;
TIM_SetCompare1(TIM4, servo1Pulse);
TIM_SetCompare2(TIM4, servo1Pulse);
TIM_SetCompare3(TIM4, servo2Pulse);
TIM_SetCompare4(TIM4, servo2Pulse);
}
}
/**
* \brief Changes the engine speed by update the duty cycle of the PWM output.
* \param[in] speed New speed of the engine. Equivalent to the duty cycle of the
* PWM. It must be smaller than the period register, which is configured
* in BSP_ENGINE_PWM_PERIOD.
*/
void bsp_EngineSpeedSet(uint32_t speed) {
switch (BSP_ENGINE_TIMER_PORT_CHANEL) {
case CHANNEL1:
/* Set the Capture Compare Register: Channel 1 */
TIM_SetCompare1(BSP_ENGINE_TIMER_PORT_BASE, speed % BSP_ENGINE_PWM_PERIOD);
break;
case CHANNEL2:
/* Set the Capture Compare Register: Channel 2 */
TIM_SetCompare2(BSP_ENGINE_TIMER_PORT_BASE, speed % BSP_ENGINE_PWM_PERIOD);
break;
case CHANNEL3:
/* Set the Capture Compare Register: Channel 3 */
TIM_SetCompare3(BSP_ENGINE_TIMER_PORT_BASE, speed % BSP_ENGINE_PWM_PERIOD);
break;
case CHANNEL4:
/* Set the Capture Compare Register: Channel 4 */
TIM_SetCompare4(BSP_ENGINE_TIMER_PORT_BASE, speed % BSP_ENGINE_PWM_PERIOD);
break;
default:
assert(BSP_ENGINE_TIMER_PORT_CHANEL);
break;
}
}
void TIM5_PWM_OUTPUT(u16 DR1,u16 DR2,u16 DR3,u16 DR4)
{
TIM_SetCompare1(TIM5,DR1);
TIM_SetCompare2(TIM5,DR2);
TIM_SetCompare3(TIM5,DR3);
TIM_SetCompare4(TIM5,DR4);
}
