单片机控制舵机机械手视频与源码 -

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; //复用推挽输出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
TIM_TimeBaseStructure.TIM_Period = arr; //设置在下一个更新事件装入活动的自动重装载寄存器周期的值 80K
TIM_TimeBaseStructure.TIM_Prescaler = psc; //设置用来作为TIMx时钟频率除数的预分频值不分频
TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分割:TDTS = Tck_tim
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; //TIM向上计数模式
TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure); //根据TIM_TimeBaseInitStruct中指定的参数初始化TIMx的时间基数单位
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; //选择定时器模式:TIM脉冲宽度调制模式2
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
TIM_OCInitStructure.TIM_Pulse = 0; //设置待装入捕获比较寄存器的脉冲值
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; //输出极性:TIM输出比较极性高
TIM_OC2Init(TIM2, &TIM_OCInitStructure);
TIM_OC3Init(TIM2, &TIM_OCInitStructure);
TIM_OC4Init(TIM2, &TIM_OCInitStructure); //根据TIM_OCInitStruct中指定的参数初始化外设TIMx
//TIM_CtrlPWMOutputs(TIM1,ENABLE); //MOE 主输出使能
TIM_OC2PreloadConfig(TIM2, TIM_OCPreload_Enable); //CH3预装载使能
TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable); //CH3预装载使能
TIM_OC4PreloadConfig(TIM2, TIM_OCPreload_Enable); //CH3预装载使能
TIM_ARRPreloadConfig(TIM2, ENABLE); //使能TIMx在ARR上的预装载寄存器
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的度数，参数依次为bca，
如果你想知道B的度数，参数依次为acb，
如果你想知道C的度数，参数依次为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;//多通道循环扫描
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);//这句意思是把系统时间分为八分之一
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);//复位
while(ADC_GetResetCalibrationStatus(ADC1));//判断复位是否完成
ADC_StartCalibration(ADC1);
while(ADC_GetCalibrationStatus(ADC1));
ADC_SoftwareStartConvCmd(ADC1,ENABLE);//执行转换功能
}
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;//设置角度参数
//下面开始计算所有要用的角度
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；

复制代码

#include <reg52.h>
#include <intrins.h>
#include <math.h>
#include <float.h>
#define FOSC 11059200L //11.0592M Hz
#define TIME_US 100 //设定定时时间
#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 // 设定圆周率
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; //设定定时器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的度数，参数依次为bca，
如果你想知道B的度数，参数依次为acb，
如果你想知道C的度数，参数依次为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;//设置角度参数
//下面开始计算所有要用的角度
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
{
}

复制代码