open------mv----四象限--line interpolation

MoveLine.py

#直线插补代码
#MoveLine.Move_Line_Interpolation(0,0,5,8,1)
#x0=0,y0=0,xe=5,ye=8,quadrant=1
#cnt= 0,x0=1,y0=0
#cnt= 1,x0=1,y0=1
#cnt= 2,x0=1,y0=2
#cnt= 3,x0=2,y0=2
#cnt= 4,x0=2,y0=3
#cnt= 5,x0=2,y0=4
#cnt= 6,x0=3,y0=4
#cnt= 7,x0=3,y0=5
#cnt= 8,x0=4,y0=5
#cnt= 9,x0=4,y0=6
#cnt= 10,x0=4,y0=7
#cnt= 11,x0=5,y0=7
#cnt= 12,x0=5,y0=8

#==================================
#MoveLine.Move_Line_Interpolation(3,5,-12,10,1)
#x0=3,y0=5,xe=-12,ye=10,quadrant=2
#cnt= 0,x0=2,y0=5
#cnt= 1,x0=2,y0=6
#cnt= 2,x0=1,y0=6
#cnt= 3,x0=0,y0=6
#cnt= 4,x0=-1,y0=6
#cnt= 5,x0=-1,y0=7
#cnt= 6,x0=-2,y0=7
#cnt= 7,x0=-3,y0=7
#cnt= 8,x0=-4,y0=7
#cnt= 9,x0=-4,y0=8
#cnt= 10,x0=-5,y0=8
#cnt= 11,x0=-6,y0=8
#cnt= 12,x0=-7,y0=8
#cnt= 13,x0=-7,y0=9
#cnt= 14,x0=-8,y0=9
#cnt= 15,x0=-9,y0=9
#cnt= 16,x0=-10,y0=9
#cnt= 17,x0=-10,y0=10
#cnt= 18,x0=-11,y0=10
#cnt= 19,x0=-12,y0=10


def judge_quadrant(x0, y0, xe, ye):
    x = xe - x0
    y = ye - y0
    if (x > 0) and (y > 0):  # 第一象限
        quadrant = 1
    elif (x < 0) and (y > 0):  # 第二象限
        quadrant = 2
    elif (x < 0) and (y < 0):  # 第三象限
        quadrant = 3
    elif (x > 0) and (y < 0):  # 第四象限
        quadrant = 4
    elif x > 0 and y == 0:
        quadrant = 5  # X轴正方向
    elif x < 0 and y == 0:
        quadrant = 6  # X轴负方向
    elif x == 0 and y > 0:
        quadrant = 7  # Y轴正方向
    elif x == 0 and y < 0:
        quadrant = 8  # Y轴负方向
    return quadrant

def interpolation(quadrant, x0, y0, xe, ye,my_step):

    x = xe - x0
    y = ye - y0
    fm = 0
    x1 = abs(x)
    y1 = abs(y)
    total = (x1 + y1) / my_step
    cnt = 0
    while cnt < total:
        if quadrant < 5:
            if fm >= 0:
                if quadrant == 1 or quadrant == 4:
                    x0 += my_step
                elif quadrant == 2 or quadrant == 3:
                    x0 -= my_step
                fm = fm - y1
            elif fm < 0:
                if quadrant == 1 or quadrant == 2:
                    y0 += my_step
                elif quadrant == 3 or quadrant == 4:
                    y0 -= my_step
                fm = fm + x1
        else:
            if quadrant == 5:
                x0 += my_step
            elif quadrant == 6:
                x0 -= my_step
            elif quadrant == 7:
                y0 += my_step
            elif quadrant == 8:
                y0 -= my_step
        print("cnt= %d,x0=%d,y0=%d"%(cnt,x0,y0))
        cnt += 1


def Move_Line_Interpolation(xo,yo,xe,ye,my_step):
        xo=xo* my_step
        yo=yo* my_step
        xe=xe* my_step
        ye=ye* my_step
        quadrant = judge_quadrant(xo, yo, xe, ye)
        print("x0=%d,y0=%d,xe=%d,ye=%d,quadrant=%d"%(xo,yo,xe,ye,quadrant))
        interpolation(quadrant, xo, yo, xe, ye,my_step)

在这里插入图片描述

main.py

import sensor, image, time,utime
from steps28BY import stepper28BY  #导入库
import MoveLine
#sensor.reset()
#sensor.set_pixformat(sensor.RGB565)
#sensor.set_framesize(sensor.QVGA)
#sensor.skip_frames(time = 2000)
clock = time.clock()
BY1=stepper28BY('E7','E8','E9','E10') #电机1初始化
BY2=stepper28BY('C4','C5','B0','B1')#电机2初始化




#4096---360
#x------1
#1------y   y=360/4096=0.087度，一个节拍0.087度
#x=4096/360=11.38

#BY1.forward(1,1024)
#deg=90
#painum=deg/0.088
#print(deg,painum)
#BY1.forward(1,painum)

BY1.Move_Degree(1,1,1) #电机1转动，正方向，延时毫秒，180度
time.sleep(1)


BY2.Move_Degree(1,1,1)  #电机2转动，正方向，延时毫秒，180度
time.sleep(1)



MoveLine.Move_Line_Interpolation(3,5,-12,10,1)


while(True):
    clock.tick()
    #img = sensor.snapshot()
   # print(clock.fps())

使用步骤：

1、把MoveLine.py拷贝到open—mv的盘内
2、在main.py中import MoveLine；
3、在函数中MoveLine.Move_Line_Interpolation(3,5,-12,10,1)
MoveLine.Move_Line_Interpolation(x0,y0,xe,ye,steplength)
x0,y0是起点坐标
xe,ye是终点坐标
steplength是步长。这里用1，根据实际情况进行设置。

上述只是用print把每个动点坐标点打印进行验证。

实际用电机的时候，需要添加代码，改为对应的电机转动的角度函数。或者是mm函数。步进。

插补代码参考
https://github.com/kaitokuroba7/CNC–interpolation