adding pink trombone, rfid arduino code, camera views rotater, !go and !turn commands
[lolcam] / send_command.py
1 #!/usr/bin/env python3
2
3 # __ ____ __
4 # | | | _ | | |
5 # | :_ | '-'| | :_ [ @ m 📸
6 # '----' '----' '----' ````````
7 #
8 # This script launches a daemon process that waits for robot driving commands to appear in a text file
9 # and also sends commands to that text file when run separately. Those commands are forwarded by the daemon
10 # as byte arrays to the serial port using the PySerial library. For LOLcam, the serial port is connected to an
11 # Arduino that translates the byte arrays into a 32-bit value that is transmitted to the robot over infra-red.
12 #
13
14 import sys, time, os, pathlib, argparse, SerialDevice
15
16 class SendCommand(SerialDevice.SerialDevice):
17
18 MCORE_VID = 6790
19 MCORE_PID = 29987
20 ARDUINO_VID = 9025
21 ARDUINO_PID = 67
22 COMMAND_PATH = os.path.join(os.path.abspath(os.path.dirname(sys.argv[0])), "command.txt")
23
24 def initialize_serial_mcore(self):
25 '''
26 Initialize port that has mCore attached
27 '''
28 self.initialize_serial_id(self.MCORE_VID, self.MCORE_PID)
29
30 def initialize_serial_arduino(self):
31 '''
32 Initialize port that has Arduino attached
33 '''
34 self.initialize_serial_id(self.ARDUINO_VID, self.ARDUINO_PID)
35
36 def buffer_command(self, *args, robot=0, chain=False):
37 '''
38 Write the command to file to be processed later by the method monitoring the file. If less than three args were passed,
39 assume a named command was passed and translate it into its components before writing the command.
40 '''
41 with open(self.COMMAND_PATH, "at") as fp:
42 if len(args) < 3:
43 command = args[0]
44 chain_out = "d"
45 if command == "wheels":
46 fp.write(f"15 12 15 1 0 {robot} 0 7 0 1 {chain:{chain_out}} 0\n")
47 elif command == "donut":
48 fp.write(f"15 12 15 1 1 {robot} 0 3 0 1 {chain:{chain_out}} 0\n")
49 elif command == "walk":
50 fp.write(f"8 13 3 1 0 {robot} 1 3 15 1 {chain:{chain_out}} 0\n")
51 elif command == "bash":
52 fp.write(f"15 15 15 1 0 {robot} 1 4 15 0 {chain:{chain_out}} 0\n")
53 elif command == "pose":
54 fp.write(f"9 15 5 1 1 {robot} 0 3 15 1 {chain:{chain_out}} 0\n")
55 elif command == "spin":
56 fp.write(f"3 15 13 1 1 {robot} 0 7 0 1 {chain:{chain_out}} 0\n")
57 elif command == "worm":
58 fp.write(f"4 15 13 1 1 {robot} 1 7 0 1 {chain:{chain_out}} 0\n")
59 elif command == "charge":
60 fp.write(f"15 15 15 1 0 {robot} 0 0 0 0 {chain:{chain_out}} 0\n")
61 elif command == "rollout":
62 fp.write(f"10 15 15 1 0 {robot} 0 3 0 0 {chain:{chain_out}} 0\n")
63 elif command == "retreat":
64 fp.write(f"15 15 15 0 1 {robot} 0 0 0 0 {chain:{chain_out}} 0\n")
65 elif command == "drill":
66 fp.write(f"4 15 15 1 0 {robot} 1 7 0 0 {chain:{chain_out}} 0\n")
67 elif command == "star":
68 fp.write(f"13 15 2 1 1 {robot} 0 7 15 1 {chain:{chain_out}} 0\n")
69 elif command == "ring":
70 fp.write(f"15 15 8 1 0 {robot} 0 7 6 0 {chain:{chain_out}} 0\n")
71 elif command == "swerve":
72 fp.write(f"14 4 15 1 0 {robot} 0 0 0 0 {chain:{chain_out}} 0\n")
73 elif command == "hang":
74 fp.write(f"14 15 4 1 0 {robot} 0 0 0 0 {chain:{chain_out}} 0\n")
75 elif command == "turn":
76 fp.write(f"6 15 15 1 1 {robot} 0 0 0 0 {chain:{chain_out}} 0\n")
77 elif command == "hand":
78 fp.write(f"0 0 0 1 0 {robot} 0 0 0 0 {chain:{chain_out}} 1\n")
79 elif command == "inch":
80 fp.write(f"4 8 8 1 0 {robot} 0 0 0 0 {chain:{chain_out}} 0\n")
81 elif command == "scooch":
82 fp.write(f"7 6 6 0 1 {robot} 0 0 0 0 {chain:{chain_out}} 0\n")
83 elif command == "45":
84 fp.write(f"2 6 6 1 1 {robot} 0 4 0 0 {chain:{chain_out}} 0\n")
85 elif command == "90":
86 fp.write(f"5 9 9 1 1 {robot} 0 1 0 0 {chain:{chain_out}} 0\n")
87 elif command == "180":
88 fp.write(f"3 11 11 1 1 {robot} 0 4 0 0 {chain:{chain_out}} 0\n")
89 elif command == "advance":
90 fp.write(f"8 11 11 1 0 {robot} 0 0 0 0 {chain:{chain_out}} 0\n")
91 elif command == "shrink":
92 fp.write(f"6 11 11 0 1 {robot} 0 0 0 0 {chain:{chain_out}} 0\n")
93 else:
94 print(f"unrecognized command: {command}")
95 else:
96 command = ""
97 malformed = False
98 for ii in range(12):
99 if len(args) > ii:
100 if int(args[ii]) < 0:
101 print(f"invalid value {args[ii]}: all values must be positive")
102 malformed = True
103 break
104 command += args[ii]
105 elif ii == 3:
106 command += "1"
107 else:
108 command += "0"
109 if ii < 11:
110 command += " "
111 if not malformed:
112 fp.write(f"{command}\n")
113
114 def watch_file_for_commands(self):
115 '''
116 Loop forever, checking a file for queued commands every iteration. If a command is found that is not set to be chained to the
117 next command, send every command up to and including that command and erase the file.
118 '''
119 while True:
120 # file containing commands, one per line
121 path = self.COMMAND_PATH
122 if not os.path.exists(path):
123 pathlib.Path(path).touch()
124 if os.path.getsize(path) > 0:
125 read_fields = []
126 with open(path, "rt") as fp:
127 # check every line in the file
128 for line in fp:
129 if line != "":
130 # single command split into components
131 fields = [int(column) for column in line.split()]
132 # store for later in case we need to send commands
133 read_fields.append(fields)
134 # if the chain field is unset, this is the last command that will take effect, so we can break
135 if fields[-2] == 0:
136 break
137 # erase file
138 os.truncate(path, 0)
139 for fields in read_fields:
140 self.send_command(fields)
141 # need to sleep between IR signals or the Arduino won't read them correctly
142 time.sleep(.5)
143 time.sleep(.02)
144
145 def send_command(self, byte_list):
146 '''
147 Write a single command to serial as array of bytes
148 '''
149 self.serial.reset_input_buffer()
150 self.serial.reset_output_buffer()
151 if self.serial is not None:
152 byte_value = bytes(byte_list)
153 self.serial.write(byte_value)
154 self.serial.write(b"\n")
155 print((
156 "sending... time: {}, left: {}, right: {}, left sign: {}, right sign: {}, robot id: {}, flip: {}, repeats: {}, "
157 "delay: {}, swap: {}, chain: {}, hand: {}").format(*byte_value))
158
159
160 if __name__ == "__main__":
161 sc = SendCommand()
162 parser = argparse.ArgumentParser(
163 description="For opening a serial connection to Arduino 9025:67, setting it to watch a file for commands and buffering commands",
164 formatter_class=argparse.ArgumentDefaultsHelpFormatter)
165 parser.add_argument("command", nargs="*", help=(
166 "either a keyword (see keywords in the code) or up to 11 components of command to buffer that will be read by device: "
167 "time: 0 - 15, left: 0 - 15, right: 0 - 15, left sign: 0/1, right sign: 0/1, "
168 "robot id: 0/1, flip: 0/1, repeats: 0 - 7, delay: 0 - 16, swap: 0/1, chain: 0/1, hand: 0/1"))
169 parser.add_argument("--list-ports", action="store_true", help="print a list of connected devices and their ports")
170 parser.add_argument("--chain", action="store_true", help="mark this command as being chained to the next")
171 parser.add_argument("--robot", type=int, choices=(0, 1), default=0, help="robot ID to send command to")
172 parser.add_argument("--port", help="device port")
173 arguments = parser.parse_args()
174 if arguments.list_ports:
175 sc.list_ports()
176 elif arguments.command:
177 sc.buffer_command(*arguments.command, robot=arguments.robot, chain=arguments.chain)
178 else:
179 if arguments.port is not None:
180 sc.initialize_serial(arguments.port)
181 else:
182 sc.initialize_serial_arduino()
183 sc.watch_file_for_commands()