97 lines
1.8 KiB
Python
97 lines
1.8 KiB
Python
import serial
|
|
from time import sleep
|
|
|
|
|
|
class meArm:
|
|
position = [90,90,90,90]
|
|
moves = []
|
|
port = ""
|
|
baudrate = 0
|
|
ser = serial.Serial()
|
|
|
|
def __init__(self, port = '/dev/ttyACM0', bautrate = 9600):
|
|
self.port = port
|
|
self.baudrate = bautrate
|
|
self.ser = serial.Serial(self.port, self.baudrate, timeout = 1)
|
|
|
|
def addMove(self, moves = [b'D1\n']):
|
|
self.moves.append(moves)
|
|
return len(self.moves) -1
|
|
|
|
def getMoves(self):
|
|
[print(i, x) for i, x in enumerate(self.moves)]
|
|
|
|
def sendCmd(self, cmd = b'H1'):
|
|
self.ser.flushInput() #Last change. if broken remove this
|
|
while(1):
|
|
line = self.ser.readline()
|
|
# print(f"sendCmd: {line}")
|
|
if(line == b'READY\r\n'):
|
|
break
|
|
self.ser.write(cmd)
|
|
self.ser.write(b'\n')
|
|
# print(f"sendCmd: wrote: {cmd}")
|
|
|
|
def execMove(self, idx):
|
|
if(idx >= len(self.moves)):
|
|
print("invalid movement index")
|
|
return
|
|
for cmd in self.moves[idx]:
|
|
print(f"sending: {cmd}")
|
|
self.sendCmd(cmd)
|
|
return len(self.moves[idx])
|
|
|
|
|
|
def getPos(self):
|
|
self.sendCmd()
|
|
sleep(0.1)
|
|
self.ser.flushInput()
|
|
c = 0
|
|
while(1):
|
|
line = self.ser.readline()
|
|
print(line)
|
|
if(line == b'READY\r\n'):
|
|
c+=1
|
|
if c ==2: break
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
if __name__ == '__main__':
|
|
m1 = [b'T70', b'L40', b'R150', b'G180', b'R90', b'D1']
|
|
m2 = [b'S1', b'T70', b'L40', b'R150', b'G180', b'R90', b'T140', b'L50', b'R150', b'G90', b'D1']
|
|
m3 = [b'S1', b'T140', b'L50', b'R150', b'G180', b'D1']
|
|
arm = meArm()
|
|
arm.getPos()
|
|
i = arm.addMove(m1)
|
|
i = arm.addMove(m2)
|
|
i = arm.addMove(m3)
|
|
|
|
arm.execMove(1)
|
|
arm.getPos()
|
|
arm.execMove(2)
|
|
|
|
|
|
|
|
|
|
|
|
|