一、本週工作:
9月 27 日 - 10月 3日
利用STM32F103RB芯片內部通用定時器2和4,產生16路週期固定20ms、佔空比控制範圍(0.5ms~2.5ms)、補進爲1us可控的PWM。輝盛舵機MG995(死區設定4微秒、0.13秒/60度(6.0V) ),產生的PWM可以正常控制該舵機。程序見附錄。
舵機簡介:
1、舵機的原理和控制
控制信號由接收機的通道進入信號調製芯片,獲得直流偏置電壓。它內部有一個基準電路,產生週期爲20ms,寬度爲1.5ms的基準信號,將獲得的直流偏置電壓與電位器的電壓比較,獲得電壓差輸出。最後,電壓差的正負輸出到電機驅動芯片決定電機的正反轉。當電機轉速一定時,通過級聯減速齒輪帶動電位器旋轉,使得電壓差爲0,電機停止轉動。
舵機的控制一般需要一個20ms左右的時基脈衝,該脈衝的高電平部分一般爲0.5ms-2.5ms範圍內的角度控制脈衝部分,總間隔爲2ms。以180度角度伺服爲例,那麼對應的控制關係是這樣的:
0.5ms------------0度;
1.0ms------------45度;
1.5ms------------90度;
2.0ms-----------135度;
2.5ms-----------180度;
2、舵機的追隨特性
假設現在舵機穩定在A點,這時候CPU發出一個PWM信號,舵機全速由A點轉向B點,在這個過程中需要一段時間,舵機才能運動到B點。
保持時間爲Tw
當Tw≥△T時,舵機能夠到達目標,並有剩餘時間;
當Tw≤△T時,舵機不能到達目標;
理論上:當Tw=△T時,系統最連貫,而且舵機運動的最快。
實際過程中w不盡相同,連貫運動時的極限△T比較難以計算出來。
假如我們的舵機1DIV =8us,當PWM信號以最小變化量即(1DIV=8us)依次變化時,舵機的分辨率最高,但是速度會減慢。
二、本週小結:
看了一些關於如何控制多路舵機的資料,發現基本上使用普通的PWM就可以正常使用,但是需要一定的精度和抗干擾性。抗干擾可以用硬件處理。在可以正常驅動16路舵機的基礎上,進行位姿分析。位姿分析如果用軟件模擬這個會比較複雜。
三、疑問:出現了一個問題如下圖,設置IO口時啓用A組和B組是可以正常產生PWM,但是如果啓用的是A組和C組PWM會出現問題。目前還不知什麼情況。。
四 下週計劃:學習機器人多個舵機控制算法,查找軟件模擬位姿動作資料。
附錄:
2、舵機的追隨特性
假設現在舵機穩定在A點,這時候CPU發出一個PWM信號,舵機全速由A點轉向B點,在這個過程中需要一段時間,舵機才能運動到B點。
保持時間爲Tw
當Tw≥△T時,舵機能夠到達目標,並有剩餘時間;
當Tw≤△T時,舵機不能到達目標;
理論上:當Tw=△T時,系統最連貫,而且舵機運動的最快。
實際過程中w不盡相同,連貫運動時的極限△T比較難以計算出來。
假如我們的舵機1DIV =8us,當PWM信號以最小變化量即(1DIV=8us)依次變化時,舵機的分辨率最高,但是速度會減慢。
#include "stm32f10x.h"
#include "duojikongzhiwork.h"
#define PWMout1 GPIO_Pin_0
#define PWMout2 GPIO_Pin_1
#define PWMout3 GPIO_Pin_2
#define PWMout4 GPIO_Pin_3
#define PWMout5 GPIO_Pin_8
#define PWMout6 GPIO_Pin_9
#define PWMout7 GPIO_Pin_10
#define PWMout8 GPIO_Pin_11
#define PWMout9 GPIO_Pin_5
#define PWMout10 GPIO_Pin_6
#define PWMout11 GPIO_Pin_7
#define PWMout12 GPIO_Pin_8
#define PWMout13 GPIO_Pin_12
#define PWMout14 GPIO_Pin_13
#define PWMout15 GPIO_Pin_14
#define PWMout16 GPIO_Pin_15
#define SetA(n) GPIO_SetBits(GPIOA,n) //IO口置1
#define ResetA(n) GPIO_ResetBits(GPIOA,n)//IO口清0
#define SetB(n) GPIO_SetBits(GPIOB,n) //IO口置1
#define ResetB(n) GPIO_ResetBits(GPIOB,n)//IO口清0
u16 PWMZQ=2500,PWMOUTbuff1,PWMOUTbuff2,PWMOUTcnt_1,PWMOUTcnt_2,PWMOUTcnt_3,PWMOUTcnt_4,PWMOUTcnt_5,PWMOUTcnt_6,PWMOUTcnt_7,PWMOUTcnt_8,PWMOUTcnt_9,PWMOUTcnt_10,PWMOUTcnt_11,PWMOUTcnt_12,PWMOUTcnt_13,PWMOUTcnt_14,PWMOUTcnt_15,PWMOUTcnt_16;
u16 PWMOUTbuf_1,PWMOUTbuf_2,PWMOUTbuf_3,PWMOUTbuf_4,PWMOUTbuf_5,PWMOUTbuf_6,PWMOUTbuf_7,PWMOUTbuf_8,PWMOUTbuf_9,PWMOUTbuf_10,PWMOUTbuf_11,PWMOUTbuf_12,PWMOUTbuf_13,PWMOUTbuf_14,PWMOUTbuf_15,PWMOUTbuf_16;
u8 mark=1,cnt1,cnt2;
/********IO口初始化*************/
void DJKZGIPO_ConfingA(void) //IOA口設置
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);//使能GPIOA口
GPIO_InitStructure.GPIO_Pin = (GPIO_Pin_12|PWMout1|PWMout2|PWMout3|PWMout4|PWMout5|PWMout6|PWMout7|PWMout8); //選擇了哪個IO口
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; //配置爲推輓輸出模式
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //IO口速度爲50M
GPIO_Init(GPIOA, &GPIO_InitStructure);
}
void DJKZGIPO_ConfingB(void)//IOB口設置
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);//使能GPIOB口
GPIO_InitStructure.GPIO_Pin = (PWMout9|PWMout10|PWMout11|PWMout12|PWMout13|PWMout14|PWMout15|PWMout16); //選擇了哪個IO口
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; //配置爲推輓輸出模式
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; //IO口速度爲50M
GPIO_Init(GPIOB, &GPIO_InitStructure);
}
/********定時器2和4中斷設置初始化*************/
void TIM4_Configuration(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE); //開啓定時器4的時鐘
TIM_ClearITPendingBit(TIM4,TIM_IT_Update); //清除中斷標誌位
TIM_TimeBaseStructure.TIM_Period = PWMOUTbuff1 - 1; //設置ARR初值
TIM_TimeBaseStructure.TIM_Prescaler = 72 - 1 ; //預分頻預分配係數 PSC
TIM_TimeBaseStructure.TIM_ClockDivision = 0; //設置時鐘分割
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上計數模式
TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure); //根據TIM_TimeBaseInitStruct中指定的參數初始化TIMx模塊的寄存器
TIM_ITConfig(TIM4,TIM_IT_Update,ENABLE); //開啓TIM4中斷
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); // 設置NVIC中斷分組2, 2位搶佔優先級,2位響應優先級
NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;//搶佔式優先級4級
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; //響應優先級4級
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //TIM4通道使能
NVIC_Init(&NVIC_InitStructure); //根據NVIC_InitStruct中指定的參數初始化外設NVIC寄存器
TIM_Cmd(TIM4, ENABLE); //使能TIM4模塊
}
void TIM4_IRQHandler(void) //TIM4中斷
{
if(TIM_GetITStatus(TIM4, TIM_IT_Update) != RESET)
{
PWMOUTcnt_1=PWMOUTcnt_2=PWMOUTcnt_3=PWMOUTcnt_4=PWMOUTcnt_5=PWMOUTcnt_6=PWMOUTcnt_7=PWMOUTcnt_8=2000;
GPIO_PinReverse(GPIOA, GPIO_Pin_12);
duojikongzhiwork1();
TIM_SetAutoreload(TIM4, PWMOUTbuff1);
TIM_ClearITPendingBit(TIM4,TIM_IT_Update);//清除中斷標誌位
}
}
void TIM2_Configuration(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
NVIC_InitTypeDef NVIC_InitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE); //開啓定時器2的時鐘
TIM_ClearITPendingBit(TIM2,TIM_IT_Update);//清除中斷標誌位
TIM_TimeBaseStructure.TIM_Period = PWMOUTbuff2 - 1; //設置ARR初值
TIM_TimeBaseStructure.TIM_Prescaler = 72 - 1 ; //預分頻預分配係數 PSC
TIM_TimeBaseStructure.TIM_ClockDivision = 0; //設置時鐘分割
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上計數模式
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure); //根據TIM_TimeBaseInitStruct中指定的參數初始化TIMx模塊的寄存器
TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE);//開啓TIM4中斷
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2); // 設置NVIC中斷分組2, 2位搶佔優先級,2位響應優先級
NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; //搶佔式優先級4級
NVIC_InitStructure.NVIC_IRQChannelSubPriority =1; //響應優先級4級
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //TIM2通道使能
NVIC_Init(&NVIC_InitStructure); //根據NVIC_InitStruct中指定的參數初始化外設NVIC寄存器
TIM_Cmd(TIM2, ENABLE); //使能TIM2模塊
}
void TIM2_IRQHandler(void) //TIM2中斷
{
if(TIM_GetITStatus(TIM2, TIM_IT_Update) != RESET)
{
PWMOUTcnt_9=PWMOUTcnt_10=PWMOUTcnt_11=PWMOUTcnt_12=PWMOUTcnt_13=PWMOUTcnt_14=PWMOUTcnt_15=PWMOUTcnt_16=2000;
duojikongzhiwork2();
TIM_SetAutoreload(TIM2, PWMOUTbuff2);
TIM_ClearITPendingBit(TIM2,TIM_IT_Update);//清除中斷標誌位
}
}
/********八路週期20ms、佔空比可控PWM*************/
void duojikongzhiwork1(void)
{
ResetA(PWMout1);
ResetA(PWMout2);
ResetA(PWMout3);
ResetA(PWMout4);
ResetA(PWMout5);
ResetA(PWMout6);
ResetA(PWMout7);
ResetA(PWMout8);
switch (cnt1)
{
case 0:
if( mark )
{PWMOUTbuf_1 = PWMOUTcnt_1; SetA(PWMout1);mark=0;}
else
{PWMOUTbuf_1 = PWMZQ-PWMOUTcnt_1; ResetA(PWMout1);cnt1 = 1;mark=1;}
PWMOUTbuff1 = PWMOUTbuf_1;
break;
case 1:
if(mark)
{PWMOUTbuf_2 = PWMOUTcnt_2; SetA(PWMout2);mark=0;}
else
{PWMOUTbuf_2 = PWMZQ-PWMOUTcnt_2; ResetA(PWMout2);cnt1 = 2;mark=1;}
PWMOUTbuff1 = PWMOUTbuf_2;
break;
case 2:
if(mark)
{PWMOUTbuf_3 = PWMOUTcnt_3; SetA(PWMout3);mark=0;}
else
{PWMOUTbuf_3 = PWMZQ-PWMOUTcnt_3;ResetA(PWMout3);cnt1 = 3;mark=1;}
PWMOUTbuff1 = PWMOUTbuf_3;
break;
case 3:
if(mark)
{PWMOUTbuf_4 = PWMOUTcnt_4; SetA(PWMout4);mark=0;}
else
{PWMOUTbuf_4 = PWMZQ-PWMOUTcnt_4; ResetA(PWMout4);cnt1 = 4;mark=1;}
PWMOUTbuff1 = PWMOUTbuf_4;
break;
case 4:
if(mark)
{PWMOUTbuf_5 = PWMOUTcnt_5; SetA(PWMout5);mark=0;}
else
{PWMOUTbuf_5 = PWMZQ-PWMOUTcnt_5;ResetA(PWMout5);cnt1 = 5;mark=1;}
PWMOUTbuff1 = PWMOUTbuf_5;
break;
case 5:
if(mark)
{PWMOUTbuf_6 = PWMOUTcnt_6; SetA(PWMout6);mark=0;}
else
{PWMOUTbuf_6 = PWMZQ-PWMOUTcnt_6;ResetA(PWMout6);cnt1 = 6;mark=1;}
PWMOUTbuff1 = PWMOUTbuf_6;
break;
case 6:
if(mark)
{PWMOUTbuf_7 = PWMOUTcnt_7; SetA(PWMout7);mark=0;}
else
{PWMOUTbuf_7 = PWMZQ-PWMOUTcnt_7;ResetA(PWMout7);cnt1 = 7;mark=1;}
PWMOUTbuff1 = PWMOUTbuf_7;
break;
case 7:
if(mark)
{PWMOUTbuf_8 = PWMOUTcnt_8; SetA(PWMout8);mark=0;}
else
{PWMOUTbuf_8 = PWMZQ-PWMOUTcnt_8;ResetA(PWMout8);cnt1= 0;mark=1;}
PWMOUTbuff1 = PWMOUTbuf_8;
break;
default: break;
}
}
/********八路週期20ms、佔空比可控PWM*************/
void duojikongzhiwork2(void)
{
ResetB(PWMout9);
ResetB(PWMout10);
ResetB(PWMout11);
ResetB(PWMout12);
ResetB(PWMout13);
ResetB(PWMout14);
ResetB(PWMout15);
ResetB(PWMout16);
switch (cnt2)
{
case 0:
if( mark )
{PWMOUTbuf_9 = PWMOUTcnt_9; SetB(PWMout9);mark=0;}
else
{PWMOUTbuf_9 = PWMZQ-PWMOUTcnt_9; ResetB(PWMout9);cnt2 = 1;mark=1;}
PWMOUTbuff2 = PWMOUTbuf_9;
break;
case 1:
if(mark)
{PWMOUTbuf_10 = PWMOUTcnt_10; SetB(PWMout10);mark=0;}
else
{PWMOUTbuf_10 = PWMZQ-PWMOUTcnt_10; ResetB(PWMout10);cnt2 = 2;mark=1;}
PWMOUTbuff2 = PWMOUTbuf_10;
break;
case 2:
if(mark)
{PWMOUTbuf_11 = PWMOUTcnt_11; SetB(PWMout11);mark=0;}
else
{PWMOUTbuf_11 = PWMZQ-PWMOUTcnt_11;ResetB(PWMout11);cnt2 = 3;mark=1;}
PWMOUTbuff2 = PWMOUTbuf_11;
break;
case 3:
if(mark)
{PWMOUTbuf_12 = PWMOUTcnt_12; SetB(PWMout12);mark=0;}
else
{PWMOUTbuf_12 = PWMZQ-PWMOUTcnt_12; ResetB(PWMout12);cnt2 = 4;mark=1;}
PWMOUTbuff2 = PWMOUTbuf_12;
break;
case 4:
if(mark)
{PWMOUTbuf_13 = PWMOUTcnt_13; SetB(PWMout13);mark=0;}
else
{PWMOUTbuf_13 = PWMZQ-PWMOUTcnt_13;ResetB(PWMout13);cnt2 = 5;mark=1;}
PWMOUTbuff2 = PWMOUTbuf_13;
break;
case 5:
if(mark)
{PWMOUTbuf_14 = PWMOUTcnt_14; SetB(PWMout14);mark=0;}
else
{PWMOUTbuf_14 = PWMZQ-PWMOUTcnt_14;ResetB(PWMout14);cnt2= 6;mark=1;}
PWMOUTbuff2 = PWMOUTbuf_14;
break;
case 6:
if(mark)
{PWMOUTbuf_15 = PWMOUTcnt_15; SetB(PWMout15);mark=0;}
else
{PWMOUTbuf_15 = PWMZQ-PWMOUTcnt_15;ResetB(PWMout15);cnt2 = 7;mark=1;}
PWMOUTbuff2 = PWMOUTbuf_15;
break;
case 7:
if(mark)
{PWMOUTbuf_16 = PWMOUTcnt_16; SetB(PWMout16);mark=0;}
else
{PWMOUTbuf_16 = PWMZQ-PWMOUTcnt_16;ResetB(PWMout16);cnt2 = 0;mark=1;}
break;
default: break;
}
2、舵機的追隨特性
假設現在舵機穩定在A點,這時候CPU發出一個PWM信號,舵機全速由A點轉向B點,在這個過程中需要一段時間,舵機才能運動到B點。
保持時間爲Tw
當Tw≥△T時,舵機能夠到達目標,並有剩餘時間;
當Tw≤△T時,舵機不能到達目標;
理論上:當Tw=△T時,系統最連貫,而且舵機運動的最快。
實際過程中w不盡相同,連貫運動時的極限△T比較難以計算出來。
假如我們的舵機1DIV =8us,當PWM信號以最小變化量即(1DIV=8us)依次變化時,舵機的分辨率最高,但是速度會減慢。