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

四自由度机械臂

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

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();
//
//}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156