使用arduino+舵机+步进电机+泵+灯带制作一个自动倒茶机机器人

1 应用背景
发扬一下我国传统文化，倒茶有九序：烫壶->置茶->温杯->高冲->低泡->分茶->敬茶->闻香->品茶。倒茶是一个需要有礼节且又繁杂不停的事情，由于客人茶杯较小，需要不停加茶，然后递送客人。使用机器人替代这个过程，减少倒茶、送茶流程。

2 结构设计

机器人需要在桌子角落的茶杯抓起，并放到桌子上的相应位置。在末端位置，需要一个手爪能够完成抓取和释放的动作。为了方便沏茶过程，与手爪连接的连杆保持水平，因此手爪的轴线与水平面垂直。向杯子注入茶水的水管与手爪的轴线保持水平。综上所述，机器人需要五个自由度，其中四个自由度为关节旋转自由度，另一个为手爪的局部自由度。

3 控制总体设计
本次设计采用Arduino的芯片作为主控板。关节角度的控制依据于对机械手臂建模之后的正运动学和逆运动学，通过运动学计算得出关节空间与手臂末端的对应关系，进而通过对各个关节的控制实现目的。

使用材料清单如下：

4 倒茶机整体结构图
制作过程省略太多，一切的不顺利，东凑凑西凑凑终于凑出了个样子了。

5 调试程序
程序是在arduino IDE上编写的，个人觉得这次编写的程序实在冗杂，不是好程序，没得办法，主要考虑马上把这个东西做好交差，读者们可以做适当优化吧。

#include <Wire.h>
#include <Servo.h>
#include <Adafruit_NeoPixel.h>
#include <avr/power.h>

/舵机引脚定义/
#define ServoPin1 0
#define ServoPin2 1
#define ServoPin3 2

/爪子的引脚定义/
#define MechanicalClawPin1 10
#define MechanicalClawPin2 11

/压力传感器引脚定义/
#define Sensor1Pin A0
#define Sensor2Pin A1
#define Sensor3Pin A2

/定义电机是否开闭/
#define Open 1
#define Close 0

/指定机械臂位置状态/
#define InitialPosition_UP 1
#define InitialPosition_DOWN 2
#define TargetPosition_UP 3
#define TargetPosition_DOWN 4

/泵的引脚定义/
#define PumpPin 9

/步进电机引脚定义/
#define dirPin 8
#define stepperPin 7
int MM=560;

/灯带的相关宏定义/
#define LEDPin 7
#define LEDCount 30
Adafruit_NeoPixel pixels = Adafruit_NeoPixel(LEDCount, LEDPin, NEO_GRB + NEO_KHZ800);

/舵机对象的建立/
Servo myservo1;
Servo myservo2;
Servo myservo3;

void setup()
{
/设置波特率9600/
Serial.begin(9600);
/步进电机引脚初始化/
pinMode(dirPin, OUTPUT);
pinMode(stepperPin, OUTPUT);
/舵机初始化/
myservo1.attach(ServoPin1);
myservo2.attach(ServoPin2);
myservo3.attach(ServoPin3);

/机械爪的引脚初始化/
pinMode(MechanicalClawPin1, OUTPUT);
pinMode(MechanicalClawPin2, OUTPUT);

/水泵引脚初始化/
pinMode(PumpPin, OUTPUT);

/灯带初始化/
#if defined (AVR_ATtiny85)
if (F_CPU == 16000000) clock_prescale_set(clock_div_1);
#endif
pixels.begin();//初始化灯带
/打开LED，流水灯，不同颜色,完成杯子的摆放动作/
CupPositionInitialize( );
Serial.println(“CupPositionInitialize()have completed”);
}

void loop()
{
/* 用于检测杯子水量的接口 */
int PressureSensor1 = analogRead(Sensor1Pin);
int PressureSensor2 = analogRead(Sensor2Pin);
int PressureSensor3 = analogRead(Sensor3Pin);

if( PressureSensor1<100)
   {
    delay(500);
    if(PressureSensor1<100)
      {
        RobotAction_PourWaterToCup1();
        Serial.println("Fill the Cup1 with tea");
      }
   }

if( PressureSensor2<100)
  {
   delay(500);
   if(PressureSensor2<100)
      {
        RobotAction_PourWaterToCup2();
        Serial.println("Fill the Cup2 with tea");
       }
   } 

 if( PressureSensor3<100)
    {
     delay(500);
     if(PressureSensor3<100)
       {
        RobotAction_PourWaterToCup3();
        Serial.println("Fill the Cup3 with tea");
       }
    }     

}
/步进电机的转向和步数设置函数/
void step(boolean dir,int steps)
{
digitalWrite(dirPin,dir);
delay(50);
for(int i=0;i<steps;i++)
{ digitalWrite(stepperPin, HIGH);
delayMicroseconds(MM);
digitalWrite(stepperPin, LOW);
delayMicroseconds(MM);
}
}

/将初始位置的三个杯子摆放到目标位置，并打开LED灯带/
void CupPositionInitialize( void )
{
LED(Open);
RobotAction_GrabCup1();
delay(500);
RobotAction_GrabCup2();
delay(500);
RobotAction_GrabCup3();
delay(500);
RobotReset();
delay(500);
LED(Close);
}

/倒水到cup1中的动作/
void RobotAction_PourWaterToCup1( void )
{
LED(Open);
step(true,3500);
RobotState_Cup1Position( TargetPosition_UP );
delay(500);
// RobotState_Cup1Position( TargetPosition_DOWN );
// delay(500);
// Pump(Open);
// delay(5000);
// Pump(Close);
// RobotState_Cup1Position( TargetPosition_UP );
// delay(500);
LED(Close);
RobotReset();
step(false,3500);
delay(1000);
}

/倒水到cup2中的动作/
void RobotAction_PourWaterToCup2( void )
{
LED(Open);
step(true,0);
RobotState_Cup2Position( TargetPosition_UP );
delay(500);
// RobotState_Cup2Position( TargetPosition_DOWN );
// delay(500);
// Pump(Open);
// delay(5000);
// Pump(Close);
// RobotState_Cup2Position( TargetPosition_UP );
// delay(500);
LED(Close);
RobotReset();
step(false,0);
delay(1000);
}

/倒水到cup3中的动作/
void RobotAction_PourWaterToCup3( void )
{
LED(Open);
step(false,3000);
RobotState_Cup3Position( TargetPosition_UP );
delay(500);
// RobotState_Cup3Position( TargetPosition_DOWN );
// delay(500);
// Pump(Open);
// delay(5000);
// Pump(Close);
// RobotState_Cup3Position( TargetPosition_UP );
// delay(500);
LED(Close);
RobotReset();
step(true,3000);
delay(1000);
}

/机械臂拿初始位置1的杯子到指定位置1处/
void RobotAction_GrabCup1( void )
{
step(false,5000);
RobotState_Cup1Position( InitialPosition_UP );
delay(500);
Grab_Cups(Open);
RobotState_Cup1Position( InitialPosition_DOWN );
delay(500);
Grab_Cups(Close);
delay(500);
step(true,8500);//目标位置的step是 3500 true
RobotState_Cup1Position(TargetPosition_UP);
delay(500);
RobotState_Cup1Position(TargetPosition_DOWN);
delay(500);
Grab_Cups(Open);
delay(2000);
RobotReset(); //先复位再旋转
step(false,3500); //回到中心位置
}

/机械臂拿初始位置2的杯子到指定位置2处/
void RobotAction_GrabCup2( void )
{
step(false,4000);
RobotState_Cup2Position( InitialPosition_UP );
delay(500);
Grab_Cups(Open);
RobotState_Cup2Position( InitialPosition_DOWN );
delay(500);
Grab_Cups(Close);
delay(500);
step(true,4000);
RobotState_Cup2Position(TargetPosition_UP);
delay(500);
RobotState_Cup2Position(TargetPosition_DOWN);
delay(500);
Grab_Cups(Open);
delay(2000);
RobotReset(); //先复位再旋转
}

/机械臂拿初始位置3的杯子到指定位置3处/
void RobotAction_GrabCup3( void )
{
step(false,3500);
RobotState_Cup3Position( InitialPosition_UP );
delay(500);
Grab_Cups(Open);
RobotState_Cup3Position( InitialPosition_DOWN );
delay(500);
Grab_Cups(Close);
delay(500);
step(true,500);
RobotState_Cup3Position(TargetPosition_UP);
delay(500);
RobotState_Cup3Position(TargetPosition_DOWN);
delay(500);
Grab_Cups(Open);
delay(2000);
RobotReset(); //先复位再旋转
step(true,3000);
}

/机械臂关于杯子1的位置状态/
void RobotState_Cup1Position ( int cmd1 )
{
switch(cmd1)
{
case 1:
myservo1.write(90);
delay(500);
myservo2.write(95);
delay(500);
myservo3.write(50);
delay(500);
break;
case 2:
myservo1.write(100);
delay(500);
myservo2.write(100);
delay(500);
myservo3.write(50);
delay(500);
break;
case 3:
myservo1.write(100);
delay(500);
myservo2.write(80);
delay(500);
myservo3.write(60);
delay(500);
break;
case 4:
myservo1.write(105);
delay(500);
myservo2.write(80);
delay(500);
myservo3.write(45);
delay(500);
break;
}
}
/机械臂关于杯子2的位置状态/
void RobotState_Cup2Position(int cmd2 )
{
switch(cmd2)
{
case 1:
myservo1.write(70);
delay(500);
myservo2.write(120);
delay(500);
myservo3.write(50);
delay(500);
break;
case 2:
myservo1.write(90);
delay(500);
myservo2.write(130);
delay(500);
myservo3.write(30);
delay(500);
break;
case 3:
myservo1.write(130);
delay(500);
myservo2.write(30);
delay(500);
myservo3.write(80);
delay(500);
break;
case 4:
myservo1.write(140);
delay(500);
myservo2.write(30);
delay(500);
myservo3.write(70);
delay(500);
break;
}
}
/机械臂关于杯子3的位置状态/
void RobotState_Cup3Position( int cmd3 )
{
switch(cmd3)
{
case 1:
myservo1.write(90);
delay(500);
myservo2.write(95);
delay(500);
myservo3.write(50);
delay(500);
break;
case 2:
myservo1.write(100);
delay(500);
myservo2.write(100);
delay(500);
myservo3.write(50);
delay(500);
break;
case 3:
myservo1.write(120);
delay(500);
myservo2.write(50);
delay(500);
myservo3.write(70);
delay(500);
break;
case 4:
myservo1.write(125);
delay(500);
myservo2.write(55);
delay(500);
myservo3.write(55);
delay(500);
break;
}
}
/机器臂复位的位置/
void RobotReset( void )
{
myservo1.write(90);
delay(500);
myservo2.write(10);
delay(500);
myservo3.write(90);
delay(500);
}

/开启水泵加水,可以使用继电器，也可以使用驱动/
void Pump (int cmd3)
{
switch(cmd3)
{ case 1:
digitalWrite(PumpPin, HIGH);
break;
case 2:
digitalWrite(PumpPin, LOW);
break;
}
}
/灯带的函数，设置有开关/
void LED (int cmd5)
{ if (cmd50)
{
for(int j=30;j<LEDCount;j–)
{ pixels.setPixelColor(j, pixels.Color(250-j5,j+20,j5+100));
pixels.show();
delay(20);
}
}
if (cmd51)
{
for(int i=0;i<LEDCount;i++)
{ pixels.setPixelColor(i, pixels.Color(250-i5,i+20,i5+100));
pixels.show();
delay(20);
}
}

    }

/抓取杯子,打开或关闭爪子/
void Grab_Cups(int cmd9 )
{
if (cmd91)
{
digitalWrite(MechanicalClawPin1, HIGH);
digitalWrite(MechanicalClawPin2, LOW);
delay(5000);
}
if (cmd90)
{
digitalWrite(MechanicalClawPin1, LOW);
digitalWrite(MechanicalClawPin2, HIGH);
delay(5000);
}
}