文档章节

基于marlin固件的SCARA机器人

YANG_G
 YANG_G
发布于 2017/06/04 12:24
字数 3035
阅读 1289
收藏 1

 

 

>>所有文件在thingiverse

 

参考:

         平行并联臂结构morgan(上图),串联臂http://www.thingiverse.com/thing:1241491(下图),Delta3D打印机代码解读Marlin精读DARM

   

定义相对坐标:

 

    

     定义x轴(主臂)的相对坐标为主臂与x负半轴轴夹角的角度(在上为正,在下为负),y轴(副臂)的相对坐标为副臂和主臂延长线夹角的角度。(图中:姿态1的相对坐标[X41,Y42],姿态2的相对坐标为[X0,Y0],姿态3的相对坐标为[X-55,Y152])。X轴最小坐标值时触发限位,Y轴最大坐标值时触发限位。

 

正解方程(相对坐标转化成绝对坐标):

void calculate_SCARA_forward_Transform(float f_scara[3])

{


//先假定中心转轴为绝对坐标原点(图中红色坐标)

//图中绿色坐标为真正的坐标


  float x_sin, x_cos, y_sin, y_cos;//如图

//f_scara[X_AXIS]为X轴相对坐标,SCARA_RAD2DEG为弧度转角度转化因子等于pi/180(相对坐标是角度制,三角函数是弧度制,需要转化)

//Linkage_1、Linkage_2分别为主、副臂长度

//delta[X_AXIS]为绝对坐标的X轴值

//SCARA_offset[X_AXIS] 、SCARA_offset[Y_AXIS] 是假定的坐标原点相对于真实坐标的坐标(真实坐标原点的偏移量)

//简单的三角函数关系就能得出

    x_sin = sin(f_scara[X_AXIS]/SCARA_RAD2DEG) * Linkage_1;  

    x_cos = cos(f_scara[X_AXIS]/SCARA_RAD2DEG) * Linkage_1;

    y_sin = sin(f_scara[X_AXIS]/SCARA_RAD2DEG + f_scara[Y_AXIS]/SCARA_RAD2DEG) * Linkage_2;

    y_cos = cos(f_scara[X_AXIS]/SCARA_RAD2DEG + f_scara[Y_AXIS]/SCARA_RAD2DEG) * Linkage_2;



    delta[X_AXIS] = -x_cos - y_cos + SCARA_offset[X_AXIS] ;//因为相对坐标从x轴负半轴开始,实际为负值,所以取反。加上偏移量变换成真正的坐标。

    delta[Y_AXIS] = x_sin + y_sin + SCARA_offset[Y_AXIS] ;  


}  

 

 

反解方程(绝对坐标转化为相对坐标):

void calculate_delta(float cartesian[3]){


  float SCARA_pos[2];

  static float SCARA_C2, SCARA_S2, SCARA_K1, SCARA_K2, SCARA_theta, SCARA_psi; 


  SCARA_pos[X_AXIS] = -cartesian[X_AXIS] * axis_scaling[X_AXIS] + SCARA_offset[X_AXIS] ;//把实际坐标(偏移后的坐标)转化成假定的坐标,X轴取反。  

  SCARA_pos[Y_AXIS] = cartesian[Y_AXIS] * axis_scaling[Y_AXIS] - SCARA_offset[Y_AXIS] ;  


  #if (Linkage_1 == Linkage_2)

    SCARA_C2 = ( ( sq(SCARA_pos[X_AXIS]) + sq(SCARA_pos[Y_AXIS]) ) / (2 * (float)L1_2) ) - 1;

  #else

    SCARA_C2 =   ( sq(SCARA_pos[X_AXIS]) + sq(SCARA_pos[Y_AXIS]) - (float)L1_2 - (float)L2_2 ) / 45000; 

  #endif//三角形ABC中,用余弦定理计算角A的值,角A和Y轴电机的角度(相对坐标)互补(余弦值取反)可以得到Y轴角度的余弦值



  SCARA_S2 = sqrt( 1 - sq(SCARA_C2) );//计算Y轴角度的正弦值

  SCARA_K1 = Linkage_1 + Linkage_2 * SCARA_C2;//直角三角形OBC中,计算OC的长

  SCARA_K2 = Linkage_2 * SCARA_S2;//计算OB的长


  SCARA_theta = ( atan2(SCARA_pos[X_AXIS],SCARA_pos[Y_AXIS])-atan2(SCARA_K1, SCARA_K2) )*-1 //计算X轴的相对坐标(弧度制),图中角1-角2,即(90-角3)-(90-角OBC),即(角3-角OBC)*-1

  SCARA_psi   =   atan2(SCARA_S2,SCARA_C2);//计算Y轴的相对坐标


  delta[X_AXIS] = SCARA_theta * SCARA_RAD2DEG; //弧度转角度     

  delta[Y_AXIS] = SCARA_psi * SCARA_RAD2DEG;   

  delta[Z_AXIS] = cartesian[Z_AXIS];

 

第四轴算法:

第四轴为旋转轴,规定两手指的连线(图中定义为手爪的X轴)与绝对坐标系X轴的夹角(0-180)为第四轴的绝对坐标,图中A_AXIS,相对坐标(即舵机的角度)为手爪Y轴(垂直于手爪X轴)与主臂的夹角,图中A_AXIS_R ,简单 几何关系得:

A_AXIS_R=270-delta[X_AXIS]-delta[Y_AXIS]-A_AXIS

在反解函数中加入:

servos[0].write(0.9*(270-delta[X_AXIS]-delta[Y_AXIS]-A_AXIS));

注:变量A_AXIS的值由G5代码获取,0.9是比例系数,因为用的舵机角度不准确,0-180实际却是0-200。

 

Marlin基础参数修改:

    Configuration.h文件中

//========================= SCARA配置 ==================================

#define SCARA  

#define scara_segments_per_second 400 


#define Linkage_1 125 // 主臂长度

#define Linkage_2 125 // 副臂长度


#define SCARA_offset_x  60 // 绝对坐标相对假定坐标的偏移量

#define SCARA_offset_y -100

#define SCARA_offset_z  0

#define SCARA_RAD2DEG 57.2957795 //弧度转角度因子 


#define L1_2 sq(Linkage_1) //主臂的平方值

#define L2_2 sq(Linkage_2) 


//========================= 基础设置 ==================================

#define SERIAL_PORT 0//端口,默认0


#define BAUDRATE 115200//波特率


#ifndef MOTHERBOARD

  #define MOTHERBOARD BOARD_RAMPS_13_EFB//主板类型,在board.h中可以找到对应主板的名称

#endif


#define EXTRUDERS 1//挤出机数量


#define POWER_SUPPLY 1//电源类型,一般为1


//============================= 温度设置 ============================

// -2 is thermocouple with MAX6675 (only for sensor 0)

// -1 is thermocouple with AD595

// 0 is not used

// 1 is 100k thermistor - best choice for EPCOS 100k (4.7k pullup)

// 2 is 200k thermistor - ATC Semitec 204GT-2 (4.7k pullup)

// 3 is Mendel-parts thermistor (4.7k pullup)

// 4 is 10k thermistor !! do not use it for a hotend. It gives bad resolution at high temp. !!

// 5 is 100K thermistor - ATC Semitec 104GT-2 (Used in ParCan & J-Head) (4.7k pullup)

// 6 is 100k EPCOS - Not as accurate as table 1 (created using a fluke thermocouple) (4.7k pullup)

// 7 is 100k Honeywell thermistor 135-104LAG-J01 (4.7k pullup)

// 71 is 100k Honeywell thermistor 135-104LAF-J01 (4.7k pullup)

// 8 is 100k 0603 SMD Vishay NTCS0603E3104FXT (4.7k pullup)

// 9 is 100k GE Sensing AL03006-58.2K-97-G1 (4.7k pullup)

// 10 is 100k RS thermistor 198-961 (4.7k pullup)

// 11 is 100k beta 3950 1% thermistor (4.7k pullup)

// 12 is 100k 0603 SMD Vishay NTCS0603E3104FXT (4.7k pullup) (calibrated for Makibox hot bed)

// 13 is 100k Hisens 3950  1% up to 300°C for hotend "Simple ONE " & "Hotend "All In ONE" 

// 20 is the PT100 circuit found in the Ultimainboard V2.x

// 60 is 100k Maker's Tool Works Kapton Bed Thermistor beta=3950

//

//    1k ohm pullup tables - This is not normal, you would have to have changed out your 4.7k for 1k

//                          (but gives greater accuracy and more stable PID)

// 51 is 100k thermistor - EPCOS (1k pullup)

// 52 is 200k thermistor - ATC Semitec 204GT-2 (1k pullup)

// 55 is 100k thermistor - ATC Semitec 104GT-2 (Used in ParCan & J-Head) (1k pullup)

//

// 1047 is Pt1000 with 4k7 pullup

// 1010 is Pt1000 with 1k pullup (non standard)

// 147 is Pt100 with 4k7 pullup

// 110 is Pt100 with 1k pullup (non standard)

// 998 and 999 are Dummy Tables. They will ALWAYS read 25°C or the temperature defined below. 

//     Use it for Testing or Development purposes. NEVER for production machine.

//     #define DUMMY_THERMISTOR_998_VALUE 25

//     #define DUMMY_THERMISTOR_999_VALUE 100


#define TEMP_SENSOR_0 1 //各个挤出头的温度传感器类型(对应上面的注释,常见的为1),0是不使用

#define TEMP_SENSOR_1 0

#define TEMP_SENSOR_2 0

#define TEMP_SENSOR_3 0  

#define TEMP_SENSOR_BED 0 //热床


#define HEATER_0_MINTEMP 5 //最低温度

#define HEATER_1_MINTEMP 5

#define HEATER_2_MINTEMP 5

#define HEATER_3_MINTEMP 5

#define BED_MINTEMP 5


#define HEATER_0_MAXTEMP 275 //最高温度

#define HEATER_1_MAXTEMP 275

#define HEATER_2_MAXTEMP 275

#define HEATER_3_MAXTEMP 275

#define BED_MAXTEMP 150

//注:如果定义了挤出头的热敏电阻但是使用的时候没插,打印机就会报错,但不定义可能会编译不成功。如果不想使用挤出头可以把挤出头温度传感器类型设为1,最低温度设为0



//============================ 限位设置 ==========================


const bool X_MIN_ENDSTOP_INVERTING = true; 

const bool Y_MIN_ENDSTOP_INVERTING = true;  

const bool Z_MIN_ENDSTOP_INVERTING = true; 

const bool X_MAX_ENDSTOP_INVERTING = true; 

const bool Y_MAX_ENDSTOP_INVERTING = true; 

const bool Z_MAX_ENDSTOP_INVERTING = true; // 坐标最大值和坐标最小值的限位开关的常开常闭设置(true为常开)

//#define DISABLE_MAX_ENDSTOPS //取消最大坐标值限位  

//#define DISABLE_MIN_ENDSTOPS //取消最小坐标值限位


#define DISABLE_X false

#define DISABLE_Y false

#define DISABLE_Z false //在电机不工作时解锁电机

#define DISABLE_E false //对于所有挤出机有效

#define DISABLE_INACTIVE_EXTRUDER true //只解锁不工作的电机


#define INVERT_X_DIR false    

#define INVERT_Y_DIR false    

#define INVERT_Z_DIR true     

#define INVERT_E0_DIR false   

#define INVERT_E1_DIR false   

#define INVERT_E2_DIR false   

#define INVERT_E3_DIR false  //电机转动方向


#define X_HOME_DIR -1

#define Y_HOME_DIR 1

#define Z_HOME_DIR 1//限位配置,1为最大坐标限位


#define X_MAX_POS 250

#define X_MIN_POS 0

#define Y_MAX_POS 250

#define Y_MIN_POS 0

#define Z_MAX_POS MANUAL_Z_HOME_POS

#define Z_MIN_POS 0                    //喷头各轴的行程设置


#define MANUAL_X_HOME_POS  -32

#define MANUAL_Y_HOME_POS 151

#define MANUAL_Z_HOME_POS 190 //触发限位时各轴的坐标或角度,这里是初始值,后可用G代码修改



//============================ 电机设置 ==========================

#define HOMING_FEEDRATE {20*60, 20*60, 20*60, 0}  //home时的速率



#define DEFAULT_AXIS_STEPS_PER_UNIT   {53.333333,44.444444,100,300}  移动单位长度或角度需要的脉冲数(对应X,Y,Z,E)

#define DEFAULT_MAX_FEEDRATE          {200, 200, 200, 25}    // 最大速度

#define DEFAULT_MAX_ACCELERATION      {1000,1000,1000,1000}    // 最大加速度


#define DEFAULT_ACCELERATION          500    // 默认加速度

#define DEFAULT_RETRACT_ACCELERATION  2000     //反转加速度


//============================ 舵机设置 ==========================


#define NUM_SERVOS 3 // 舵机个数

//注:原固件中的舵机是给自动调平用的,如果要用g代码控制舵机需要自己添加

 

 

Marlin固件算法修改

     Marlin_main.cpp文件中

          1.找到calculate_SCARA_forward_Transform(正解)和calculate_delta(反解)函数,修改成上面的算法。

          2.找到代码的这个部分,在#ifdef SCARA中加入ManualHomePos[3]数组变量,这个数组储存的是各个轴触发限位时的相对坐标(也就是初始相对坐标的值)

#ifdef DELTA

  float delta[3] = { 0, 0, 0 };

  #define SIN_60 0.8660254037844386

  #define COS_60 0.5

  // these are the default values, can be overriden with M665

  float delta_radius = DELTA_RADIUS;

  float delta_tower1_x = -SIN_60 * delta_radius; // front left tower

  float delta_tower1_y = -COS_60 * delta_radius;       

  float delta_tower2_x =  SIN_60 * delta_radius; // front right tower

  float delta_tower2_y = -COS_60 * delta_radius;       

  float delta_tower3_x = 0;                      // back middle tower

  float delta_tower3_y = delta_radius;

  float delta_diagonal_rod = DELTA_DIAGONAL_ROD;

  float delta_diagonal_rod_2 = sq(delta_diagonal_rod);

  float delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND;

#endif


#ifdef SCARA

  float axis_scaling[3] = { 1, 1, 1 }; 

  float ManualHomePos[3]={MANUAL_X_HOME_POS,MANUAL_Y_HOME_POS,MANUAL_Y_HOME_POS};

  float SCARA_offset[3]={SCARA_offset_x,SCARA_offset_y,SCARA_offset_z};

#endif               

        

          3.找到写G28的那一段代码 ,添加如下代码,即把触发限位时各轴的相对坐标定义成实际的限位触发角度。

if(code_seen('G'))

  {

    switch((int)code_value())

    {

          ...

     case 28:

               ...

          #ifdef SCARA


             calculate_SCARA_forward_Transform(ManualHomePos);

             for(int8_t i=0; i < 3; i++)

             {

                 destination[i]=current_position[i]=delta[i];

                 delta[i]=ManualHomePos[i];

             }   

             plan_set_position(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], current_position[E_AXIS]);

          #endif 

               ...

     break;

     }

}

 

 

Marlin固件中添加写入EEPROM的参数:

在ConfigurationStore.cpp文件中的Config_StoreSettings()、Config_RetrieveSettings()函数下添加以下 代码:(两个函数分别是写入和恢复参数设置)

#ifdef SCARA

  EEPROM_WRITE_VAR(i,SCARA_offset);//原点偏移量

  EEPROM_WRITE_VAR(i,ManualHomePos);//触发限位角度

  #endif


     在Config_ResetDefault()下添加:(此函数为恢复默认参数)


#ifdef SCARA

  SCARA_offset[X_AXIS] = SCARA_offset_x; 

  SCARA_offset[Y_AXIS] = SCARA_offset_y;

  SCARA_offset[Z_AXIS] = SCARA_offset_z;

  ManualHomePos[X_AXIS] = MANUAL_X_HOME_POS;

  ManualHomePos[Y_AXIS] = MANUAL_Y_HOME_POS;

  ManualHomePos[Z_AXIS] = MANUAL_Z_HOME_POS;

  #endif

 在Marlin.h中定义

#ifdef SCARA

extern float axis_scaling[3]; 

extern float SCARA_offset[3];

extern float ManualHomePos[3]; 

#endif

 

 

Marlin固件中添加自定义的G、M代码:

   

     1.添加修改限位角度的M代码(M360)。找到写M代码的部分加入以下代码。

case 360:

        for(int8_t i=0; i < 3; i++)

        {

            if(code_seen(axis_codes[i])) 

            {

                ManualHomePos[i] = code_value();//把触发限位时的相对坐标设置成输入的值

            }

        }

        break;

  

   2.添加设置偏移量的M代码(M361)


case 361:

        for(int8_t i=0; i < 3; i++)

        {

            if(code_seen(axis_codes[i])) 

            {

                SCARA_offset[i] = code_value();

            }

        }

        break;

 

   3.添加控制舵机的G代码。首先要在 Configuration.h文件中定义舵机

case 5: // G5 servo contrl

      if (code_seen('A')) 

        servos[0].write(code_value());//舵机A转动所输入的角度

      if (code_seen('B')) 

        servos[1].write(code_value());

      if (code_seen('C')) 

        servos[2].write(code_value());

    break;

 

   4.添加角度模式 。Marlin_main.cpp文件中添加角度模式的开关变量。

#ifdef SCARA

  float axis_scaling[3] = { 1, 1, 1 }; 

  float ManualHomePos[3]={MANUAL_X_HOME_POS,MANUAL_Y_HOME_POS,MANUAL_Y_HOME_POS}; 

  float SCARA_offset[3]={SCARA_offset_x,SCARA_offset_y,SCARA_offset_z};

  bool angle_mode=false;

#endif   

 

   在G代码的case中添加case 95 。即使用G95代码切换角度模式和坐标模式。

case 95: 

      angle_mode=!angle_mode;

      break;

 

 反解函数calculate_delta修改为如下,即角度模式为假进行坐标换算,为真不换算。


if(angle_mode==false)

  {

    delta[X_AXIS] = SCARA_theta * SCARA_RAD2DEG; // Multiply by 180/Pi - Theta is support arm angle (shoulder)

    delta[Y_AXIS] = SCARA_psi * SCARA_RAD2DEG;   // - Psi sub arm angle (elbow)

    delta[Z_AXIS] = cartesian[Z_AXIS]-SCARA_offset[Z_AXIS];

  }

  else

  {

      delta[X_AXIS] = cartesian[X_AXIS];

      delta[Y_AXIS] = cartesian[Y_AXIS];

      delta[Z_AXIS] = cartesian[Z_AXIS];

  }

   

  5.添加当前坐标设定的G代码。原有固件在G92(case 92)代码中只能设置E轴的当前坐标。所以添加如下代码可以把机械手任意的位置设置成想要的坐标。

原理即修改坐标偏移量SCARA_offset[i]但不保存到固件

#ifdef SCARA

        if (i == X_AXIS || i == Y_AXIS||i == Z_AXIS) {


                    SCARA_offset[i]=SCARA_offset[i]-current_position[i]+code_value();//新的坐标偏移量=旧的坐标偏移量-当前坐标+想要设定的坐标

                    current_position[i] = code_value();  //设定当前坐标为输入的值

        }

#endif

 

 

RAMPS主板详解:

如果arduino2560用的是ams1117的稳压芯片,建议把二极管D1去掉,两个板子单独供电。

常用G、M代码:

M360 触发限位角度设置 

M361 原点偏移设置

G5 舵机角度控制

G0 快速点定位

G1 直线插补

G4 延迟

G28 回归所有轴

G92 当前坐标设定

G95 切换绝对/相对坐标

 

M0 停止

M3 主轴正转

M4 主轴反转

M5 主轴停止

M17 启动步进电机

M20 读取SD卡

M21 初始化SD卡

M22 弹出SD卡

M23 选择SD卡的文件

M24 开始SD卡的打印

M25 暂停SD卡打印

M41 循环

M82 挤出机绝对坐标

M83 挤出机相对坐标

M84 解锁步进电机

M92 设置步进电机脉冲数

M101 正转挤出机

M102 反转挤出机

M104 设置挤出机温度

M105 获取温度

M106 打开风扇

M107 关闭风扇

M112 紧急停止

M114 获取当前坐标和角度

M119 获取限位开关状态

M201 设置最大打印速度

M202 设置最大移动速度

M203 设置电机最大速度

M204 设置默认加速度

M500 保存设置到EEPROM

M501 读取EEPROM中的配置

M502 恢复出厂设置

M503 读取出厂设置

 

© 著作权归作者所有

共有 人打赏支持
下一篇: 三轮全向底盘
YANG_G
粉丝 4
博文 4
码字总数 10801
作品 0
福州
私信 提问
【课程】众为兴SCARA机器人培训

《众为兴SCARA机器人培训》 已在睿慕课全面上线 课程链接 www.aiimooc.com 前往【睿慕课】官网 or 公众号 都可以进行观看 / 讲师是谁? / 李明明 众为兴机器人系统 高级讲师 1. 2012年公派留...

睿慕课
2017/12/04
0
0
Marlin 中文版本 LCD

Marlin是3d打印机中最常用、功能很强大的固件。主要用于3d打印机及CNC控制。并且支持LCD显示等。但一直以来没有中文LCD的版本。MakerLab开发了中文版本,让新手可以比较容易的使用Marlin+LC...

AndySze
2014/05/13
4.2K
0
机器人的野蛮生长:从机器换人到智慧制造

  智能制造已经成为全球制造业新趋势,而机器人作为实现智能制造的关键技术,拥有广阔的市场前景,深受资本市场的青睐。如今,机器人被广泛应用于汽车工业、电子制造、金属加工、化学和塑料...

中国机器人
01/11
0
0
长大小申/MakerD3Printer

#MakerD3Printer 中文本土化的3D打印机控制系统 编不下去, 好吧!我坦白,就是reprtier 的固件,我中文化了! 最新版Repetier-Firmware 0.9.2.3. 支持Arduino due 32位的主板。 还在用8位a...

长大小申
2015/05/06
0
0
Matlab Robotics工具包简介

Robotics工具包是由Peter Croke团队开发的一款基于Matlab平台的机器人运算、仿真的强大工具。2017年6月工具包的v10发布,新版的工具包功能更为强大,且支持更为简便的.mltbx格式安装。工具包...

卓求
2017/10/06
0
0

没有更多内容

加载失败,请刷新页面

加载更多

【HAVENT原创】NodeJS 两个模块进行 RSA 加密解密(匹配Java RSA)

业务逻辑需要使用 NodeJS 进行公钥加密传输给 Java 后端进行私钥解密,但是默认 NodeJS 使用的 RSA padding 模式与 Java 的不一致,所以需要配置。 不啰嗦,上代码,分别用 crypto 和 node-r...

HAVENT
8分钟前
0
0
Notification通知栏

目录介绍 1.Notification简单概述 2.Notification通知用途 3.Notification的基本操作 3.1 Notification创建必要的属性 3.2 Notification简单创建步骤 3.3 关于setSmallIcon()与setLargeIcon(...

潇湘剑雨
17分钟前
0
0
云环境所面临的安全威胁

为了云计算的健康发展和等级保护工作在新形势下顺利推进,就不能忽视对云计算面临的各类安全威胁的研究和分析,并制定和建立相应的等级保护政策、技术体系,应对即将到来的云浪潮。在云环境中...

linuxCool
17分钟前
0
0
server unexpectedly closed network connection

2011-10-04 01:17:05 今天用PUTTY或winscp远程连接服务器时, 均提示如下错误server unexpectedly closed network connection 服务器防火墙已经关闭, sshd.conf中的USEDNS也设为了no, 重启ssh...

爱吃窝窝头
31分钟前
0
0
配合OAuth2进行单设备登录拦截

要进行单设备登录,在其他地点登录后,本地的其他操作会被拦截返回登录界面。 原理就在于要在登录时在redis中存储Session,进行操作时要进行Session的比对。 具体实现,假设我们的OAuth 2的登...

算法之名
36分钟前
12
0

没有更多内容

加载失败,请刷新页面

加载更多

返回顶部
顶部