搜索
bottom↓
回复: 18

用stc15w和旧光驱移植arduino写字机GRBL之六:motion_control

[复制链接]

出0入0汤圆

发表于 2018-12-1 12:27:41 | 显示全部楼层 |阅读模式
本帖最后由 XTXB 于 2018-12-1 12:25 编辑

stc15w4k32s4和旧光驱移植arduino写字机GRBL连载:
用stc15w4k32s4和旧光驱移植arduino写字机GRBL之一:机架搭建
https://www.amobbs.com/thread-5701202-1-1.html

用stc15w4k32s4和旧光驱移植arduino写字机GRBL之二:PCB制作
https://www.amobbs.com/thread-5701573-1-1.html

用stc15w4k32s4和旧光驱移植arduino写字机GRBL之三:Bresenham算法
https://www.amobbs.com/thread-5701994-1-1.html

用stc15w4k32s4和旧光驱移植arduino写字机GRBL之四:FIFO算法
https://www.amobbs.com/thread-5702730-1-1.html

用stc15w4k32s4和旧光驱移植arduino写字机GRBL之五:G_Code
https://www.amobbs.com/thread-5702784-1-1.html

用stc15w4k32s4和旧光驱移植arduino写字机GRBL之六:运动控制motion_control

GRBL的运动控制算法有两个主要函数mc_line( ),mc_arc( ):
1,mc_line( )将线段直接送入规划器Plane_buffer_line( ) ,最后在stpper进行精插补Bresenham算法。
2,mc_arc( )将弧先粗细分(就是拆分成多条逼近弧的小线段,个人认为精细分属于Bresenham算法),然后按照第1步进行后续。
下面是将弧拆分成多条小线段的粗细分算法:

已知:起点坐标P *position,终点坐标W *target,旋转方向isclockwise,圆心对起点的相对坐标*offset,圆半径radius
求:各个粗细分点坐标T  *arc_target

1,先计算弧对应的圆心角∠POW(angular_travel)。
2,计算弧长(millimeters_of_travel)。
3,GEBL预设粗细分的最小弧长settings.mm_per_arc_segment=0.1mm, 则总粗细分数量segments=弧长/0.1mm。
4,计算每条小弧对应的圆心角(theta_per_segment)=φ。
5,计算每增加一个φ小弧终点的坐标(arc_target),为减小计算量,用泰勒级数近似计算三角函数。
6,为避免累积误差,每隔25个φ,用三角函数计算一次当前点的坐标。
7,调用mc_line( )函数。

以上1,2,3比较简单,GRBL代码如下:
float angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
//计算圆心角
float millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel));//计算小线段的弧长
uint16_t segments = floor(millimeters_of_travel/settings.mm_per_arc_segment);//小线段的数量
float theta_per_segment = angular_travel/segments;//每条小线段对应的圆心角φ 


下面重点探讨4,5,6 的算法:

在上图中X'OY'为绝对坐标系,在以圆心为坐标原点的坐标系XOY中,设起点坐标P(Px,Py),弧半径r,
逆时针方向转φ角度到T,求细分点坐标T(Tx,Ty):


本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x

阿莫论坛20周年了!感谢大家的支持与爱护!!

阿莫论坛才是最爱国的,关心国家的经济、社会的发展、担心国家被别国牵连卷入战争、知道珍惜来之不易的和平发展,知道师夷之长,关注世界的先进文化与技术,也探讨中国文化的博大精深,也懂得警惕民粹主义的祸国殃民等等等等,无不是爱国忧民的表现。(坛友:tianxian)

出0入0汤圆

发表于 2018-12-1 13:09:26 | 显示全部楼层
楼主,请帮忙看一下
eeprom.c文件中,下面2个函数是否有错误:(红字部分)

void memcpy_to_eeprom_with_checksum(unsigned int destination, char *source, unsigned int size) {
  unsigned char checksum = 0;
  for(; size > 0; size--) {
    checksum = (checksum << 1) || (checksum >> 7);//  || 应为 |
    checksum += *source;
    eeprom_put_char(destination++, *(source++));
  }
  eeprom_put_char(destination, checksum);
}

int memcpy_from_eeprom_with_checksum(char *destination, unsigned int source, unsigned int size) {
  unsigned char data, checksum = 0;
  for(; size > 0; size--) {
    data = eeprom_get_char(source++);
    checksum = (checksum << 1) || (checksum >> 7); ///  || 应为 |

    checksum += data;   
    *(destination++) = data;
  }
  return(checksum == eeprom_get_char(source));
}

出0入0汤圆

发表于 2018-12-1 13:10:25 | 显示全部楼层
checksum = (checksum << 1) || (checksum >> 7);//  || 应为 |

出0入0汤圆

 楼主| 发表于 2018-12-1 14:10:45 | 显示全部楼层
本帖最后由 XTXB 于 2018-12-1 17:23 编辑
su33691 发表于 2018-12-1 13:10
checksum = (checksum > 7);//  || 应为 |


还没啃这部分代码,不清楚函数具体作用,但就代码看,左移右移的折腾,似乎要进行位判断,所以用逻辑值运算符||,
但后面又有数值的比较checksum == eeprom_get_char(source) ,数值运算符|又似乎会更合理一些。
具体怎样,要弄懂函数功能,才可能知道原由,我查到调用这个函数的函数如下:
uint8_t settings_read_startup_line(uint8_t n, char *line)
{
  uint16_t addr = n*(LINE_BUFFER_SIZE+1)+EEPROM_ADDR_STARTUP_BLOCK;
  if (!(memcpy_from_eeprom_with_checksum((char*)line, addr, LINE_BUFFER_SIZE))) {
    // Reset line with default value
    line[0] = 0;
    settings_store_startup_line(n, line);
    return(false);
  } else {
    return(true);
  }
}

出0入0汤圆

 楼主| 发表于 2018-12-1 16:09:44 | 显示全部楼层
本帖最后由 XTXB 于 2018-12-1 17:16 编辑

int memcpy_from_eeprom_with_checksum(char *destination, unsigned int source, unsigned int size)
{
        .............
        return(checksum == eeprom_get_char(source));
}
还有,这里返回的就是个逻辑真值,函数干嘛是int型,uint8_t不是可以省一点么?

出0入0汤圆

发表于 2018-12-1 17:20:27 | 显示全部楼层
请问一下楼主,这个假设
sinT=sina=a  
cosT=cosa=1-a²/2
这个假设不成立啊
这个假设没办法推倒除 sin²T+cos²T = 1
请楼主指正

出0入0汤圆

 楼主| 发表于 2018-12-1 17:26:58 | 显示全部楼层
本帖最后由 XTXB 于 2018-12-1 18:26 编辑
amxx 发表于 2018-12-1 17:20
请问一下楼主,这个假设
sinT=sina=a  
cosT=cosa=1-a²/2


sinT=sina,cosT=cosa是假设,
sina=a-...  ,cosa=1-a²/2+....... ,是泰勒级数
a是弧度
sin1°=0.01745
1°=(π/180)弧度≈0.01745329弧度
所以sina≈a

出0入0汤圆

发表于 2018-12-3 08:05:48 | 显示全部楼层
XTXB 发表于 2018-12-1 17:26
sinT=sina,cosT=cosa是假设,
sina=a-...  ,cosa=1-a²/2+....... ,是泰勒级数
a是弧度

谢谢楼主!补习一下。

出0入0汤圆

 楼主| 发表于 2018-12-3 16:21:11 | 显示全部楼层
更正:
float millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel));//计算小线段的弧长应为计算弧长

出0入0汤圆

发表于 2018-12-5 23:53:42 | 显示全部楼层
XTXB 发表于 2018-12-1 16:09
int memcpy_from_eeprom_with_checksum(char *destination, unsigned int source, unsigned int size)
{
        . ...

这个是临时变量没所谓,如果是32bitMCU用uint32_t会快点。

出0入0汤圆

发表于 2019-2-22 15:36:03 | 显示全部楼层
本帖最后由 王涛 于 2019-2-22 15:37 编辑

关于圆弧插补这里用到泰勒公式的的目的是:我的理解是考虑到AVR单片机做SIN COS很耗时(其实也是泰勒级数的展开)所以这里就只用到了二阶三阶展开做近似计算,然后误差累计后再修正。如果是带有DSP的ARM这里就直接交给DSP处理精度又高速度又快。

出0入0汤圆

发表于 2019-3-4 23:20:36 | 显示全部楼层
楼主,这里有点不太明白,
以上1,2,3比较简单,GRBL代码如下:
float angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
中的
r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1
这句是指哪条线段或是哪两点??

出0入0汤圆

 楼主| 发表于 2019-3-5 09:13:47 | 显示全部楼层
本帖最后由 XTXB 于 2019-3-5 11:50 编辑
hkjabcd 发表于 2019-3-4 23:20
楼主,这里有点不太明白,
以上1,2,3比较简单,GRBL代码如下:
float angular_travel = atan2(r_axis0*rt_a ...

应该是在坐标系XOY中的P和W,要用到正切公式tanφ=K1-K2/(1+K1*K2),φ是夹角,K1和K2是OP和OW的斜率,这是以前做的笔记,你看看:


//起点坐标 position,终点坐标target  圆心相对于起始点的偏移向量offset,弧半径radius,旋转方向 isclockwise
void mc_arc(float *position, float *target, float *offset, uint8_t axis_0, uint8_t axis_1,
  uint8_t axis_linear, float feed_rate, uint8_t invert_feed_rate, float radius, uint8_t isclockwise)
{      
  float center_axis0 = position[axis_0] + offset[axis_0];//圆心坐标
  float center_axis1 = position[axis_1] + offset[axis_1];
  float linear_travel = target[axis_linear] - position[axis_linear];//起点到终点的轴的进给速率
  float r_axis0 = -offset[axis_0];  // Radius vector from center to current location
  float r_axis1 = -offset[axis_1];//圆心指向圆弧起点P的向量坐标
  float rt_axis0 = target[axis_0] - center_axis0;//圆心指向圆弧终点W的向量坐标
  float rt_axis1 = target[axis_1] - center_axis1;
  //
  
  // CCW angle between position and target from circle center. Only one atan2() trig computation required.
  float angular_travel = atan2(r_axis0*rt_axis1-r_axis1*rt_axis0, r_axis0*rt_axis0+r_axis1*rt_axis1);
  //计算圆心角

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x

出0入0汤圆

发表于 2019-3-6 19:11:30 | 显示全部楼层
XTXB 发表于 2019-3-5 09:13
应该是在坐标系XOY中的P和W,要用到正切公式tanφ=K1-K2/(1+K1*K2),φ是夹角,K1和K2是OP和OW的斜率,这 ...

谢谢!有时间再仔细看下

出0入0汤圆

发表于 2019-3-22 10:28:02 | 显示全部楼层
XTXB 发表于 2018-12-3 16:21
更正:
float millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel));//计算小线段 ...

hypot(angular_travel*radius, fabs(linear_travel));                //是计算直角三角形斜边函数吗??
弧长 = 圆心角*半径,即 =angular_travel*radius;
linear_travel指的是哪段呢??

出0入0汤圆

 楼主| 发表于 2019-3-22 12:28:02 | 显示全部楼层
hkjabcd 发表于 2019-3-22 10:28
hypot(angular_travel*radius, fabs(linear_travel));                //是计算直角三角形斜边函数吗??
弧长 = 圆心角 ...


个人理解linear_travel表示Z方向上的距离,angular_travel*radius是XY平面的弧长,fabs(linear_travel)是Z方向的距离,Z轴与XY平面夹角为90°,
  确切的讲是一个稍微弯曲的立起来的直角三角形,空间路径是斜边,如果Z方向没有进给=0,弧长就只有XY平面中的PTQW这段。

出0入0汤圆

发表于 2019-3-22 20:50:21 | 显示全部楼层
XTXB 发表于 2019-3-22 12:28
个人理解linear_travel表示Z方向上的距离,angular_travel*radius是XY平面的弧长,fabs(linear_travel)是 ...

有道理,
但是
float millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel));//计算弧长
uint16_t segments = floor(millimeters_of_travel/settings.mm_per_arc_segment);//小线段的数量 = 弧长/每一小段的步进量
如果是Z轴的和PW的斜边,后面的函数用millimeters_of_travel求每一小段的数量就有点不可思议了

出0入0汤圆

 楼主| 发表于 2019-3-24 07:06:18 | 显示全部楼层
本帖最后由 XTXB 于 2019-3-24 07:15 编辑
hkjabcd 发表于 2019-3-22 20:50
有道理,
但是
float millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel));//计 ...


不知道你的不可思议指的是什么,我的理解是这样的:
小线段的数量 = 总长度/每一小段的步进量
总长度=Z轴的和PW的斜边=millimeters_of_travel,最小步进量=settings.mm_per_arc_segment);
貌似没问题啊

出16170入6148汤圆

发表于 2023-10-25 17:12:38 来自手机 | 显示全部楼层
帖子移动通知:
原分论坛:8051/STC32【已下线】
目标分论坛:51单片机
移动时间:0小时之后
回帖提示: 反政府言论将被立即封锁ID 在按“提交”前,请自问一下:我这样表达会给举报吗,会给自己惹麻烦吗? 另外:尽量不要使用Mark、顶等没有意义的回复。不得大量使用大字体和彩色字。【本论坛不允许直接上传手机拍摄图片,浪费大家下载带宽和论坛服务器空间,请压缩后(图片小于1兆)才上传。压缩方法可以在微信里面发给自己(不要勾选“原图),然后下载,就能得到压缩后的图片】。另外,手机版只能上传图片,要上传附件需要切换到电脑版(不需要使用电脑,手机上切换到电脑版就行,页面底部)。
您需要登录后才可以回帖 登录 | 注册

本版积分规则

手机版|Archiver|amobbs.com 阿莫电子技术论坛 ( 粤ICP备2022115958号, 版权所有:东莞阿莫电子贸易商行 创办于2004年 (公安交互式论坛备案:44190002001997 ) )

GMT+8, 2024-4-15 13:41

© Since 2004 www.amobbs.com, 原www.ourdev.cn, 原www.ouravr.com

快速回复 返回顶部 返回列表