基于marlin固件的SCARA机器人
博客专区 > YANG_G 的博客 > 博客详情
基于marlin固件的SCARA机器人
YANG_G 发表于6个月前
基于marlin固件的SCARA机器人
  • 发表于 6个月前
  • 阅读 140
  • 收藏 0
  • 点赞 0
  • 评论 0

 

 

>>所有文件在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 读取出厂设置

 

共有 人打赏支持
粉丝 2
博文 4
码字总数 10803
×
YANG_G
如果觉得我的文章对您有用,请随意打赏。您的支持将鼓励我继续创作!
* 金额(元)
¥1 ¥5 ¥10 ¥20 其他金额
打赏人
留言
* 支付类型
微信扫码支付
打赏金额:
已支付成功
打赏金额: