Rover2019/motorTestCLI.py at master · technocratsRover/Rover2019 · 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
from nanpy import ArduinoApi, SerialManager
from time import sleep
import threading


# FRONT BACK RIGHT LEFT
motorState = [False, False, False, False]

def resetAllMotors():
  for i in range(len(motorState)):
    motorState[i]=False

# The motors pins are pinOffset + 0, pinOffset + 1, pinOffset + 2, pinOffset + 3
pinOffset = 7

# Connect to arduino
try:
  connection = SerialManager("/dev/ttyUSB0")
  a = ArduinoApi(connection=connection)
except:
  print("Failed to connect to arduino!")

def motor():
  print("Motor thread Started!")
  while True:
    for i in range(len(motorState)):
      if motorState[i]:
        a.digitalWrite(pinOffset+i, a.HIGH)
      else:
        a.digitalWrite(pinOffset+i, a.LOW)

# def input():
#   while True:
#     print("Enter command: ", end='')
#     motorNum = int(input().rstrip)
#     try:
#       motorState[motorNum]=True
#     except IndexError:
#       if motorNum ==  4:
#         resetAllMotors()
#       else:
#         print("Invalid motor Number, Valid options: [0, 1, 2, 3, 4]")

motorThread = threading.Thread(target=motor)
# inputThread = threading.Thread(target=input)
motorThread.start()
# inputThread.start()
while True:
  print("Enter command: ", end='')
  try:
    motorNum = int(input())
  except ValueError:
    print("Enter an integer!, Valid options: [0, 1, 2, 3, 4]")
    continue
  try:
    resetAllMotors()
    motorState[motorNum]=True
  except IndexError:
    if motorNum ==  4:
      resetAllMotors()
    else:
      print("Invalid motor Number, Valid options: [0, 1, 2, 3, 4]")