python 写字机器人 写字机器人软件程序

写此博客的主要目的是为了记录下来调试过程中所遇到的问题，更重要的是记录下来解决遇到的那些问题的方法，以供之后此后遇到问题之参考。
该写字机器人或叫机械臂的主控芯片为STM32F103C8T6，机械臂拥有三个水平自由度，一个竖直自由度，该机械臂的关节处均用的是舵机驱动。
从此出发，我们便需要用STM32的定时器外设产生四路能够准确驱动舵机的PWM波。初始化定时器外设的部分代码如下。

GPIO_InitTypeDef GPIO_InitStructure;  
 TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;  
 TIM_OCInitTypeDef  TIM_OCInitStructure; 

RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);// ①使能 tim3时钟  
 RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA , ENABLE);   //①使能 GPIO 外设时钟使能        
 RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB , ENABLE); 
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6; //TIM3_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_TimeBaseInit(TIM3, &TIM_TimeBaseStructure); //②初始化 TIMx 
 TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1; //脉宽调制模式 2  
 TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable; //比较输出使能
 TIM_OCInitStructure.TIM_Pulse=500; //设置待装入捕获比较寄存器的脉冲值 
 TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High; //输出极性高 
 TIM_OC1Init(TIM3, &TIM_OCInitStructure);  //③初始化外设 TIMx 
 TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable);  //CH1预装载使能 
TIM_CtrlPWMOutputs(TIM3,ENABLE); //⑤MOE 主输出使能  
 TIM_ARRPreloadConfig(TIM3, ENABLE); //使能 TIMx 在 ARR 上的预装载寄存器    
 TIM_Cmd(TIM3, ENABLE);  //④使能 TIM3

上面的程序便是产生了可以控制舵机的旋转的四路PWM信号由于篇幅原因只写了一路。舵机的控制周期为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);//第三个舵机的角度
  TIM_SetCompare2(TIM3,s_2);
  //********改动*******
  TIM_SetCompare3(TIM3,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("\r\n\r\n"); //串口发送数据方便调试
}
prex=prex+dx;
prey=prey+dy;//更新坐标
printf("初始坐标%f%f",prex,prey);
}

第三个函数实现抬笔落笔功能，代码如下

void lift(int sta)//1抬笔0落笔
{int i=0;
 if(sta) { TIM_SetCompare4(TIM3,1700);delay_ms(1000); }
 else 
 {for(i=0;i<=10;i++)
 {TIM_SetCompare4(TIM3,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）三角形画好

本程序在调试过程中也遇到了很多问题，还有待解决的便是坐标定位的准确性问题。但写个正字三角形还是可以的。效果图如下

换根粗笔效果或许更好一点。调试的时候是一个一个函数试其能否完成该函数功能。