epmc_serial_py/four_motor_support.py at main · robocre8/epmc_serial_py · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
from epmc_serial import EPMCSerialClient, SupportedNumOfMotors
import time

controller = EPMCSerialClient()

# variable for communication
pos0=0.0; pos1=0.0; pos2=0.0; pos3=0.0
vel0=0.0; vel1=0.0; vel2=0.0; vel3=0.0

# [4 rev/sec, 2 rev/sec, 1 rev/sec, 0.5 rev/sec]
targetVel = [1.571, 3.142, 6.284, 12.568]
vel = targetVel[1]
v = 0.0

readTime = None
readTimeInterval = 0.02 # 50Hz

cmdTime = None
cmdTimeInterval = 5.0

# EPMC Connect
serial_port = '/dev/ttyUSB0'
serial_baudrate = 115200
serial_timeout = 0.018 #value < 0.02 (for 50Hz comm)
controller.supportedNumOfMotors(SupportedNumOfMotors.FOUR)
controller.connect(serial_port, serial_baudrate, serial_timeout)

success = controller.clearDataBuffer()
controller.writeSpeed(0.0, 0.0, 0.0, 0.0)
print('configuration complete')

timeout_ms = 10000
controller.setCmdTimeout(timeout_ms)
success, val0 = controller.getCmdTimeout()
if success: # only update if read was successfull
  timeout_ms = val0
  print("command timeout in ms: ", timeout_ms)
else:
  print("ERROR: could not read motor command timeout")

sendHigh = True

readTime = time.time()
cmdTime = time.time()

while True:
  if time.time() - cmdTime > cmdTimeInterval:
    if sendHigh:
      # print("command high")
      v = vel
      controller.writeSpeed(v, v, v, v)
      vel = vel*-1
      sendHigh = False
    else:
      # print("command low")
      v = 0.0
      controller.writeSpeed(v, v, v, v)
      sendHigh = True


    cmdTime = time.time()


  if time.time() - readTime > readTimeInterval:
    try:
      # controller.writeSpeed(v, v, v, v)
      success, val = controller.readMotorData()
      if success: # only update if read was successfull
        pos0 = val[0]; pos1 = val[1]; pos2 = val[2]; pos3 = val[3]
        vel0 = val[4]; vel1 = val[5]; vel2 = val[6]; vel3 = val[7]

      print(f"motor0_readings: [{pos0}, {vel0}]")
      print(f"motor1_readings: [{pos1}, {vel1}]")
      print(f"motor2_readings: [{pos2}, {vel2}]")
      print(f"motor3_readings: [{pos3}, {vel3}]")
      print("")
    except:
      pass

    readTime = time.time()