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

XTXB

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

楼主，请帮忙看一下
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

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

XTXB

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);
  }
}

XTXB

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

amxx

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

XTXB

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

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

谢谢楼主！补习一下。

XTXB

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

flash3g

XTXB 发表于 2018-12-1 16:09
int memcpy_from_eeprom_with_checksum(char *destination, unsigned int source, unsigned int size)
{
        . ...

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

王涛

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

hkjabcd

楼主，这里有点不太明白，
以上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

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);
  //计算圆心角

hkjabcd

XTXB 发表于 2019-3-5 09:13
应该是在坐标系XOY中的P和W，要用到正切公式tanφ=K1-K2/(1+K1*K2)，φ是夹角，K1和K2是OP和OW的斜率，这 ...

谢谢！有时间再仔细看下

hkjabcd

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

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

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

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.

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