標題:
單片機控制舵機機械手視頻與源碼
[打印本頁]
作者:
順子
時間:
2018-9-24 22:32
標題:
單片機控制舵機機械手視頻與源碼
http://v.youku.com/v_show/id_XMTM3MTYwNTAyNA==.html?sharekey=fb1ccb77e43fc5a4635eb1317ec079ce3
小弟自己業(yè)余愛好,閑時做了一個舵機控制裝置擺弄一下,利用三角函數(shù)還算是可以用搖桿隨意控制方向。有待改進,請多包涵,應(yīng)大家需求我也可以貼出代碼!謝謝!
//設(shè)置接收dma接口的模擬數(shù)據(jù)(自動接收)
__IO uint16_t ADC_ConvertedValue[2];
首先設(shè)置pwm三個舵機接口
stm32源碼:
void TIM3_PWM_Init(u16 arr,u16 psc)
{
GPIO_InitTypeDef GPIO_InitStructure;
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);//
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA , ENABLE);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1|GPIO_Pin_2|GPIO_Pin_3; //TIM_CH1
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; //復(fù)用推挽輸出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
TIM_TimeBaseStructure.TIM_Period = arr; //設(shè)置在下一個更新事件裝入活動的自動重裝載寄存器周期的值 80K
TIM_TimeBaseStructure.TIM_Prescaler = psc; //設(shè)置用來作為TIMx時鐘頻率除數(shù)的預(yù)分頻值 不分頻
TIM_TimeBaseStructure.TIM_ClockDivision = 0; //設(shè)置時鐘分割:TDTS = Tck_tim
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上計數(shù)模式
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure); //根據(jù)TIM_TimeBaseInitStruct中指定的參數(shù)初始化TIMx的時間基數(shù)單位
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; //選擇定時器模式:TIM脈沖寬度調(diào)制模式2
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比較輸出使能
TIM_OCInitStructure.TIM_Pulse = 0; //設(shè)置待裝入捕獲比較寄存器的脈沖值
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; //輸出極性:TIM輸出比較極性高
TIM_OC2Init(TIM2, &TIM_OCInitStructure);
TIM_OC3Init(TIM2, &TIM_OCInitStructure);
TIM_OC4Init(TIM2, &TIM_OCInitStructure); //根據(jù)TIM_OCInitStruct中指定的參數(shù)初始化外設(shè)TIMx
//TIM_CtrlPWMOutputs(TIM1,ENABLE); //MOE 主輸出使能
TIM_OC2PreloadConfig(TIM2, TIM_OCPreload_Enable); //CH3預(yù)裝載使能
TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable); //CH3預(yù)裝載使能
TIM_OC4PreloadConfig(TIM2, TIM_OCPreload_Enable); //CH3預(yù)裝載使能
TIM_ARRPreloadConfig(TIM2, ENABLE); //使能TIMx在ARR上的預(yù)裝載寄存器
TIM_Cmd(TIM2, ENABLE); //使能TIM1
}
float Right_Angle_Hypotenuse(float H,float X)//求直角斜邊的距離
{
return sqrt(pow(H,2)+pow(X,2));
}
float Right_Angle(float H,float X)//求直角三角形的角度
{
return atan(X/H)*180/PI;
}
/*三角形ABC角度分別為A,B,C,邊分別為AB=c,BC=a,CA=b;
如果你想知道A的度數(shù),參數(shù)依次為bca,
如果你想知道B的度數(shù),參數(shù)依次為acb,
如果你想知道C的度數(shù),參數(shù)依次為abc,
*/
float Angle(float a,float b,float c)//求任意三角形的每個角的角度
{
float angle;
angle=acos((pow(a,2)+pow(b,2)-pow(c,2))/(2*a*b));
angle=angle*180/PI;
if(angle<0)angle=180-fabs(angle);
return angle;
}
void YaoGan_XYD_init(void)
{
GPIO_InitTypeDef HS_Gpio;
ADC_InitTypeDef HS_ADC;
DMA_InitTypeDef HS_DMA;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);//開啟IO口A時鐘
RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE);
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
DMA_DeInit(DMA1_Channel1);
HS_DMA.DMA_PeripheralBaseAddr = (u32)&ADC1->DR;//((u32)0x40012400+0x4c);
HS_DMA.DMA_MemoryBaseAddr=(u32)&ADC_ConvertedValue;
HS_DMA.DMA_DIR=DMA_DIR_PeripheralSRC;
HS_DMA.DMA_BufferSize=2;
HS_DMA.DMA_PeripheralInc=DMA_PeripheralInc_Disable;
HS_DMA.DMA_MemoryInc=DMA_MemoryInc_Enable;
HS_DMA.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;
HS_DMA.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
HS_DMA.DMA_Mode = DMA_Mode_Circular;
HS_DMA.DMA_Priority = DMA_Priority_High;
HS_DMA.DMA_M2M = DMA_M2M_Disable;
DMA_Init(DMA1_Channel1, &HS_DMA);
DMA_Cmd(DMA1_Channel1, ENABLE);
HS_Gpio.GPIO_Pin=GPIO_Pin_10; //開10號Io口
HS_Gpio.GPIO_Mode=GPIO_Mode_IPU; //上拉輸入
HS_Gpio.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOB,&HS_Gpio); //初始化IO口
HS_Gpio.GPIO_Pin=GPIO_Pin_0|GPIO_Pin_1; //開啟0,1號Io口
HS_Gpio.GPIO_Mode=GPIO_Mode_AIN; //
HS_Gpio.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_Init(GPIOB,&HS_Gpio); //初始化IO口
HS_ADC.ADC_Mode = ADC_Mode_Independent;
HS_ADC.ADC_ScanConvMode = ENABLE;//多通道循環(huán)掃描
HS_ADC.ADC_ContinuousConvMode = ENABLE;
HS_ADC.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None;
HS_ADC.ADC_DataAlign = ADC_DataAlign_Right;
HS_ADC.ADC_NbrOfChannel = 2;
ADC_Init(ADC1,&HS_ADC);
// RCC_ADCCLKConfig(RCC_PCLK2_Div8);//這句意思是把系統(tǒng)時間分為八分之一
ADC_RegularChannelConfig(ADC1,ADC_Channel_8,1,ADC_SampleTime_1Cycles5);
ADC_RegularChannelConfig(ADC1,ADC_Channel_9,2,ADC_SampleTime_1Cycles5);
ADC_DMACmd(ADC1, ENABLE);
ADC_Cmd(ADC1,ENABLE);//ADC1使能
ADC_ResetCalibration(ADC1);//復(fù)位
while(ADC_GetResetCalibrationStatus(ADC1));//判斷復(fù)位是否完成
ADC_StartCalibration(ADC1);
while(ADC_GetCalibrationStatus(ADC1));
ADC_SoftwareStartConvCmd(ADC1,ENABLE);//執(zhí)行轉(zhuǎn)換功能
}
u8 Key(void)//返回按鍵值
{
return GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_10);
}
u16 ADC1_DMA1_2(u8 x)//
{
return ADC_ConvertedValue[x];
}
u16 AngleMinMax(u16 angle)
{
if(angle<ANGLEMIN){angle=ANGLEMIN;}
else if(angle>ANGLEMAX){angle=ANGLEMAX;}
return angle;
}
//主程序
#include "stm32f10x.h"
#include "init.h"
u16 LD_L1Angle;//臀部舵機的角度 左邊
u16 LD_L2Angle;//膝蓋舵機的角度 左邊
u16 LD_L3Angle;//腳踝舵機的角度 左邊
float LD_H;//機器人從臀部舵機到腳踝舵機的垂直高度
float LD_L1H;//機器人從臀部舵機到膝蓋舵機的距離 左邊
float LD_L2H;//機器人從膝蓋舵機到腳踝舵機的距離 左邊
float LD_LH;//機器人臀部到腳踝的距離 左邊
float LD_LX;//機器人前后移動的距離,變量
u16 YaoGanX;
u16 YaoGanY;
#define YGMIN 1750
#define YGMAX 2250
void init()
{
LD_H=80;//mm
LD_L1H=77;
LD_L2H=80;
LD_LX=0.0;
}
int main(void)
{
init();
TIM3_PWM_Init(200,3600);//不分頻。PWM頻率=72000/(899+1)=80Khz
time_ms(10);
YaoGan_XYD_init();
time_ms(10);
while (1)
{
YaoGanX=ADC1_DMA1_2(1);
YaoGanY=ADC1_DMA1_2(0);
if(Key()==0)
{
time_ms(10);
if(Key()==0)
{
LD_H=80;//mm
LD_LX=0.0;
}
}
if(YaoGanX<YGMIN)
{
LD_LX+=fabs(YaoGanX-YGMIN)/2000.0;
}else if(YaoGanX>YGMAX)
{
LD_LX-=fabs(YaoGanX-YGMAX)/2000.0;
}
if(YaoGanY<YGMIN)
{
LD_H-=fabs(YaoGanY-YGMIN)/2000.0;
}else if(YaoGanY>YGMAX)
{
LD_H+=fabs(YaoGanY-YGMAX)/2000.0;
}
if(LD_H<25){LD_H=25;}if(LD_H>120){LD_H=120;}
if(LD_LX<-90){LD_LX=-90;}if(LD_LX>90){LD_LX=90;}
Left_leg_Movement();
TIM_SetCompare2(TIM2,LD_L1Angle);
TIM_SetCompare3(TIM2,LD_L2Angle);
TIM_SetCompare4(TIM2,LD_L3Angle);
time_ms(10);
}
}
float Left_leg_Movement(void)//左腿運動
{
float CAB,ABC,BCA,CAD,ACD;//設(shè)置角度參數(shù)
//下面開始計算所有要用的角度
LD_LH=Right_Angle_Hypotenuse(LD_H,fabs(LD_LX));//計算斜邊
CAB=Angle(LD_L1H,LD_LH,LD_L2H);
ABC=Angle(LD_L1H,LD_L2H,LD_LH);
BCA=Angle(LD_LH,LD_L2H,LD_L1H);
CAD=Right_Angle(LD_H,fabs(LD_LX));
ACD=Right_Angle(fabs(LD_LX),LD_H);
if(LD_LX<0)
{
LD_L1Angle=(u16)((90-CAD-CAB)+80)/4;
LD_L3Angle=(u16)((BCA+ACD)-30)/4;
}
if(LD_LX==0)
{
LD_L1Angle=(u16)((90-CAB)+80)/4;
LD_L3Angle=(u16)((BCA+90)-30)/4;
}
if(LD_LX>0)
{
LD_L1Angle=(u16)((90-(CAB-CAD)+80))/4;
LD_L3Angle=(u16)((180-ACD+BCA)-30)/4;
}
LD_L2Angle=(u16)((180-ABC)+45)/4;
LD_L1Angle=AngleMinMax(LD_L1Angle);
LD_L2Angle=AngleMinMax(LD_L2Angle);
LD_L3Angle=AngleMinMax(LD_L3Angle);
return 1;
}
舵機1是最上面的接PA1;
舵機2是最上面的接PA2;
舵機3是最上面的接PA3;
搖桿的X方向為PB0;
搖桿的Y方向為PB1;
復(fù)制代碼
51單片機源碼:
//機器人琳達的行動代碼編寫
#include <reg52.h>
#include <intrins.h>
#include <math.h>
#include <float.h>
#define FOSC 11059200L //11.0592M Hz
#define TIME_US 100 //設(shè)定定時時間
#define uchar unsigned char
#define uint unsigned int
P0M0=0xff;
P0M1=0x00;//強推
uchar serval[2];
uint pwm[]={1382,1382,1382};
uchar pwm_flag=0;
uint ms0_5con=461;//0.5
uint ms2_5con=2304;//2.5
sbit D1=P0^7;//舵機1
sbit D2=P0^0;//舵機2
sbit D3=P0^1;//舵機3
sbit led=P1^0;
sbit led1=P1^1;
//機器人叫 琳達 簡稱LD
uint count;//舵機變量
#define PI 3.141593 // 設(shè)定圓周率
float LD_H;//機器人從臀部舵機到腳踝舵機的垂直高度
float LD_LX;//機器人前后移動的距離,變量
float LD_L1H;//機器人從臀部舵機到膝蓋舵機的距離 左邊
float LD_L2H;//機器人從膝蓋舵機到腳踝舵機的距離 左邊
uint LD_L1Angle;//臀部舵機的角度 左邊
uint LD_L2Angle;//膝蓋舵機的角度 左邊
uint LD_L3Angle;//腳踝舵機的角度 左邊
float LD_LH;//機器人臀部到腳踝的距離 左邊
float in;
uint y=0;
void init()
{
/*
TMOD=0x01; //設(shè)定定時器0的工作方式為1
TH0=(65536-(FOSC/12*TIME_US)/1000000)/256;
TL0=(65536-(FOSC/12*TIME_US)/1000000)%256;
ET0=1; //開啟定時器0中斷
TR0=1; //開啟定時器
EA=1; //打開總中斷
*/
TMOD|=0x20;
TH1=0xfd;
TL1=0xfd;
TR1=1;
REN=1;
SM0=0;
SM1=1;
EA=1;
ES=1;
TMOD|=0x01;
TH0=-ms2_5con>>8;
TL0=-ms2_5con;
EA=1;
ET0=1;
TR0=1;
LD_H=120;//mm
LD_L1H=77;
LD_L2H=80;
in=-1.0;
LD_LX=0.0;
}
void time(unsigned int ms)
{
uint i,j;
for(i=0;i<ms;i++)
#if FOSC == 11059200L
for(j=0;j<114;j++);
#elif FOSC == 12000000L
for(j=0;j<123;j++);
#elif FOSC == 24000000L
for(j=0;j<249;j++);
#else
for(j=0;j<114;j++);
#endif
}
void main()
{
uint x=0;
init();
while(1)
{
switch(x)
{
case 0:
LD_LX+=in;
if(LD_LX<-80.0){x=1;}
if(LD_LX>50.0){x=1;}
led=0;
led1=1;
break;
case 1:
LD_H+=in;
if(LD_H<90.0){in=1.0;x=0;}
if(LD_H>120.0){in=-1.0;x=0;}
led=1;
led1=0;
break;
}
Left_Leg_Movement();
}
}
float Right_Angle_Hypotenuse(float H,float X)//求直角斜邊的距離
{
return sqrt(pow(H,2)+pow(X,2));
}
float Right_Angle(float H,float X)//求直角三角形的角度
{
return atan(X/H)*180/PI;
}
/*三角形ABC角度分別為A,B,C,邊分別為AB=c,BC=a,CA=b;
如果你想知道A的度數(shù),參數(shù)依次為bca,
如果你想知道B的度數(shù),參數(shù)依次為acb,
如果你想知道C的度數(shù),參數(shù)依次為abc,
*/
float Angle(float a,float b,float c)//求任意三角形的每個角的角度
{
float angle;
angle=acos((pow(a,2)+pow(b,2)-pow(c,2))/(2*a*b));
angle=angle*180/PI;
if(angle<0)angle=180-abs(angle);
return angle;
}
void Left_leg_Movement()//左腿運動
{
float CAB,ABC,BCA,CAD,ACD;//設(shè)置角度參數(shù)
//下面開始計算所有要用的角度
LD_LH=Right_Angle_Hypotenuse(LD_H,abs(LD_LX));//計算斜邊
CAB=Angle(LD_L1H,LD_LH,LD_L2H);
ABC=Angle(LD_L1H,LD_L2H,LD_LH);
BCA=Angle(LD_LH,LD_L2H,LD_L1H);
CAD=Right_Angle(LD_H,abs(LD_LX));
ACD=Right_Angle(abs(LD_LX),LD_H);
if(LD_LX<=0)
{
LD_L1Angle=(int)(180-CAD-CAB)/7;
LD_L3Angle=(int)(BCA+ACD)/7-6;
}
if(LD_LX>0)
{
LD_L1Angle=(int)(180+CAD-CAB)/7;
LD_L3Angle=(int)(180-ACD+BCA)/7-6;
}
LD_L2Angle=(int)(180-ABC)/7;
}
void Timer0Int() interrupt 1
{
/*
TH0=(65536-(FOSC/12*TIME_US)/1000000)/256;
TL0=(65536-(FOSC/12*TIME_US)/1000000)%256;
*/
count++;
if(count > 200)
{
count=0;
}
if(LD_L1Angle<7)LD_L1Angle=7;if(LD_L1Angle>20)LD_L1Angle=20;
if(LD_L2Angle<7)LD_L2Angle=7;if(LD_L2Angle>20)LD_L2Angle=20;
if(LD_L3Angle<7)LD_L3Angle=7;if(LD_L3Angle>20)LD_L3Angle=20;
/*
if(count<LD_L1Angle){D1=1;}else{D1=0;}
if(count<LD_L2Angle){D2=1;}else{D2=0;}
if(count<LD_L3Angle){D3=1;}else{D3=0;}
*/
switch(pwm_flag)
{
case 1:
D1=1;
TH0=-pwm[0]>>8;
TL0=-pwm[0];
break;
case 2:
D1=0;
TH0=-(ms2_5con-pwm[0])>>8;
TL0=-(ms2_5con-pwm[0]);
pwm_flag=0;
break;
case 3:
TH0=0xff;
TL0=0x80;
pwm_flag=0;
break;
}
pwm_flag++;
switch(y)
{
case 0:
pwm[0]+=10;
if(pwm[0]>2000)
{
y=1;
}
break;
case 1:
pwm[0]-=10;
if(pwm[0]<500)
{
y=0;
}
break;
}
}
void ser() interrupt 4
{
}
復(fù)制代碼
作者:
2059816435
時間:
2018-11-1 20:34
很強啊老哥,可以交流一下
作者:
Fudan
時間:
2018-11-3 01:08
可以啊,幫頂
歡迎光臨 (http://www.torrancerestoration.com/bbs/)
Powered by Discuz! X3.1