机器谱

R037】Delta并联机械臂

作者:机器谱

图文展示3264(1)

图文展示3264(1)

副标题

电磁铁搬运
逆运动学控制

电磁铁搬运

1. 功能说明

   R037样机是一款Delta并联机械臂。本文示例将利用Delta并联机械臂实现不同点定点搬运磁铁物料的效果。

2. 结构说明

   Delta并联机械臂,其驱动系统采用精度较高的42步进电机;传动系统为丝杠和万向球节;执行末端为搭载电磁铁的圆盘支架。

3. Delta机械臂运动学算法

   这里给大家介绍一种Delta并联机械臂的运动轨迹解法,通过控制电机的转动参数,最终求解出电磁铁圆盘支架的运动轨迹规律,样机采用R037b

      该机械臂由3个丝杠平台构成,通过并联的方式同时控制同一个端点的运动;三个丝杠位于一个正三角形边线的中心位置,连杆采用球头万向节连杆结构。

① 首先我们建立一个空间直角坐标系,该直角坐标系以三个丝杠平台在俯视图方向投影的内切圆心为原点,x轴与tower1和tower3之间的连线平行,y轴过tower2,其中z=0的平面设置在三个限位开关所在平面。

② 建立坐标系之后,我们可以得出3个限位开关Z轴投影的坐标为A=(-msin(60°),mcos(60°),0);B=(0,m,0);C=(msin(60°),mcos(60°),0);其中m为在xy投影面上正三角形的内切圆心到B点的距离。

③确定各限位开关的位置(即确定各丝杠平台上滑块的初始位置),丝杠平台的运动可简化为如下:【其中N点为滑块初始位置,Q点为端点初始位置,P为Q点在丝杠平台上Z轴的投影;N1,P1,Q1点为丝杠平台运动后的位,T点为某一固定点,假设为delta机械臂上端点在Z向可以运动的最大值在丝杠平台Z向的投影点】

④逆运动学是根据Q1点的位置确定NN1的距离。     

在图中有几个可以通过测量得到已知值,分别是连杆长度NQ/N1Q1、NT的距离、终点Q1点的坐标;假设我们输入的量是终点Q1的坐标(X1,Y1,Z1);这里需要注意的是Z1坐标为负值,为了方便理解在后面的推导中我们都对Z1取绝对值。     

我们需要计算的是NN1的距离:

其中Q1的Z坐标与P1的Z坐标一致,所以NP1为已知量为Q1的Z坐标值Z1,即可以将上面的公式改为:

这里我们只需要计算出N1P1的值即可:

其中NIQ1为连杆长度,可通过测量得知,所以这里面需要我们计算出Q1P1。     

⑤求出Q1P1:【该长度我们可以通过两点坐标距离公式得出,借助俯视图投影进行计算】

为方便计算Q1P1,图中我们将N,N1,P,P1,T点都投影到Z为0的点,则Q1(X1,Y1,0)。

根据点坐标公式可得:

综上所述:

注意前面我们对Z1取了一次绝对值,实际Z1为负值,

所以最终推导公式为:

这样我们就求出了NN1(丝杠移动距离)与Q1(执行端运动的终点)坐标的关系。

4. 功能实现

4.1 电子硬件

在这个示例中,我们采用了以下硬件,请大家参考:

主控板

Basra主控板(兼容Arduino Uno)

扩展板Bigfish2.1
SH-ST扩展板
传感器触碰传感器
电机步进电机
电池11.1v动力电池

其它

电磁铁、USB线


/*------------------------------------------------------------------------------------

  版权说明:Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 机器谱 2023-02-08 https://www.robotway.com/                                 

------------------------------------------------------------------------------------*/

#include "Arduino.h"

#include <AccelStepper.h>

#include <MultiStepper.h>

#include "Configuration.h"


AccelStepper stepper_x(1, 2, 5);      //tower1

AccelStepper stepper_y(1, 3, 6);      //tower2

AccelStepper stepper_z(1, 4, 7);      //tower3

//AccelStepper stepper_a(1, 12, 13);


MultiStepper steppers;


float delta[NUM_STEPPER];                         

float cartesian[NUM_AXIS] = {0.0, 0.0, 0.0};         //当前坐标

float destination[NUM_AXIS];                         //目标坐标

boolean dataComplete = false;


float down = -111;

float up = -105;


/*********************************************Main******************************************/

void setup() {

  Serial.begin(9600);

  pinMode(EN, OUTPUT);


  steppers.addStepper(stepper_x);

  steppers.addStepper(stepper_y);

  steppers.addStepper(stepper_z);


  stepperSet(1600, 400.0);

  stepperReset();

  delay(1000);


  Get_command(0, 0, down);

  Process_command();

 

  delay(1000);

}


void loop() {

  float r = 25;

  float x1 = 0.0;

  float y1 = 0.0;


  Get_command(25, 0, down);

  Process_command();

  delay(1000);

 

  for(int i=0;i<2;i++){

    for(float i=0.0;i<=360;i+=10){

      x1 = r * cos(i / 180 * 3.141592);

      y1 = r * sin(i / 180 * 3.141592);

     

      Get_command(x1, y1, down);

      Process_command();                                     

    }

  }

  delay(1000);


  for(int j=0;j<2;j++){

    for(float i=360.0;i>=0;i-=10){

      x1 = r * cos(i / 180 * 3.141592);

      y1 = r * sin(i / 180 * 3.141592);

     

      Get_command(x1, y1, down);

      Process_command();                                     

    }

  }

  delay(1000);




  Get_command(0, 0, down);

  Process_command();  

 

  test();

  delay(1000);

 

  stepperReset();

  delay(1000);

 

}


/***************************************Get_commond*******************************************/

void Get_command(float _dx, float _dy, float _dz){  

  destination[0] = _dx;

  destination[1] = _dy;

  destination[2] = _dz;

 

  if(destination[0] == 0 && destination[1] == 0 && destination[2] == 0){

      stepperReset();  

  }

  else

  {

      dataComplete = true;

  }

 

  if(serial_notes){

   Serial.print("destinationPosition: ");

   Serial.print(destination[0]);

   Serial.print(" ");

   Serial.print(destination[1]);

   Serial.print(" ");

   Serial.println(destination[2]);

  }


}


/***************************************Process_command***************************************/

void Process_command(){

  if(dataComplete){

    digitalWrite(EN, LOW);

   

    if(cartesian[0] == destination[0] && cartesian[1] == destination[1] && cartesian[2] == destination[2]){

      return;  

    }

    else

    {


      Line_DDA(destination[0], destination[1], destination[2]);

    }

   

  }

  dataComplete = false;

  digitalWrite(EN, HIGH);

}


/************************************************** DDA ************************************************/

void Line_DDA(float x1, float y1, float z1)

{

  float x0, y0, z0;         // 当前坐标点

  float cx, cy;             // x、y 方向上的增量

 

  x0 = cartesian[0];y0 = cartesian[1];z0 = cartesian[2];

   

  int steps = abs(x1 - x0) > abs(y1 - y0) ? abs(x1 - x0) : abs(y1 - y0);

 

  cx = (float)(x1 - x0) / steps;

  cy = (float)(y1 - y0) / steps;

 

  for(int i = 0; i <= steps; i++)

  {

    cartesian[0] = x0 - cartesian[0];

    cartesian[1] = y0 - cartesian[1];

    cartesian[2] = z1 - cartesian[2];


    calculate_delta(cartesian);

   

    stepperSet(1350.0, 50.0);

    stepperMove(delta[0], delta[1], delta[2]);

 

    cartesian[0] = x0;

    cartesian[1] = y0;

    cartesian[2] = z1;

 

    x0 += cx;

    y0 += cy;

   

    if(serial_notes){

      Serial.print("currentPosition: ");

      for(int i=0;i<3;i++){

         Serial.print(cartesian[i]);

         Serial.print(" ");

      }   

      Serial.println();

      Serial.println("-------------------------------------------------");

    }


  }


}


/***************************************calculateDelta****************************************/

void calculate_delta(float cartesian[3])

{

  if(cartesian[0] == 0 && cartesian[1] == 0 && cartesian[2] == 0){

      delta[0] = 0; delta[1] =0; delta[2] = 0;

  }

  else

  {

      delta[TOWER_1] = sqrt(delta_diagonal_rod_2

                       - sq(delta_tower1_x-cartesian[X_AXIS])

                       - sq(delta_tower1_y-cartesian[Y_AXIS])

                       ) + cartesian[Z_AXIS];

      delta[TOWER_2] = sqrt(delta_diagonal_rod_2

                       - sq(delta_tower2_x-cartesian[X_AXIS])

                       - sq(delta_tower2_y-cartesian[Y_AXIS])

                       ) + cartesian[Z_AXIS];

      delta[TOWER_3] = sqrt(delta_diagonal_rod_2

                       - sq(delta_tower3_x-cartesian[X_AXIS])

                       - sq(delta_tower3_y-cartesian[Y_AXIS])

                       ) + cartesian[Z_AXIS];


     for(int i=0;i<3;i++){

        delta[i] = ((delta[i] - 111.96) * stepsPerRevolution / LEAD);

     }

  }


  if(serial_notes){

      Serial.print("cartesian x="); Serial.print(cartesian[X_AXIS]);

      Serial.print(" y="); Serial.print(cartesian[Y_AXIS]);

      Serial.print(" z="); Serial.println(cartesian[Z_AXIS]);

   

      Serial.print("delta tower1="); Serial.print(delta[TOWER_1]);       

      Serial.print(" tower2="); Serial.print(delta[TOWER_2]);

      Serial.print(" tower3="); Serial.println(delta[TOWER_3]);

  }


}


/****************************************stepperMove******************************************/

void stepperMove(long _x, long _y, long _z){             

    long positions[3];

    positions[0] = _x;                        //steps < 0, 向下运动 ; steps > 0, 向上运动

    positions[1] = _y;

    positions[2] = _z;


    steppers.moveTo(positions);

    steppers.runSpeedToPosition();


    stepper_x.setCurrentPosition(0);

    stepper_y.setCurrentPosition(0);

    stepper_z.setCurrentPosition(0);

}


/****************************************stepperSet******************************************/

void stepperSet(float _v, float _a){

  stepper_x.setMaxSpeed(_v);                  //MaxSpeed: 650

  stepper_x.setAcceleration(_a);  

  stepper_y.setMaxSpeed(_v);

  stepper_y.setAcceleration(_a);

  stepper_z.setMaxSpeed(_v);

  stepper_z.setAcceleration(_a);


}


/****************************************stepperReset******************************************/

void stepperReset(){

  digitalWrite(EN, LOW);

 

  if(cartesian[2] != 0){

    Get_command(0, 0, cartesian[2]);

    Process_command();

    digitalWrite(EN, LOW);

  }

 

  while(digitalRead(SENSOR_TOWER1) && digitalRead(SENSOR_TOWER2) && digitalRead(SENSOR_TOWER3)){

    stepperMove(10, 10, 10);

  }


  stepperSet(1200.0, 100.0);

  stepperMove(-400, 0, 0);

  while(digitalRead(SENSOR_TOWER1)){

    stepperMove(10, 0, 0);

   

  }


  stepperMove(0, -400, 0);

  while(digitalRead(SENSOR_TOWER2)){

    stepperMove(0, 10, 0);

  }


  stepperMove(0, 0, -400);

  while(digitalRead(SENSOR_TOWER3)){

    stepperMove(0, 0, 10);

  }


  for(int i=0;i<3;i++){

     cartesian[i] = 0.0;

  }  


  if(serial_notes) Serial.println("resetComplete!");


  digitalWrite(EN, HIGH);

}


/*************************************************** electromagnet *************************************************************/

void putUp(){

   digitalWrite(9, HIGH);

   digitalWrite(10, LOW);

}


void putDown(){

   digitalWrite(9, LOW);

   digitalWrite(10, LOW);

}


/**************************************************   test    ******************************************************************/

void test(){

    Get_command(0, 0, down);

    Process_command();

    delay(500);

    putUp();

 

    Get_command(0, 0, up);

    Process_command();  

    Get_command(25, 0, up);

    Process_command();


    Get_command(25, 0, down);

    Process_command();  

    putDown();

    delay(500);

    putDown();

    putUp();


    Get_command(25, 0, up);

    Process_command();   

    Get_command(0, 25, up);

    Process_command();   


    Get_command(0, 25, down);

    Process_command();

    putDown();  

    delay(500);

    putDown();

    putUp();


    Get_command(0, 25, up);

    Process_command();   

    Get_command(-25, 0, up);

    Process_command();   


    Get_command(-25, 0, down);

    Process_command();  

    putDown();

    delay(500);

    putUp();


    Get_command(-25, 0, up);

    Process_command();   

    Get_command(0, -25, up);

    Process_command();   


    Get_command(0, -25, down);

    Process_command();  

    putDown();

    delay(500);

    putUp();


    Get_command(0, -25, up);

    Process_command();   

    Get_command(25, 0, up);

    Process_command();   


    Get_command(25, 0, down);

    Process_command();   

    putDown();

    delay(500);

    putUp();


    Get_command(25, 0, up);

    Process_command();   

    Get_command(0, 0, up);

    Process_command();   


    Get_command(0, 0, down);

    Process_command();   

    delay(500);

    putDown();

}

4.2 电路连接说明

① 硬件连接-电子元件

各轴步进电机与SH-ST步进电机扩展板的接线顺序如下(从上至下):
X:红蓝黑绿

Y:红蓝黑绿

Z:黑绿红蓝

② 硬件连接-限位传感器

各个轴的限位传感器(触碰)与Bigfish扩展板的接线如下:
X:A0

Y:A3

Z:A4

③ 电磁铁连在Bigfish扩展板的D5、D6接口上。

4.3 编写程序

编程环境:Arduino 1.8.19

Delta机械臂有两种运动方式:第一种是自动运行搬运;第二种是用电脑发送指令,然后再根据指令运动。     

这里仅列出Delta机械臂自动运行搬运(Delta.ino)的程序:【其它的程序源码可下载文末资料获取】

5. 扩展

    下图是另一种外观的Delta机械臂(R037c),控制原理完全一样。

运作视频如下:

6. 资料清单

序号

内容
1

【R037】-例程源代码

2【R037】-样机3D文件


文件下载
【整体打包】-【R037】Delta并联机械臂-电磁铁搬运-资料附件.zip
106.62MB下载20次下载
上一页 1 下一页

逆运动学控制

1. 功能描述

      本文示例将实现R037d样机Delta并联机械臂根据逆运动学的两点间运动的功能。通过在Arduino编译器上串口输入目标点坐标(x,y,z),实现从初始点到目标点的两点间运动。

2. 运动学算法

下面介绍一下Delta机械臂的运动学算法,我们先简化机构模型:

      图中tower1,tower2,tower3代表三个丝杠平台,在每个丝杠平台上有一个滑块,滑块通过一根连杆与端点的一个点连接,最终端点的运动状态由3个滑块的移动位置来决定,所以需要算出每个丝杠平台移动与端点运动的关系方程,实际就是确定滑块运动与端点运动的关系。

      首先我们建立一个空间直角坐标系,该直角坐标系以三个丝杠平台在俯视图方向投影的内切圆心为原点,x 轴与tower1和tower3之间的连线平行,y 轴过tower2,其中z=0的平面设置在三个限位开关所在平面。

      建立坐标系之后,我们可以得出3个限位开关的坐标为A=(-msin(60°),mcos(60°), 0);B=(0,m,0);C=(msin(60°),mcos(60°),0);其中m为在xy投影面上正三角形的内切圆心到B点的距离:

确定各限位开关的位置,即确定各丝杠平台上滑块的初始位置,丝杠平台的运动可简化为如下:

      其中N点为滑块初始位置,Q点为端点初始位置,P为Q点在丝杠平台上Z轴的投影;N1,P1,Q1点为丝杠平台运动后的位,T点为某一固定点,假设为Delta机械臂上端点在Z向可以运动的最大值在丝杠平台Z向的投影点。

      逆运动学是根据Q1点的位置确定NN1的距离。在图中有几个已知的值,分别是连杆长度NQ/N1Q1,NT的距离, Q1点的坐标,其中我们输入的量是Q1的坐标(X1,Y1,Z1),这里需要注意的是Z1坐标为负值,为了方便理解,在后面的推导中我们都对Z1取绝对值。我们需要计算的是NN1的距离:

NN1 = NP1- N1P1

      其中Q1的Z坐标与P1的Z坐标一致,所以NP1为已知量在Z1,即这里我们只需要计算出N1P1的值即可:

NN1 = NT - N1P1- Z1

根据勾股定理:

      其中NIQ1为连杆长度,可通过测量得知,所以这里面需要我们计算出Q1P1,该长度我们可以通过两点坐标距离公式得出,借助俯视图投影进行计算:

      为方便计算Q1P1,图中我们将N,N1,P,P1,T点都投影到Z为0的点,则Q1(X1, Y1,0)。根据点坐标公式可得:

综上所述:

      这样我们就求出了NN1(丝杠移动距离)与Q1(执行端运动的终点)坐标的关系。

3. 电子硬件

在这个示例中,我们采用了以下硬件,请大家参考:

主控板

Basra主控板(兼容Arduino Uno)

扩展板

Bigfish2.1扩展板

SH-ST扩展板

传感器

触碰传感器

电机

步进电机

电池

11.1v动力电池

其它

电磁铁


电路连接:

① 硬件连接-电子元件

      各轴步进电机与SH-ST步进电机扩展板的接线顺序如下所示:

② 硬件连接-限位传感器

各个轴的限位传感器(触碰)与Bigfish扩展板的接线如下:X:A3

      Y:A4

      Z:A0

③ 电磁铁连在Bigfish扩展板的9/10针脚。

4. 功能实现

编程环境:Arduino 1.8.2

      功能:实现Delta并联机械臂根据逆运动学的两点间运动。通过在Arduino编译器上串口输入目标点坐标(x,y,z)(注意输入格式中无“()”),实现从初始点到目标点的两点间运动。

下面提供一个参考程序(Delta.ino):

/*------------------------------------------------------------------------------------

  版权说明:Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 机器谱 2024-01-29 https://www.robotway.com/

  ------------------------------*/

/**************************+++++++++++程序使用 Arduino1.8.2 编写,编译错误请使用较新版本+++++++++++*************************/

#include "Arduino.h"

#include <AccelStepper.h>

#include <MultiStepper.h>

#include "Configuration.h"

AccelStepper stepper_x(1, 2, 5);      //tower1

AccelStepper stepper_y(1, 3, 6);      //tower2

AccelStepper stepper_z(1, 4, 7);      //tower3

//AccelStepper stepper_a(1, 12, 13);

MultiStepper steppers;

float delta[NUM_STEPPER];                         

float cartesian[NUM_AXIS] = {0.0, 0.0, 0.0};         //当前坐标

float destination[NUM_AXIS];                         //目标坐标

boolean dataComplete = false;

float down = 65;

float up = 60;

/*********************************************Main******************************************/

void setup() {

  Serial.begin(9600);

  pinMode(EN, OUTPUT);

  steppers.addStepper(stepper_x);

  steppers.addStepper(stepper_y);

  steppers.addStepper(stepper_z);

  stepperSet(3200,1600.0);

  stepperReset();

  delay(1000);

  Get_command(0, 0, down);

  Process_command();

 

  delay(1000);

}

void loop() {

  float r = 25;

  float x1 = 0.0;

  float y1 = 0.0;

  Get_command(25, 0, down);

  Process_command();

  delay(1000);

 

  for(int i=0;i<2;i++){

    for(float i=0.0;i<=360;i+=10){

      x1 = r * cos(i / 180 * 3.141592);

      y1 = r * sin(i / 180 * 3.141592);

     

      Get_command(x1, y1, down);

      Process_command();                                     

    }

  }

  delay(1000);

  for(int j=0;j<2;j++){

    for(float i=360.0;i>=0;i-=10){

      x1 = r * cos(i / 180 * 3.141592);

      y1 = r * sin(i / 180 * 3.141592);

     

      Get_command(x1, y1, down);

      Process_command();                                     

    }

  }

  delay(1000);

  Get_command(0, 0, down);

  Process_command();  

 

  test();

  delay(1000);

 

  stepperReset();

  delay(1000);

 

}

/***************************************Get_commond*******************************************/

void Get_command(float _dx, float _dy, float _dz){  

  destination[0] = _dx;

  destination[1] = _dy;

  destination[2] = _dz;

 

  if(destination[0] == 0 && destination[1] == 0 && destination[2] == 0){

      stepperReset();  

  }

  else

  {

      dataComplete = true;

  }

 

  if(serial_notes){

   Serial.print("destinationPosition: ");

   Serial.print(destination[0]);

   Serial.print(" ");

   Serial.print(destination[1]);

   Serial.print(" ");

   Serial.println(destination[2]);

  }

}

/***************************************Process_command***************************************/

void Process_command(){

  if(dataComplete){

    digitalWrite(EN, LOW);

   

    if(cartesian[0] == destination[0] && cartesian[1] == destination[1] && cartesian[2] == destination[2]){

      return;  

    }

    else

    {

      Line_DDA(destination[0], destination[1], destination[2]);

    }

   

  }

  dataComplete = false;

  digitalWrite(EN, HIGH);

}

/************************************************** DDA ************************************************/

void Line_DDA(float x1, float y1, float z1)

{

  float x0, y0, z0;         // 当前坐标点

  float cx, cy;             // x、y 方向上的增量

 

  x0 = cartesian[0];y0 = cartesian[1];z0 = cartesian[2];

   

  int steps = abs(x1 - x0) > abs(y1 - y0) ? abs(x1 - x0) : abs(y1 - y0);

 

  cx = (float)(x1 - x0) / steps;

  cy = (float)(y1 - y0) / steps;

 

  for(int i = 0; i <= steps; i++)

  {

    cartesian[0] = x0 - cartesian[0];

    cartesian[1] = y0 - cartesian[1];

    cartesian[2] = z1 - cartesian[2];

    calculate_delta(cartesian);

   

    stepperSet(3200.0,1600.0);

    stepperMove(delta[0], delta[1], delta[2]);

 

    cartesian[0] = x0;

    cartesian[1] = y0;

    cartesian[2] = z1;

 

    x0 += cx;

    y0 += cy;

   

    if(serial_notes){

      Serial.print("currentPosition: ");

      for(int i=0;i<3;i++){

         Serial.print(cartesian[i]);

         Serial.print(" ");

      }   

      Serial.println();

      Serial.println("-------------------------------------------------");

    }

  }

}

/***************************************calculateDelta****************************************/

void calculate_delta(float cartesian[3])

{

  if(cartesian[0] == 0 && cartesian[1] == 0 && cartesian[2] == 0){

      delta[0] = 0; delta[1] =0; delta[2] = 0;

  }

  else

  {

      delta[TOWER_1] = sqrt(delta_diagonal_rod_2

                       - sq(delta_tower1_x-cartesian[X_AXIS])

                       - sq(delta_tower1_y-cartesian[Y_AXIS])

                       ) + cartesian[Z_AXIS];

      delta[TOWER_2] = sqrt(delta_diagonal_rod_2

                       - sq(delta_tower2_x-cartesian[X_AXIS])

                       - sq(delta_tower2_y-cartesian[Y_AXIS])

                       ) + cartesian[Z_AXIS];

      delta[TOWER_3] = sqrt(delta_diagonal_rod_2

                       - sq(delta_tower3_x-cartesian[X_AXIS])

                       - sq(delta_tower3_y-cartesian[Y_AXIS])

                       ) + cartesian[Z_AXIS];

     for(int i=0;i<3;i++){

        delta[i] = ((delta[i] - 111.96) * stepsPerRevolution / LEAD);

     }

  }

  if(serial_notes){

      Serial.print("cartesian x="); Serial.print(cartesian[X_AXIS]);

      Serial.print(" y="); Serial.print(cartesian[Y_AXIS]);

      Serial.print(" z="); Serial.println(cartesian[Z_AXIS]);

   

      Serial.print("delta tower1="); Serial.print(delta[TOWER_1]);       

      Serial.print(" tower2="); Serial.print(delta[TOWER_2]);

      Serial.print(" tower3="); Serial.println(delta[TOWER_3]);

  }

}

/****************************************stepperMove******************************************/

void stepperMove(long _x, long _y, long _z){             

    long positions[3];

    positions[0] = _x;                        //steps < 0, 向下运动 ; steps > 0, 向上运动

    positions[1] = _y;

    positions[2] = _z;

    steppers.moveTo(positions);

    steppers.runSpeedToPosition();

    stepper_x.setCurrentPosition(0);

    stepper_y.setCurrentPosition(0);

    stepper_z.setCurrentPosition(0);

}

/****************************************stepperSet******************************************/

void stepperSet(float _v, float _a){

  stepper_x.setMaxSpeed(_v);                  //MaxSpeed: 650

  stepper_x.setAcceleration(_a);  

  stepper_y.setMaxSpeed(_v);

  stepper_y.setAcceleration(_a);

  stepper_z.setMaxSpeed(_v);

  stepper_z.setAcceleration(_a);

}

/****************************************stepperReset******************************************/

void stepperReset(){

  digitalWrite(EN, LOW);

 

  if(cartesian[2] != 0){

    Get_command(0, 0, cartesian[2]);

    Process_command();

    digitalWrite(EN, LOW);

  }

 

  while(digitalRead(SENSOR_TOWER1) && digitalRead(SENSOR_TOWER2) && digitalRead(SENSOR_TOWER3)){

    stepperMove(10, 10, 10);

  }

  stepperSet(3200.0,1600.0);

  stepperMove(-100, 0, 0);

  while(digitalRead(SENSOR_TOWER1)){

    stepperMove(10, 0, 0);

   

  }

  stepperMove(0, -100, 0);

  while(digitalRead(SENSOR_TOWER2)){

    stepperMove(0, 10, 0);

  }

  stepperMove(0, 0, -100);

  while(digitalRead(SENSOR_TOWER3)){

    stepperMove(0, 0, 10);

  }

  for(int i=0;i<3;i++){

     cartesian[i] = 0.0;

  }  

  if(serial_notes) Serial.println("resetComplete!");

  digitalWrite(EN, HIGH);

}

/*************************************************** electromagnet *************************************************************/

void putUp(){

   digitalWrite(9, HIGH);

   digitalWrite(10, LOW);

}

void putDown(){

   digitalWrite(9,LOW);

   digitalWrite(10, LOW);

}

/**************************************************   test    ******************************************************************/

void test(){

    Get_command(0, 0, down);

    Process_command();

    delay(500);

    putUp();

 

    Get_command(0, 0, up);

    Process_command();  

    Get_command(-25, 0, up);

    Process_command();

    Get_command(-25, 0, down);

    Process_command();  

    putDown();

    delay(500);

    putDown();

    putUp();

    Get_command(-25, 0, up);

    Process_command();   

    Get_command(0, -25, up);

    Process_command();   

    Get_command(0, -25, down);

    Process_command();

    putDown();  

    delay(500);

    putDown();

    putUp();

    Get_command(0, -25, up);

    Process_command();   

    Get_command(25, 0, up);

    Process_command();   

    Get_command(25, 0, down);

    Process_command();  

    putDown();

    delay(500);

    putUp();

    Get_command(25, 0, up);

    Process_command();   

    Get_command(0, 25, up);

    Process_command();   

    Get_command(0, 25, down);

    Process_command();  

    putDown();

    delay(500);

    putUp();

    Get_command(0, 25, up);

    Process_command();   

    Get_command(-25, 0, up);

    Process_command();   

    Get_command(-25, 0, down);

    Process_command();   

    putDown();

    delay(500);

    putUp();

    Get_command(-25, 0, up);

    Process_command();   

    Get_command(0, 0, up);

    Process_command();   

    Get_command(0, 0, down);

    Process_command();   

    delay(500);

    putDown();

}


5. 资料清单

序号

内容
1

程序源代码

2
样机3D文件


文件下载
样机方案-【R037】Delta并联机械臂-逆运动学控制-资料附件.rar
3.16MB下载
上一页 1 下一页
© 2022 机器时代(北京)科技有限公司  版权所有
机器谱——机器人共享方案网站
学习  开发  设计  应用