此博客的主要目的是记录调试中遇到的问题,更重要的是记录如何解决遇到的问题,以供后续遇到的问题参考。
写字机器人或称机械臂的主控芯片为STM32F103C8T6,机械臂具有三个水平自由度,一个垂直自由度,该机械臂的关节采用舵机驱动。
接下来,我们需要使用STM32的定时外围设备产生能够正确驱动舵机的4个系统的PWM波。 初始化计时器外围设备的部分代码如下。
gpio _ inittypedefgpio _ init structure;
lyd tk _ timebaseinittypedeflydtk _ time base structure;
lyd tk _ ocinittypedeflydtk _ ocinitstructure;
RCC _ AP B1 periphclockcmd (RCC _ AP B1 per iph _ lyd tk 3,ENABLE ); //沉默不语的美女三个时钟
RCC _ AP B2 periphclockcmd (RCC _ AP B2 per iph _ gpioa,ENABLE );//启用gpio外围设备的时钟启用码
RCC _ AP B2 periphclockcmd (RCC _ AP B2 per iph _ gpiob,ENABLE );
gpio _ init structure.gpio _ pin=gpio _ pin _ 6; //lydtk3_CH1
gpio _ init structure.gpio _ mode=gpio _ mode _ af _ PP; //复用推挽输出
gpio _ init structure.gpio _ speed=gpio _ speed _ 50m Hz;
gpio_init(gpioa,GPIO_InitStructure );
lyd tk _ time base structure.lyd tk _ period=arr; //读取下一个更新事件设置活动自动重新读取寄存器周期值80K
lyd tk _ time base structure.lyd tk _ prescaler=PSC; 将用作lydtkx时钟频率除数的预分频值设定为不分频
lyd tk _ time base structure.lyd tk _ clock division=0; //时钟分割:TDTS=Tck_设定沉默的美女
lyd tk _ time base structure.lyd tk _ counter mode=lyd tk _ counter mode _ up; //递增计数
lydtk_timebaseinit(lydtk3,lydtk_TimeBaseStructure );//初始化lydtkx
lyd tk _ ocinitstructure.lyd tk _ oc mode=lyd tk _ oc mode _ PWM 1; //脉宽调制模式2
lyd tk _ ocinitstructure.lyd tk _ output state=lyd tk _ output state _ enable; //比较输出使能
lyd tk _ ocinitstructure.lyd tk _ pulse=500; //设定要加载到捕捉比较寄存器中的脉冲值
lyd tk _ ocinitstructure.lyd tk _ oc polarity=lyd tk _ oc polarity _ high; //输出极性高
lydTK_oC1init(lydTK3,lydtk_OCInitStructure ); //初始化外围设备lydtkx
lydTK_oC1preloadconfig(lydTK3,lydtk_OCPreload_Enable ); //CH1预加载使能
lydTK_ctrlpwmoutputs(lydTK3,ENABLE ); //MOE主输出使能
lydTK_arrpreloadconfig(lydTK3,ENABLE ); 启用lydtkx预加载到ARR的寄存器
lydTK_cmd(lydTK3,ENABLE );//启用lyd tk 3
上述步骤产生了可以控制舵机旋转的4个系统的PWM信号,由于篇幅的原因只写了1个系统。 舵机控制周期为20ms,控制高电平时间可以转动,0.5ms对应-90,1ms对应-45…2.5ms对应90,最大可达90。 我在初始化时把三台水平舵机调到-90,一台垂直舵机调到0。 (组装的原因要比顶板水平。
舵机的机械结构如下
如果以舵机2为原点构建直角坐标系,现在的目标是如何在该平面上的任意两点之间正确地画直线。 从该机械结构的角度看,舵机无论如何运动都是曲线,所以只能将直线分割为许多段,每一段舵机运动的轨迹确实是曲线,但如果该段非常微小,在较大的范围内可以近似地看作是该直线上的曲线
先设定初始位置,舵机二、三设定为-90,伸臂沿X纯真毛衣方向,每个关节只能逆时针旋转。 那么,可以在决定的第一象限的任意一点(手臂展半径的范围内)进行计算
算出要达到该点俩个舵机应该所处的角度,那么将俩个水平舵机转到计算的应该所处的角度即可到达我们规定要转动到的位置坐标。第一个函数实现的功能便是给定任意一个合理的目标点,我们便控制俩个水平舵机的角度将机械臂旋转至该点。具体物理量如下图
#define L1 11.2//LI臂长
#define L2 6.5//L2臂长
#define PI 3.1415926//PI
void set_XY(double x_1,double y_1)
{
delay_ms(1);
double m;//到原点长度
double a1;
double a2;
double a3;//其它角度变量
u16 s_1,s_2,s_3;//三个舵机的角度
m=sqrt(x_1* x_1 + y_1 * y_1);
a1= return_angle(L1, m, L2);//余弦定理
a2= atan2(x_1, y_1); //
a3=return_angle(L1, L2, m);
s_2=floor(((a2-a3)/PI)*2000+500);//第二个舵机的角度
s_3=floor(((PI-a1)/PI)*2000+500);//第三个舵机的角度
lydtk_SetCompare2(lydtk3,s_2);
//********改动*******
lydtk_SetCompare3(lydtk3,s_3);
delay_ms(200);//******改动*********
}
下面便设计第二个函数,该函数实现的是分很多微元从当前点移动到目标点,会套用之前写的第一个函数以实现。
void movepot(double x,double y,double prex,double prey )
{//prex prey 是当前位置坐标
double dx;double dy;//x,y坐标变化
double distance;
int c;
int i;
dx=x-prex;
dy=y-prey;
distance=sqrt(dx * dx + dy * dy);//两点距离
c=floor(6*distance);//1cm分6步走
if(c<1){c=1;}
for(i=0;i<=c;i++)
{
set_XY(prex + (i * dx / c), prey+ (i * dy / c));
printf("ok");
printf("现在坐标%f",(prex + (i * dx / c)));
printf("rnrn"); //串口发送数据方便调试
}
prex=prex+dx;
prey=prey+dy;//更新坐标
printf("初始坐标%f%f",prex,prey);
}
第三个函数实现抬笔落笔功能,代码如下
void lift(int sta)//1抬笔0落笔
{int i=0;
if(sta) { lydtk_SetCompare4(lydtk3,1700);delay_ms(1000); }
else
{for(i=0;i<=10;i++)
{lydtk_SetCompare4(lydtk3,2000-50*i);delay_ms(500);}}//慢慢落下
}
第四第五个函数便实现了使机械臂先抬笔之后以一个较慢的速度移动到目标点之后再慢慢落下(quikmove函数)
void movepotslow(double x,double y,double prex,double prey )//抬起来较快移动
{
double dx;double dy;//x,y坐标变化
double distance;
int c;
int i;
dx=x-prex;
dy=y-prey;
distance=sqrt(dx * dx + dy * dy);
c=floor(distance);
if(c<1){c=1;}
for(i=0;i<=c;i++)
{
set_XY(prex + (i * dx / c), prey+ (i * dy / c));
}
prex=prex+dx;
prey=prey+dy;//更新坐标
}
void quickmove(double x,double y,double prex,double prey)//快速移动先抬后落
{
lift(1);
delay_ms(500);
movepotslow(x,y,prex,prey);
lift(0);
}
设计了如上的函数之后我们便可以利用上述函数来进行绘制图形或者写字,画三角形的具体步骤举例如下:
quickmove(13,5,0,17.5);
//先调用此函数让笔从起始点快速抬笔移动到画三角形处再落笔
movepot(13,10,13,5);
//再调用此函数让笔从现在点(13,5)画到(13,10)
movepot(9,6,13,10);
//再调用此函数让笔从现在点(13,10)画到(9,6)
movepot(13,6,9,6);
//再调用此函数让笔从现在点(9,6)画到(13,6)三角形画好
本程序在调试过程中也遇到了很多问题,还有待解决的便是坐标定位的准确性问题。但写个正字三角形还是可以的。效果图如下
换根粗笔效果或许更好一点。调试的时候是一个一个函数试其能否完成该函数功能。