Skip to content

Instantly share code, notes, and snippets.

@hephaestus9

hephaestus9/BBB_Code.py

Last active August 29, 2015 14:15
Show Gist options
  • Save hephaestus9/b46f486fd7366c5b7642 to your computer and use it in GitHub Desktop.
Save hephaestus9/b46f486fd7366c5b7642 to your computer and use it in GitHub Desktop.
Download ZIP
BBB Code
Raw
BBB_Code.py
# -*- coding: utf-8 -*-
# J.Brian 1-22-2015
import Adafruit_BBIO.UART as uart
import socket
import serial
import threading
import ast
import time
from random import randint
class ROV():
TABLE = [
0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31, 65,
157, 195, 33, 127, 252, 162, 64, 30, 95, 1, 227, 189, 62, 96, 130, 220,
35, 125, 159, 193, 66, 28, 254, 160, 225, 191, 93, 3, 128, 222, 60, 98,
190, 224, 2, 92, 223, 129, 99, 61, 124, 34, 192, 158, 29, 67, 161, 255,
70, 24, 250, 164, 39, 121, 155, 197, 132, 218, 56, 102, 229, 187, 89, 7,
219, 133, 103, 57, 186, 228, 6, 88, 25, 71, 165, 251, 120, 38, 196, 154,
101, 59, 217, 135, 4, 90, 184, 230, 167, 249, 27, 69, 198, 152, 122, 36,
248, 166, 68, 26, 153, 199, 37, 123, 58, 100, 134, 216, 91, 5, 231, 185,
140, 210, 48, 110, 237, 179, 81, 15, 78, 16, 242, 172, 47, 113, 147, 205,
17, 79, 173, 243, 112, 46, 204, 146, 211, 141, 111, 49, 178, 236, 14, 80,
175, 241, 19, 77, 206, 144, 114, 44, 109, 51, 209, 143, 12, 82, 176, 238,
50, 108, 142, 208, 83, 13, 239, 177, 240, 174, 76, 18, 145, 207, 45, 115,
202, 148, 118, 40, 171, 245, 23, 73, 8, 86, 180, 234, 105, 55, 213, 139,
87, 9, 235, 181, 54, 104, 138, 212, 149, 203, 41, 119, 244, 170, 72, 22,
233, 183, 85, 11, 136, 214, 52, 106, 43, 117, 151, 201, 74, 20, 246, 168,
116, 42, 200, 150, 21, 75, 169, 247, 182, 232, 10, 84, 215, 137, 107, 53
]
def __init__(self):
self.UDP_IPReceive = ""
self.UDP_IPSend = "192.168.1.13"
self.UDP_PORT_RECV = 5005
self.UDP_PORT_SEND = 5006
self.receiveSock = socket.socket(socket.AF_INET, # Internet
socket.SOCK_DGRAM) # UDP
self.receiveSock.bind((self.UDP_IPReceive, self.UDP_PORT_RECV))
self.sendSock = socket.socket(socket.AF_INET, # Internet
socket.SOCK_DGRAM) # UDP
uart.setup("UART2") # Sensor Board
self.telemetry = serial.Serial(port="/dev/ttyO2", baudrate=115200)
self.telemetry.close()
self.telemetry.open()
uart.setup("UART1") # Motor Driver Board
self.thrusters = serial.Serial(port="/dev/ttyO1", baudrate=115200)
self.thrusters.close()
self.thrusters.open()
self.vert = 0
self.port = 0
self.stbd = 0
self.port1 = 0
self.stbd1 = 0
self.port2 = 0
self.stbd2 = 0
self.receiver()
self.telemetryStatus()
self.sum = 0x00
def telemetryStatus(self):
def getTelemetryStatus(serialPort, processData):
while True:
line = serialPort.readline()
processData(line)
receiver_thread = threading.Thread(target=getTelemetryStatus, args=(self.telemetry, self.processData))
receiver_thread.daemon = True
receiver_thread.start()
def processData(self, sensorData):
try:
sensorData = sensorData.split(":")
endData = sensorData[9]
sensorData[9] = endData[:-2]
sensorData = str({"sensor stick": {"G_Dt": sensorData[0],
"Accel": [sensorData[1], sensorData[2], sensorData[3]],
"Magnetom": [sensorData[4], sensorData[5], sensorData[6]],
"Gyro": [sensorData[7], sensorData[8], sensorData[9]]}, "depth sensor": [], "altimeter": []})
# print(sensorData)
self.sendUDP(sensorData)
except:
self.sendUDP(str(sensorData))
def sendUDP(self, sensorData):
# print(sensorData)
self.sendSock.sendto(sensorData, (self.UDP_IPSend, self.UDP_PORT_SEND))
def receiver(self):
def receiverListen(sock, stringCreator):
while True:
data, addr = sock.recvfrom(1024) # buffer size is 1024 bytes
# print(("received message:", data))
stringCreator(data)
receiver_thread = threading.Thread(target=receiverListen, args=(self.receiveSock, self.createCommandString))
receiver_thread.daemon = True
receiver_thread.start()
def createCommandString(self, data):
data = ast.literal_eval(data)
for key, val in list(data.items()):
if "Axis" in key:
# For Logitech Extreme 3D: Axis 0 - Lateral, Axis 1 - Fwd/Rev, Axis 2 - Turn, Axis 3 - Elevation
if data['Axis'] == 0: # Lateral
pass # can't do lateral right now'
elif data['Axis'] == 1: # Fwd -, Rev +
self.port1 = -1 * int(round(data['value'] * 255))
self.stbd1 = -1 * int(round(data['value'] * 255))
elif data['Axis'] == 2: # Turn ***Sent from Linux: 2, Sent from Windows: 3
if data['value'] < 0: # Turn Left
self.port2 = -1 * int(round(-1 * data['value'] * 255))
self.stbd2 = -1 * int(round(data['value'] * 255))
else: # Turn Right
self.port2 = -1 * int(round((-1 * data['value']) * 255))
self.stbd2 = -1 * int(round(data['value'] * 255))
elif data['Axis'] == 3: # Vert Up -, Dwn + ***Sent from Linux: 3, Sent from Windows: 2
self.vert = int(round(data['value'] * 255))
else:
print(('Error in Axis Data!'))
if self.port1 > self.port2:
# print(("port1 > port2: "))
# print((self.port1, self.port2))
if self.port2 >= 0 and self.port1 > 0:
port = self.port1 + self.port2
if port > 255:
port = 255
self.port = port
elif self.port2 < 0 and self.port1 > 0:
self.port = self.port1 + self.port2
elif self.port1 <= 0 and self.port2 < 0:
port = self.port2 + self.port1
if port < -255:
port = -255
self.port = port
else:
self.port = self.port2 - self.port1
elif self.port2 > self.port1:
# print(("port2 > port1: "))
# print((self.port2, self.port1))
if self.port1 >= 0 and self.port2 > 0:
port = self.port2 + self.port1
if port > 255:
port = 255
self.port = port
elif self.port1 < 0 and self.port2 > 0:
self.port = self.port2 + self.port1
elif self.port2 <= 0 and self.port1 < 0:
port = self.port1 + self.port2
if port < -255:
port = -255
self.port = port
else:
self.port = self.port1 - self.port2
elif self.port1 == self.port2:
self.port = self.port1
else:
print(("There is a problem in the way port thrust is being calculated!"))
if self.stbd1 > self.stbd2:
# print(("stbd1 > stbd2: "))
# print((self.stbd1, self.stbd2))
if self.stbd2 >= 0 and self.stbd1 > 0:
stbd = self.stbd1 + self.stbd2
if stbd > 255:
stbd = 255
self.stbd = stbd
elif self.stbd2 < 0 and self.stbd1 > 0:
self.stbd = self.stbd1 + self.stbd2
elif self.stbd1 <= 0 and self.stbd2 < 0:
stbd = self.stbd2 + self.stbd1
if stbd < -255:
stbd = -255
self.stbd = stbd
else:
self.stbd = self.stbd2 - self.stbd1
elif self.stbd2 > self.stbd1:
# print(("stbd2 > stbd1: "))
# print((self.stbd2, self.stbd1))
if self.stbd1 >= 0 and self.stbd2 > 0:
stbd = self.stbd2 + self.stbd1
if stbd > 255:
stbd = 255
self.stbd = stbd
elif self.stbd1 < 0 and self.stbd2 > 0:
self.stbd = self.stbd2 + self.stbd1
elif self.stbd2 <= 0 and self.stbd1 < 0:
stbd = self.stbd1 + self.stbd2
if stbd < -255:
stbd = -255
self.stbd = stbd
else:
self.stbd = self.stbd1 - self.stbd2
elif self.stbd1 == self.stbd2:
self.stbd = self.stbd1
else:
print(("There is a problem in the way stbd thrust is being calculated!"))
#command string format: START_MESSAGE, VERT, VERT_VALUE, PORT, PORT_VALUE, STBD, STBD_VALUE, AUX1, AUX1_VALUE, AUX2, AUX2_VALUE, AUX3, AUX3_VALUE, CRC, END_MESSAGE
# if VERT, PORT, or STBD _VALUE is 125(0x7D), 126(0x7E) or 127(0x7F) then use the escape and 0x5D(125), 0x5E(126) or 0x5F(127)
START_MESSAGE = chr(0x7F)
VERT = chr(0x76)
PORT = chr(0x70)
STBD = chr(0x73)
AUX1 = chr(0x61)
AUX2 = chr(0x62)
AUX3 = chr(0x63)
END_MESSAGE = chr(0x7E)
ESCAPE = chr(0x7D)
vert = []
port = []
stbd = []
if self.vert < 0:
vert.append(chr(0xF9)) # 0
vert.append(chr(0xF9)) # 1
if (-1 * self.vert) == 125:
vert.append(ESCAPE) # 2
vert.append(chr(0x5D)) # 3
if (-1 * self.vert) == 126:
vert.append(ESCAPE) # 2
vert.append(chr(0x5E)) # 3
if (-1 * self.vert) == 127:
vert.append(ESCAPE) # 2
vert.append(chr(0x5F)) # 3
else:
vert.append(chr(-1 * self.vert)) # 2
else:
if self.vert == 125:
vert.append(ESCAPE) # 0
vert.append(chr(0x5D)) # 1
if self.vert == 126:
vert.append(ESCAPE) # 0
vert.append(chr(0x5E)) # 1
if self.vert == 127:
vert.append(ESCAPE) # 0
vert.append(chr(0x5F)) # 1
else:
vert.append(chr(self.vert)) # 0
if self.port < 0:
port.append(chr(0xF9)) # 0
port.append(chr(0xF9)) # 1
if (-1 * self.port) == 125:
port.append(ESCAPE) # 2
port.append(chr(0x5D)) # 3
if (-1 * self.port) == 126:
port.append(ESCAPE) # 2
port.append(chr(0x5E)) # 3
if (-1 * self.port) == 127:
port.append(ESCAPE) # 2
port.append(chr(0x5F)) # 3
else:
port.append(chr(-1 * self.port)) # 2
else:
if self.port == 125:
port.append(ESCAPE) # 0
vert.append(chr(0x5D)) # 1
if self.port == 126:
port.append(ESCAPE) # 0
port.append(chr(0x5E)) # 1
if self.port == 127:
port.append(ESCAPE) # 0
port.append(chr(0x5F)) # 1
else:
port.append(chr(self.port)) # 0
if self.stbd < 0:
stbd.append(chr(0xF9)) # 0
stbd.append(chr(0xF9)) # 1
if (-1 * self.stbd) == 125:
stbd.append(ESCAPE) # 2
stbd.append(chr(0x5D)) # 3
if (-1 * self.stbd) == 126:
stbd.append(ESCAPE) # 2
stbd.append(chr(0x5E)) # 3
if (-1 * self.stbd) == 127:
stbd.append(ESCAPE) # 2
stbd.append(chr(0x5F)) # 3
else:
stbd.append(chr(-1 * self.stbd)) # 2
else:
if self.stbd == 125:
stbd.append(ESCAPE) # 0
stbd.append(chr(0x5D)) # 1
if self.stbd == 126:
stbd.append(ESCAPE) # 0
stbd.append(chr(0x5E)) # 1
if self.stbd == 127:
stbd.append(ESCAPE) # 0
stbd.append(chr(0x5F)) # 1
else:
stbd.append(chr(self.stbd)) # 0
command = [START_MESSAGE, VERT]
for b in vert:
command.append(b)
command.append(PORT)
for b in port:
command.append(b)
command.append(STBD)
for b in stbd:
command.append(b)
#TODO: Add Aux commands here.
#Add some complexity to the string
for i in range(2):
val = randint(0, 255)
command.append(chr(val))
crc = self.digest(command)
command.insert(1, chr(crc))
length = len(command)
length = length - 1
command.insert(2, chr(length))
# print crc
command.append(END_MESSAGE)
self.sendThrusterCommand(command)
elif "Button" in key:
pass
elif "Hat":
pass
else:
print(("Error in Data Keys!"))
def sendThrusterCommand(self, command):
if self.thrusters.isOpen():
# print((command))
for b in command:
self.thrusters.write(b)
else:
print(('Communication Error! - sendThrusterCommand'))
def _update(self, b):
self.sum = self.TABLE[self.sum^b]
def digest(self, st):
# print st
self.sum = 0
st = st[1:-1]
# print st
for ch in st:
self._update(ord(ch))
return self.sum
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment