设为首页收藏本站
开启辅助访问 切换到宽版

 找回密码
 立即注册

QQ登录

只需一步，快速开始

热门： 51单片机 | 24小时必答区 | 单片机教程 | 单片机DIY制作 | STM32 | Cortex M3 | 模数电子 | 电子DIY制作 | 音响/功放 | 拆机乐园 | Arduino | 嵌入式OS | 程序设计

搜索
»论坛 › 嵌入式/单片机论坛 › STM32/8 › 单片机控制舵机机械手视频与源码
返回列表 发新帖
查看: 4155|回复: 2
打印 上一主题 下一主题
收起左侧

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

[复制链接]
跳转到指定楼层
楼主
ID:401931 发表于 2018-9-24 22:32 | 只看该作者 回帖奖励 |倒序浏览 |阅读模式


小弟自己业余爱好，闲时做了一个舵机控制装置摆弄一下，利用三角函数还算是可以用摇杆随意控制方向。有待改进，请多包涵，应大家需求我也可以贴出代码！谢谢！

//设置接收dma接口的模拟数据(自动接收)
__IO uint16_t ADC_ConvertedValue[2];

首先设置pwm三个舵机接口

stm32源码：
  1. void TIM3_PWM_Init(u16 arr,u16 psc)
  2. {
  3.   GPIO_InitTypeDef GPIO_InitStructure;
  4.         TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
  5.         TIM_OCInitTypeDef  TIM_OCInitStructure;
  6.       
  7.         RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);//
  8.         RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA , ENABLE);
  9.       
  10.         GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1|GPIO_Pin_2|GPIO_Pin_3; //TIM_CH1
  11.         GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;  //复用推挽输出
  12.         GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  13.         GPIO_Init(GPIOA, &GPIO_InitStructure);

  15.       
  16.         TIM_TimeBaseStructure.TIM_Period = arr; //设置在下一个更新事件装入活动的自动重装载寄存器周期的值         80K
  17.         TIM_TimeBaseStructure.TIM_Prescaler = psc; //设置用来作为TIMx时钟频率除数的预分频值  不分频
  18.         TIM_TimeBaseStructure.TIM_ClockDivision = 0; //设置时钟分割:TDTS = Tck_tim
  19.         TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;  //TIM向上计数模式
  20.         TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure); //根据TIM_TimeBaseInitStruct中指定的参数初始化TIMx的时间基数单位


  23.         TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; //选择定时器模式:TIM脉冲宽度调制模式2
  24.         TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
  25.         TIM_OCInitStructure.TIM_Pulse = 0; //设置待装入捕获比较寄存器的脉冲值
  26.         TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; //输出极性:TIM输出比较极性高
  27.         TIM_OC2Init(TIM2, &TIM_OCInitStructure);
  28.         TIM_OC3Init(TIM2, &TIM_OCInitStructure);
  29.         TIM_OC4Init(TIM2, &TIM_OCInitStructure);  //根据TIM_OCInitStruct中指定的参数初始化外设TIMx

  31.   //TIM_CtrlPWMOutputs(TIM1,ENABLE);        //MOE 主输出使能      

  33.         TIM_OC2PreloadConfig(TIM2, TIM_OCPreload_Enable);  //CH3预装载使能      
  34.         TIM_OC3PreloadConfig(TIM2, TIM_OCPreload_Enable);  //CH3预装载使能      
  35.         TIM_OC4PreloadConfig(TIM2, TIM_OCPreload_Enable);  //CH3预装载使能         
  36.       
  37.         TIM_ARRPreloadConfig(TIM2, ENABLE); //使能TIMx在ARR上的预装载寄存器
  38.       
  39.         TIM_Cmd(TIM2, ENABLE);  //使能TIM1
  40. }


  43. float Right_Angle_Hypotenuse(float H,float X)//求直角斜边的距离
  44. {
  45.         return sqrt(pow(H,2)+pow(X,2));
  46. }

  48. float Right_Angle(float H,float X)//求直角三角形的角度
  49. {
  50.         return atan(X/H)*180/PI;
  51. }

  53. /*三角形ABC角度分别为A，B，C，边分别为AB=c，BC=a，CA=b；
  54.   如果你想知道A的度数，参数依次为bca，
  55.         如果你想知道B的度数，参数依次为acb，
  56.         如果你想知道C的度数，参数依次为abc，
  57. */
  58. float Angle(float a,float b,float c)//求任意三角形的每个角的角度
  59. {
  60.         float angle;
  61.         angle=acos((pow(a,2)+pow(b,2)-pow(c,2))/(2*a*b));
  62.         angle=angle*180/PI;
  63.         if(angle<0)angle=180-fabs(angle);
  64.         return angle;
  65. }

  67. void YaoGan_XYD_init(void)
  68. {
  69.         GPIO_InitTypeDef HS_Gpio;
  70.         ADC_InitTypeDef  HS_ADC;
  71.         DMA_InitTypeDef  HS_DMA;
  72.       
  73.         RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);//开启IO口A时钟
  74.         RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE);
  75.         RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
  76.       
  77.         DMA_DeInit(DMA1_Channel1);
  78.         HS_DMA.DMA_PeripheralBaseAddr = (u32)&ADC1->DR;//((u32)0x40012400+0x4c);
  79.         HS_DMA.DMA_MemoryBaseAddr=(u32)&ADC_ConvertedValue;
  80.         HS_DMA.DMA_DIR=DMA_DIR_PeripheralSRC;
  81.         HS_DMA.DMA_BufferSize=2;
  82.         HS_DMA.DMA_PeripheralInc=DMA_PeripheralInc_Disable;
  83.         HS_DMA.DMA_MemoryInc=DMA_MemoryInc_Enable;
  84.         HS_DMA.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;  
  85.         HS_DMA.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
  86.         HS_DMA.DMA_Mode = DMA_Mode_Circular;  
  87.         HS_DMA.DMA_Priority = DMA_Priority_High;
  88.         HS_DMA.DMA_M2M = DMA_M2M_Disable;  
  89.         DMA_Init(DMA1_Channel1, &HS_DMA);
  90.         DMA_Cmd(DMA1_Channel1, ENABLE);
  91.       
  92.       
  93.         HS_Gpio.GPIO_Pin=GPIO_Pin_10; //开10号Io口
  94.         HS_Gpio.GPIO_Mode=GPIO_Mode_IPU;   //上拉输入
  95.         HS_Gpio.GPIO_Speed=GPIO_Speed_50MHz;
  96.         GPIO_Init(GPIOB,&HS_Gpio);  //初始化IO口
  97.       
  98.         HS_Gpio.GPIO_Pin=GPIO_Pin_0|GPIO_Pin_1; //开启0,1号Io口
  99.         HS_Gpio.GPIO_Mode=GPIO_Mode_AIN;   //
  100.         HS_Gpio.GPIO_Speed=GPIO_Speed_50MHz;
  101.         GPIO_Init(GPIOB,&HS_Gpio);  //初始化IO口
  102.       
  103.         HS_ADC.ADC_Mode = ADC_Mode_Independent;
  104.         HS_ADC.ADC_ScanConvMode = ENABLE;//多通道循环扫描
  105.         HS_ADC.ADC_ContinuousConvMode = ENABLE;
  106.         HS_ADC.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None;
  107.         HS_ADC.ADC_DataAlign = ADC_DataAlign_Right;
  108.         HS_ADC.ADC_NbrOfChannel = 2;
  109.         ADC_Init(ADC1,&HS_ADC);
  110. //        RCC_ADCCLKConfig(RCC_PCLK2_Div8);//这句意思是把系统时间分为八分之一
  111.         ADC_RegularChannelConfig(ADC1,ADC_Channel_8,1,ADC_SampleTime_1Cycles5);
  112.   ADC_RegularChannelConfig(ADC1,ADC_Channel_9,2,ADC_SampleTime_1Cycles5);
  113.         ADC_DMACmd(ADC1, ENABLE);
  114.         ADC_Cmd(ADC1,ENABLE);//ADC1使能
  115.         ADC_ResetCalibration(ADC1);//复位
  116.         while(ADC_GetResetCalibrationStatus(ADC1));//判断复位是否完成
  117.         ADC_StartCalibration(ADC1);
  118.         while(ADC_GetCalibrationStatus(ADC1));
  119.         ADC_SoftwareStartConvCmd(ADC1,ENABLE);//执行转换功能
  120. }

  122. u8 Key(void)//返回按键值
  123. {
  124.   return GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_10);
  125. }


  128. u16 ADC1_DMA1_2(u8 x)//
  129. {
  130.         return ADC_ConvertedValue[x];
  131. }

  133. u16 AngleMinMax(u16 angle)
  134. {
  135.         if(angle<ANGLEMIN){angle=ANGLEMIN;}
  136.         else if(angle>ANGLEMAX){angle=ANGLEMAX;}
  137.         return angle;
  138. }

  140. //主程序
  141. #include "stm32f10x.h"
  142. #include "init.h"

  144. u16  LD_L1Angle;//臀部舵机的角度 左边
  145. u16  LD_L2Angle;//膝盖舵机的角度 左边
  146. u16  LD_L3Angle;//脚踝舵机的角度 左边

  148. float LD_H;//机器人从臀部舵机到脚踝舵机的垂直高度
  149. float LD_L1H;//机器人从臀部舵机到膝盖舵机的距离 左边
  150. float LD_L2H;//机器人从膝盖舵机到脚踝舵机的距离 左边
  151. float LD_LH;//机器人臀部到脚踝的距离 左边
  152. float LD_LX;//机器人前后移动的距离，变量

  154. u16 YaoGanX;
  155. u16 YaoGanY;
  156. #define YGMIN 1750
  157. #define YGMAX 2250

  159. void init()
  160. {
  161.         LD_H=80;//mm
  162.         LD_L1H=77;
  163.         LD_L2H=80;
  164.         LD_LX=0.0;      
  165.       
  166. }

  168. int main(void)
  169. {
  170.         init();
  171.         TIM3_PWM_Init(200,3600);//不分频。PWM频率=72000/(899+1)=80Khz
  172.         time_ms(10);
  173.         YaoGan_XYD_init();
  174.         time_ms(10);
  175.         while (1)
  176.         {
  177.                 YaoGanX=ADC1_DMA1_2(1);
  178.                 YaoGanY=ADC1_DMA1_2(0);

  180.                 if(Key()==0)
  181.                 {
  182.                         time_ms(10);
  183.                         if(Key()==0)
  184.                         {
  185.                                 LD_H=80;//mm
  186.                                 LD_LX=0.0;      
  187.                         }
  188.                 }
  189.                 if(YaoGanX<YGMIN)
  190.                 {
  191.                         LD_LX+=fabs(YaoGanX-YGMIN)/2000.0;
  192.                 }else if(YaoGanX>YGMAX)
  193.                 {
  194.                         LD_LX-=fabs(YaoGanX-YGMAX)/2000.0;
  195.                 }
  196.                 if(YaoGanY<YGMIN)
  197.                 {
  198.                         LD_H-=fabs(YaoGanY-YGMIN)/2000.0;
  199.                 }else if(YaoGanY>YGMAX)
  200.                 {
  201.                         LD_H+=fabs(YaoGanY-YGMAX)/2000.0;
  202.                 }
  203.                 if(LD_H<25){LD_H=25;}if(LD_H>120){LD_H=120;}
  204.                 if(LD_LX<-90){LD_LX=-90;}if(LD_LX>90){LD_LX=90;}
  205.                 Left_leg_Movement();
  206.                
  207.                 TIM_SetCompare2(TIM2,LD_L1Angle);
  208.                 TIM_SetCompare3(TIM2,LD_L2Angle);
  209.                 TIM_SetCompare4(TIM2,LD_L3Angle);
  210.                 time_ms(10);
  211.         }
  212.                
  213. }

  215. float Left_leg_Movement(void)//左腿运动
  216. {
  217.         float CAB,ABC,BCA,CAD,ACD;//设置角度参数
  218.         //下面开始计算所有要用的角度
  219.         LD_LH=Right_Angle_Hypotenuse(LD_H,fabs(LD_LX));//计算斜边
  220.         CAB=Angle(LD_L1H,LD_LH,LD_L2H);
  221.         ABC=Angle(LD_L1H,LD_L2H,LD_LH);
  222.         BCA=Angle(LD_LH,LD_L2H,LD_L1H);
  223.         CAD=Right_Angle(LD_H,fabs(LD_LX));
  224.         ACD=Right_Angle(fabs(LD_LX),LD_H);
  225.       
  226.         if(LD_LX<0)
  227.         {
  228.                 LD_L1Angle=(u16)((90-CAD-CAB)+80)/4;
  229.                 LD_L3Angle=(u16)((BCA+ACD)-30)/4;
  230.         }
  231.         if(LD_LX==0)
  232.         {
  233.                 LD_L1Angle=(u16)((90-CAB)+80)/4;
  234.                 LD_L3Angle=(u16)((BCA+90)-30)/4;
  235.         }
  236.         if(LD_LX>0)
  237.         {
  238.                 LD_L1Angle=(u16)((90-(CAB-CAD)+80))/4;
  239.                 LD_L3Angle=(u16)((180-ACD+BCA)-30)/4;
  240.         }
  241.         LD_L2Angle=(u16)((180-ABC)+45)/4;
  242.       
  243.       
  244.         LD_L1Angle=AngleMinMax(LD_L1Angle);
  245.         LD_L2Angle=AngleMinMax(LD_L2Angle);
  246.         LD_L3Angle=AngleMinMax(LD_L3Angle);
  247.         return 1;
  248. }
  249. 舵机1是最上面的接PA1；
  250. 舵机2是最上面的接PA2；
  251. 舵机3是最上面的接PA3；

  253. 摇杆的X方向为PB0；
  254. 摇杆的Y方向为PB1；
复制代码


51单片机源码：
//机器人琳达的行动代码编写
  1. #include <reg52.h>
  2. #include <intrins.h>
  3. #include <math.h>
  4. #include <float.h>
  5. #define FOSC 11059200L //11.0592M Hz
  6. #define TIME_US 100 //设定定时时间
  7. #define uchar unsigned char
  8. #define uint unsigned int

  10. P0M0=0xff;
  11. P0M1=0x00;//强推
  12. uchar serval[2];
  13. uint pwm[]={1382,1382,1382};
  14. uchar pwm_flag=0;
  15. uint  ms0_5con=461;//0.5
  16. uint  ms2_5con=2304;//2.5
  17. sbit D1=P0^7;//舵机1
  18. sbit D2=P0^0;//舵机2
  19. sbit D3=P0^1;//舵机3
  20. sbit led=P1^0;
  21. sbit led1=P1^1;
  22. //机器人叫 琳达  简称LD
  23. uint count;//舵机变量
  24. #define  PI  3.141593  // 设定圆周率

  26. float LD_H;//机器人从臀部舵机到脚踝舵机的垂直高度
  27. float LD_LX;//机器人前后移动的距离，变量
  28. float LD_L1H;//机器人从臀部舵机到膝盖舵机的距离 左边
  29. float LD_L2H;//机器人从膝盖舵机到脚踝舵机的距离 左边
  30. uint  LD_L1Angle;//臀部舵机的角度 左边
  31. uint  LD_L2Angle;//膝盖舵机的角度 左边
  32. uint  LD_L3Angle;//脚踝舵机的角度 左边
  33. float LD_LH;//机器人臀部到脚踝的距离 左边
  34. float in;
  35. uint y=0;
  36. void init()
  37. {
  38. /*
  39. TMOD=0x01; //设定定时器0的工作方式为1
  40. TH0=(65536-(FOSC/12*TIME_US)/1000000)/256;
  41. TL0=(65536-(FOSC/12*TIME_US)/1000000)%256;
  42. ET0=1; //开启定时器0中断
  43. TR0=1; //开启定时器
  44. EA=1;  //打开总中断
  45. */
  46. TMOD|=0x20;
  47. TH1=0xfd;
  48. TL1=0xfd;
  49. TR1=1;
  50. REN=1;
  51. SM0=0;
  52. SM1=1;
  53. EA=1;
  54. ES=1;

  56. TMOD|=0x01;
  57. TH0=-ms2_5con>>8;
  58. TL0=-ms2_5con;
  59. EA=1;
  60. ET0=1;
  61. TR0=1;


  64. LD_H=120;//mm
  65. LD_L1H=77;
  66. LD_L2H=80;
  67. in=-1.0;
  68. LD_LX=0.0;
  69. }
  70. void time(unsigned int ms)
  71. {
  72. uint i,j;
  73. for(i=0;i<ms;i++)
  74. #if FOSC == 11059200L
  75.   for(j=0;j<114;j++);
  76. #elif FOSC == 12000000L
  77.    for(j=0;j<123;j++);
  78. #elif FOSC == 24000000L
  79.   for(j=0;j<249;j++);
  80. #else
  81.   for(j=0;j<114;j++);
  82. #endif
  83. }

  85. void main()
  86. {
  87. uint x=0;
  88. init();
  89. while(1)
  90. {
  91.   switch(x)
  92.   {
  93.   case 0:
  94.    LD_LX+=in;
  95.    if(LD_LX<-80.0){x=1;}
  96.    if(LD_LX>50.0){x=1;}
  97.    led=0;
  98.    led1=1;
  99.    break;
  100.   case 1:
  101.    LD_H+=in;
  102.    if(LD_H<90.0){in=1.0;x=0;}
  103.    if(LD_H>120.0){in=-1.0;x=0;}
  104.    led=1;
  105.    led1=0;
  106.    break;
  107.   }
  108.   Left_Leg_Movement();
  109.   
  110. }
  111. }
  112. float Right_Angle_Hypotenuse(float H,float X)//求直角斜边的距离
  113. {
  114. return sqrt(pow(H,2)+pow(X,2));
  115. }
  116. float Right_Angle(float H,float X)//求直角三角形的角度
  117. {
  118. return atan(X/H)*180/PI;
  119. }
  120. /*三角形ABC角度分别为A，B，C，边分别为AB=c，BC=a，CA=b；
  121.   如果你想知道A的度数，参数依次为bca，
  122. 如果你想知道B的度数，参数依次为acb，
  123. 如果你想知道C的度数，参数依次为abc，
  124. */
  125. float Angle(float a,float b,float c)//求任意三角形的每个角的角度
  126. {
  127. float angle;
  128. angle=acos((pow(a,2)+pow(b,2)-pow(c,2))/(2*a*b));
  129. angle=angle*180/PI;
  130. if(angle<0)angle=180-abs(angle);
  131. return angle;
  132. }

  134. void Left_leg_Movement()//左腿运动
  135. {
  136. float CAB,ABC,BCA,CAD,ACD;//设置角度参数
  137. //下面开始计算所有要用的角度
  138. LD_LH=Right_Angle_Hypotenuse(LD_H,abs(LD_LX));//计算斜边
  139. CAB=Angle(LD_L1H,LD_LH,LD_L2H);
  140. ABC=Angle(LD_L1H,LD_L2H,LD_LH);
  141. BCA=Angle(LD_LH,LD_L2H,LD_L1H);
  142. CAD=Right_Angle(LD_H,abs(LD_LX));
  143. ACD=Right_Angle(abs(LD_LX),LD_H);

  145. if(LD_LX<=0)
  146. {
  147.   LD_L1Angle=(int)(180-CAD-CAB)/7;
  148.   LD_L3Angle=(int)(BCA+ACD)/7-6;
  149. }
  150. if(LD_LX>0)
  151. {
  152.   LD_L1Angle=(int)(180+CAD-CAB)/7;
  153.   LD_L3Angle=(int)(180-ACD+BCA)/7-6;
  154. }
  155. LD_L2Angle=(int)(180-ABC)/7;

  157.   
  158. }

  160. void Timer0Int() interrupt 1
  161. {
  162. /*
  163. TH0=(65536-(FOSC/12*TIME_US)/1000000)/256;
  164. TL0=(65536-(FOSC/12*TIME_US)/1000000)%256;
  165. */

  167. count++;
  168. if(count > 200)
  169. {
  170.   count=0;
  171. }
  172. if(LD_L1Angle<7)LD_L1Angle=7;if(LD_L1Angle>20)LD_L1Angle=20;
  173. if(LD_L2Angle<7)LD_L2Angle=7;if(LD_L2Angle>20)LD_L2Angle=20;
  174. if(LD_L3Angle<7)LD_L3Angle=7;if(LD_L3Angle>20)LD_L3Angle=20;

  176. /*

  178. if(count<LD_L1Angle){D1=1;}else{D1=0;}
  179. if(count<LD_L2Angle){D2=1;}else{D2=0;}
  180. if(count<LD_L3Angle){D3=1;}else{D3=0;}

  182. */
  183. switch(pwm_flag)
  184. {
  185. case 1:
  186.   D1=1;
  187.   TH0=-pwm[0]>>8;
  188.   TL0=-pwm[0];
  189.   break;
  190. case 2:
  191.   D1=0;
  192.   TH0=-(ms2_5con-pwm[0])>>8;
  193.   TL0=-(ms2_5con-pwm[0]);
  194.   pwm_flag=0;
  195.   break;
  196. case 3:
  197.   TH0=0xff;
  198.   TL0=0x80;
  199.   pwm_flag=0;
  200.   break;
  201. }
  202. pwm_flag++;


  205. switch(y)
  206.   {
  207.   case 0:
  208.    pwm[0]+=10;
  209.    if(pwm[0]>2000)
  210.    {
  211.     y=1;
  212.    }
  213.    break;
  214.   case 1:
  215.    pwm[0]-=10;
  216.    if(pwm[0]<500)
  217.    {
  218.     y=0;
  219.    }
  220.    break;
  221.   }
  222. }
  223. void ser() interrupt 4
  224. {

  226.   
  227.   
  228. }
复制代码








分享到:  QQ好友和群QQ好友和群 QQ空间QQ空间 腾讯微博腾讯微博 腾讯朋友腾讯朋友
收藏收藏2 分享淘帖 顶顶 踩踩
回复

使用道具 举报

沙发
ID:277392 发表于 2018-11-1 20:34 | 只看该作者
很强啊老哥，可以交流一下
回复

使用道具 举报

板凳
ID:419977 发表于 2018-11-3 01:08 | 只看该作者
可以啊，帮顶
回复

使用道具 举报

返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表