四自由度机械臂
机械臂物体抓取
本次代码可以实现的目的:
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;
if ((b * b - 4 * a * c) >= 0)
{
theta2 = (-b - sqrt(b * b - 4 * a * c)) / 2 / a;
theta2 = asin(theta2) * 180 / PI;
}
k = (l2 * l2 + m * m + n * n - l1 * l1) / 2 / l2;
a = m * m + n * n;
b = -2 * k * m;
c = k * k - n * n;
if ((b * b - 4 * a * c) >= 0)
{
theta3 = (-b + sqrt(b * b - 4 * a * c)) / 2 / a;
theta3 = asin(theta3) * 180 / PI;
}
theta3 = theta3 - theta2;
theta2 = 90 - theta2 - 3; //3du error
theta3 = 90 - theta3;
theta4 = 180 - theta2 - theta3 + Alpha;
theta1 = theta;
if (theta1 > 180)
theta1 = 180;
if (theta2 > 180)
theta2 = 180;
if (theta3 > 180)
theta3 = 180;
if (theta4 > 180)
theta4 = 180;
if (theta5 > 180)
theta5 = 180;
if (theta6 > 180)
theta6 = 180;
if (theta1 < 0)
theta1 = 0;
if (theta2 < 0)
theta2 = 0;
if (theta3 < 0)
theta3 = 0;
if (theta4 < 0)
theta4 = 0;
if (theta5 < 0)
theta5 = 0;
if (theta6 < 0)
theta6 = 0;
}
/******************************************************
开始转动,每个舵机同时到达指定地点
*******************************************************/
void go()
{
float a,b,c,d,e,f;
a = myservo1.read();
b = myservo2.read();
c = myservo3.read();
d = myservo4.read();
e = myservo5.read();
f = myservo6.read();
unsigned long starttime;
starttime = millis();
while ((millis() - starttime) < 4000) //类似于P控制
{
a=a-(a-theta1)/20.00;
myservo1.write(a);
b=b-(b-theta2)/20.00;
myservo2.write(b);
c=c-(c-theta3)/20.00;
myservo3.write(c);
d=d-(d-theta4)/20.00;
myservo4.write(d);
e=e-(e-theta5)/20.00;
myservo5.write(e);
f=f-(f-theta6)/20.00;
myservo6.write(f);
delay(50 - Speed);
}
}
/******************************************************
setup初始化
*******************************************************/
void setup() {
myservo1.attach(10, 500, 2500); //初始化各个舵机
myservo2.attach(9);
myservo3.attach(8);
myservo4.attach(7);
myservo5.attach(6);
myservo6.attach(5, 500, 2500);
myservo1.write(theta);
myservo2.write(theta - 3);
myservo3.write(theta);
myservo4.write(theta);
myservo5.write(theta);
myservo6.write(theta);
delay(3000);
Serial.begin(9600);
}
/******************************************************
沿正方形轨迹运动
*******************************************************/
void loop() {
Kinematic_Analysis(y, z, theta, alpha);
go();
Kinematic_Analysis(y-50, z,theta, alpha);
go();
Kinematic_Analysis(y-50, z-50, theta,alpha);
go();
Kinematic_Analysis(y, z-50 ,theta, alpha);
go();
}
/******************************************************
抓取物品
*******************************************************/
//void loop() {
//
// Kinematic_Analysis(125, 100, 90, -90);theta5=50;
// go();theta5=100;go();
// Kinematic_Analysis(150, 300,90, 0);
// go();
// Kinematic_Analysis(150, 300, 0,0);
// go();
// Kinematic_Analysis(120, 80 ,0, -90);
// go();theta5=50;go();
//
//}