基于arduino UNO控制的四自由度机械手臂物体的抓取

Genicre_

于 2019-11-08 17:35:05 发布

阅读量1.2w

点赞数 25

文章标签：机械臂

本文链接：https://blog.csdn.net/weixin_43498585/article/details/102977128

版权

四自由度机械臂

在这里插入图片描述

机械臂物体抓取

本次代码可以实现的目的：

1末端在一个yoz平面上画一个正方形

2物品的抓取

#include <Servo.h>
#define l1 105  //机械臂的参数
#define l2 100
#define l3 105
Servo myservo1, myservo2, myservo3, myservo4, myservo5, myservo6;

float Speed =25;   //舵机速度
float theta1 = 90, theta2, theta3, theta4, theta5 = 90, theta6 = 90;
float y = 150, z = 300, theta = 90, alpha = 0;
/**************************************************************************
  函数功能：数学模型
  入口参数：末端执行器位姿态
  返回  值：无
**************************************************************************/
void Kinematic_Analysis(float y, float z, float theta, float Alpha)       //Alpha=[0,180]
{
   
  float m, n, k, a, b, c;
  m = y - l3 * cos(Alpha); //中间变量
  n = z - l3 * sin(Alpha) - 108; //中间变量

  k = (l1 * l1 + m * m + n * n - l2 * l2) / 2 / l1;
  a = m * m + n * n;
  b = -2 * k * m;
  c = k * k - n * n;