用stc15w和旧光驱移植arduino写字机GRBL之六：motion_control (amobbs.com 阿莫电子技术论坛) -

XTXB 发表于 2018-12-1 12:27:41

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

本帖最后由 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)：

su33691 发表于 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));
}

su33691 发表于 2018-12-1 13:10:25

checksum = (checksum << 1) || (checksum >> 7);//|| 应为 |

XTXB 发表于 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;
settings_store_startup_line(n, line);
return(false);
} else {
return(true);
}
}

XTXB 发表于 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不是可以省一点么？

amxx 发表于 2018-12-1 17:20:27

请问一下楼主，这个假设
sinT=sina=a
cosT=cosa=1-a²/2
这个假设不成立啊
这个假设没办法推倒除 sin²T+cos²T = 1
请楼主指正

XTXB 发表于 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

amxx 发表于 2018-12-3 08:05:48

XTXB 发表于 2018-12-1 17:26
sinT=sina，cosT=cosa是假设，
sina=a-...，cosa=1-a²/2+....... ，是泰勒级数
a是弧度

谢谢楼主！补习一下。

XTXB 发表于 2018-12-3 16:21:11

更正：
float millimeters_of_travel = hypot(angular_travel*radius, fabs(linear_travel));//计算小线段的弧长应为计算弧长

flash3g 发表于 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会快点。

王涛发表于 2019-2-22 15:36:03

本帖最后由王涛于 2019-2-22 15:37 编辑

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

hkjabcd 发表于 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
这句是指哪条线段或是哪两点？？

XTXB 发表于 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 + offset;//圆心坐标
float center_axis1 = position + offset;
float linear_travel = target - position;//起点到终点的轴的进给速率
float r_axis0 = -offset;// Radius vector from center to current location
float r_axis1 = -offset;//圆心指向圆弧起点P的向量坐标
float rt_axis0 = target - center_axis0;//圆心指向圆弧终点W的向量坐标
float rt_axis1 = target - 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);
//计算圆心角

hkjabcd 发表于 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的斜率，这 ...

谢谢！有时间再仔细看下

hkjabcd 发表于 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指的是哪段呢？？

XTXB 发表于 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这段。

hkjabcd 发表于 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求每一小段的数量就有点不可思议了

XTXB 发表于 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);
貌似没问题啊

armok. 发表于 2023-10-25 17:12:38

帖子移动通知：
原分论坛：8051/STC32【已下线】
目标分论坛：51单片机
移动时间：0小时之后

页: [1]

amobbs.com 阿莫电子技术论坛's Archiver

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