#!/usr/bin/env python
import socket
from hedgehog.client import connect
print("run")
IP = "" # blank if you want it to listen to every adress - clients ip if it should only listen to the client
PORT = 1337
# garbage but i dont care
with connect(emergency=15) as hedgehog:
server_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
server_socket.bind((IP, PORT))
server_socket.listen(1)
while True:
(client_socket, addr) = server_socket.accept()
msg = client_socket.recv(1024)
#print(str(msg))
test = str(msg)
isL = False
r = ""
l = ""
i = 2
while (i < len(test) - 1):
#print(test[i])
if (str(test[i]) == "."):
second_arm_rotation_speed = 0.0
second_arm_rotation_speed_manual = 0.0
second_arm_rotation_target_speed = 1.0
wrist_speed = 0.0
wrist_speed_manual = 0.0
wrist_target_speed = 0.0
wrist_rotation_speed = 0.0
wrist_rotation_speed_manual = 0.0
wrist_rotation_target_speed = 0.0
acceleration = 0.000873 # 0.05° per frame (30 frames or 0.5 sec to top speed)
deceleration = acceleration
with connect('tcp://localhost:10789', emergency=15) as hedgehog:
with connect('tcp://hedgehog13.local:10789', emergency=15) as hedgehog2:
hedgehog2.set_servo(base, True, hu.from_rad(base_position))
hedgehog2.set_servo(main_arm, True, hu.from_rad(main_arm_position))
hedgehog2.set_servo(second_arm, True, hu.from_rad(second_arm_position))
hedgehog.set_servo(second_arm_r, True, hu.from_rad(second_arm_rotation))
hedgehog.set_servo(wrist_r, True, hu.from_rad(wrist_rotation))
hedgehog.set_servo(wrist, True, hu.from_rad(wrist_position))
hedgehog.set_servo(gripper, True, hu.from_rad(gripper_position))
def shutdown(signal, frame):
client.loop_stop(force=True)
print("stopping servos ...")
hedgehog.set_servo(0, False, 1)
hedgehog.set_servo(1, False, 1)
def __init__(self, parent=None):
super(ECUI, self).__init__(parent)
# Settings
self.autoabortEnabled = True
# Hedgehog
self.stack = ExitStack()
self.hedgehog = self.stack.enter_context(
connect(endpoint='tcp://raspberrypi.local:10789')) # FIXME
# Simulated Hedgehog
#self.hedgehog = SimulatedHedgehog()
#Strain data received
self.measure = StrainData()
#Neopixel test
self.socket_neo(b'SafeOn')
# Actuators and Sensors
self.servo_fuel = Servo(name='fuel',
hedgehog=self.hedgehog,
servoPort=0,
feedbackPort=0)
self.servo_oxidizer = Servo(name='oxidizer',
hedgehog=self.hedgehog,
servoPort=1,
feedbackPort=1)
self.igniter_arc = Igniter(name='arc',
hedgehog=self.hedgehog,
igniterPort=0)
self.igniter_pyro = Igniter(name='pyro',
hedgehog=self.hedgehog,
igniterPort=1,
feedbackPort=2)
self.pressureSensor_fuel = PressureSensor(name='fuel',
hedgehog=self.hedgehog,
port=8)
self.pressureSensor_oxidizer = PressureSensor(name='oxidizer',
hedgehog=self.hedgehog,
port=9)
self.pressureSensor_chamber = PressureSensor(name='chamber',
hedgehog=self.hedgehog,
port=10)
self.temperatureSensor_chamber = TemperatureSensor(
name='chamber', hedgehog=self.hedgehog, port=11)
# Countdown Timer and Sequence
self.countdownTimer = CountdownTimer(self.countdownEvent)
self.sequence = Sequence()
# Regular Timer
self.timer = QTimer()
self.timer.setInterval(100)
self.timer.timeout.connect(self.__timerTick)
self.inputVoltage = 0.0
self.set_safe = True
self.set_igniter = False
self.loggingValues = []
# Main tab
self.tab_main = QWidget()
self.tab_main.layout = QVBoxLayout(self)
self.tab_main.setLayout(self.tab_main.layout)
self.label_countdownClock = QLabel(self.countdownTimer.getTimeString(),
self)
self.label_countdownClock.setStyleSheet('color: #000000')
self.label_countdownClock.setToolTip("Countdown Clock")
self.label_inputVoltage = QLabel("Input Voltage: 0.0V", self)
self.label_inputVoltage.setStyleSheet('color: #000000')
self.label_inputVoltage.setToolTip("Input Voltage")
self.btn_countdownStartStop = QPushButton("Start", self)
self.btn_countdownStartStop.setToolTip("Start the Countdown")
self.btn_countdownStartStop.clicked.connect(
self.countdownStartStopReset)
self.btn_countdownStartStop.resize(
self.btn_countdownStartStop.sizeHint())
self.btn_countdownStartStop.setStyleSheet('background-color: #00ff00;')
self.layout_countdown = QHBoxLayout()
self.layout_countdown.addWidget(self.label_inputVoltage,
alignment=Qt.AlignLeft)
self.layout_countdown.addWidget(self.label_countdownClock,
alignment=Qt.AlignCenter)
self.layout_countdown.addWidget(self.btn_countdownStartStop,
alignment=Qt.AlignRight)
self.tab_main.layout.addLayout(self.layout_countdown)
self.sequencePlot = SequencePlot(self,
sequence=self.sequence,
countdownTimer=self.countdownTimer,
width=5,
height=4)
self.tab_main.layout.addWidget(self.sequencePlot,
alignment=Qt.AlignTop)
self.checkbox_manualControl = QCheckBox("Manual Control", self)
self.checkbox_manualControl.setToolTip("Enable Manual Control")
self.checkbox_manualControl.clicked.connect(
self.manualControlEnableDisable)
self.checkbox_manualControl.resize(
self.checkbox_manualControl.sizeHint())
self.checkbox_manualControlIgniter = QCheckBox("Igniter", self)
self.checkbox_manualControlIgniter.setToolTip("Igniter Manual Control")
self.checkbox_manualControlIgniter.clicked.connect(
self.manualControlIgniterEnableDisable)
self.checkbox_manualControlIgniter.resize(
self.checkbox_manualControlIgniter.sizeHint())
self.checkbox_manualControlIgniter.setEnabled(False)
self.label_manualControlFuel = QLabel(
"Fuel Target: %3d%% Fuel Currently: %3d%% Fuel Pressure: %2.1fbar"
% (self.servo_fuel.getPositionTargetPercent(),
self.servo_fuel.getPositionCurrentPercent(),
self.pressureSensor_fuel.getValue()), self)
self.slider_manualControlFuel = QSlider(
Qt.Horizontal
) # FIXME: buggy when slided manually, goes out of range or doesnt reach limits
self.slider_manualControlFuel.setToolTip("Fuel Manual Control")
self.slider_manualControlFuel.setRange(0, 100)
self.slider_manualControlFuel.sliderMoved.connect(
self.manualControlFuelChange)
self.slider_manualControlFuel.resize(
self.slider_manualControlFuel.sizeHint())
self.slider_manualControlFuel.setEnabled(False)
self.layout_manualControlFuel = QVBoxLayout()
self.layout_manualControlFuel.addWidget(self.label_manualControlFuel,
alignment=Qt.AlignLeft)
self.layout_manualControlFuel.addWidget(self.slider_manualControlFuel,
alignment=Qt.AlignTop)
self.label_manualControlOxidizer = QLabel(
"Oxidizer Target: %3d%% Oxidizer Currently: %3d%% Oxidizer Pressure: %2.1fbar"
% (self.servo_oxidizer.getPositionTargetPercent(),
self.servo_oxidizer.getPositionCurrentPercent(),
self.pressureSensor_oxidizer.getValue()), self)
self.slider_manualControlOxidizer = QSlider(
Qt.Horizontal
) # FIXME: buggy when slided manually, goes out of range or doesnt reach limits
self.slider_manualControlOxidizer.setToolTip("Oxidizer Manual Control")
self.slider_manualControlOxidizer.setRange(0, 100)
self.slider_manualControlOxidizer.sliderMoved.connect(
self.manualControlOxidizerChange)
self.slider_manualControlOxidizer.resize(
self.slider_manualControlOxidizer.sizeHint())
self.slider_manualControlOxidizer.setEnabled(False)
self.layout_manualControlOxidizer = QVBoxLayout()
self.layout_manualControlOxidizer.addWidget(
self.label_manualControlOxidizer, alignment=Qt.AlignLeft)
self.layout_manualControlOxidizer.addWidget(
self.slider_manualControlOxidizer, alignment=Qt.AlignTop)
self.layout_manualControl = QVBoxLayout()
self.layout_manualControl.addWidget(self.checkbox_manualControl,
alignment=Qt.AlignLeft)
self.layout_manualControl.addWidget(self.checkbox_manualControlIgniter,
alignment=Qt.AlignLeft)
self.layout_manualControl.addLayout(self.layout_manualControlFuel)
self.layout_manualControl.addLayout(self.layout_manualControlOxidizer)
self.tab_main.layout.addLayout(self.layout_manualControl)
# Settings tab
self.tab_settings = QWidget()
self.tab_settings.layout = QHBoxLayout(self)
self.tab_settings.setLayout(self.tab_settings.layout)
self.checkbox_calibration = QCheckBox("Servo Calibration", self)
self.checkbox_calibration.setToolTip("Enable Servo Calibration Mode")
self.checkbox_calibration.clicked.connect(
self.calibrationEnableDisable)
self.checkbox_calibration.resize(self.checkbox_calibration.sizeHint())
self.label_cal_fuel = QLabel(
"Fuel (min=%dus, max=%dus)" %
(self.servo_fuel.getMinUs(), self.servo_fuel.getMaxUs()), self)
self.spinbox_cal_fuel = QSpinBox(self)
self.spinbox_cal_fuel.setMinimum(500)
self.spinbox_cal_fuel.setMaximum(2500)
self.spinbox_cal_fuel.setSingleStep(1)
self.spinbox_cal_fuel.setValue(self.servo_fuel.getPositionTargetUs())
self.spinbox_cal_fuel.setEnabled(False)
self.spinbox_cal_fuel.valueChanged.connect(self.calFuelValueChanged)
self.btn_cal_fuel_min = QPushButton("Set Min", self)
self.btn_cal_fuel_min.setToolTip(
"Set the Minimum Value, corresponds to 0%")
self.btn_cal_fuel_min.clicked.connect(self.calFuelSetMin)
self.btn_cal_fuel_min.setEnabled(False)
self.btn_cal_fuel_min.resize(self.btn_cal_fuel_min.sizeHint())
self.btn_cal_fuel_max = QPushButton("Set Max", self)
self.btn_cal_fuel_max.setToolTip(
"Set the Maximum Value, corresponds to 100%")
self.btn_cal_fuel_max.clicked.connect(self.calFuelSetMax)
self.btn_cal_fuel_max.setEnabled(False)
self.btn_cal_fuel_max.resize(self.btn_cal_fuel_max.sizeHint())
self.label_cal_oxidizer = QLabel(
"Oxidizer (min=%dus, max=%dus)" %
(self.servo_oxidizer.getMinUs(), self.servo_oxidizer.getMaxUs()),
self)
self.spinbox_cal_oxidizer = QSpinBox(self)
self.spinbox_cal_oxidizer.setMinimum(500)
self.spinbox_cal_oxidizer.setMaximum(2500)
self.spinbox_cal_oxidizer.setSingleStep(1)
self.spinbox_cal_oxidizer.setValue(
self.servo_oxidizer.getPositionTargetUs())
self.spinbox_cal_oxidizer.setEnabled(False)
self.spinbox_cal_oxidizer.valueChanged.connect(
self.calOxidizerValueChanged)
self.btn_cal_oxidizer_min = QPushButton("Set Min", self)
self.btn_cal_oxidizer_min.setToolTip(
"Set the Minimum Value, corresponds to 0%")
self.btn_cal_oxidizer_min.clicked.connect(self.calOxidizerSetMin)
self.btn_cal_oxidizer_min.setEnabled(False)
self.btn_cal_oxidizer_min.resize(self.btn_cal_oxidizer_min.sizeHint())
self.btn_cal_oxidizer_max = QPushButton("Set Max", self)
self.btn_cal_oxidizer_max.setToolTip(
"Set the Maximum Value, corresponds to 100%")
self.btn_cal_oxidizer_max.clicked.connect(self.calOxidizerSetMax)
self.btn_cal_oxidizer_max.setEnabled(False)
self.btn_cal_oxidizer_max.resize(self.btn_cal_oxidizer_max.sizeHint())
self.layout_calibration_fuel = QHBoxLayout()
self.layout_calibration_fuel.addWidget(self.label_cal_fuel,
alignment=Qt.AlignTop)
self.layout_calibration_fuel.addWidget(self.spinbox_cal_fuel,
alignment=Qt.AlignTop)
self.layout_calibration_fuel.addWidget(self.btn_cal_fuel_min,
alignment=Qt.AlignTop)
self.layout_calibration_fuel.addWidget(self.btn_cal_fuel_max,
alignment=Qt.AlignTop)
self.layout_calibration_oxidizer = QHBoxLayout()
self.layout_calibration_oxidizer.addWidget(self.label_cal_oxidizer,
alignment=Qt.AlignTop)
self.layout_calibration_oxidizer.addWidget(self.spinbox_cal_oxidizer,
alignment=Qt.AlignTop)
self.layout_calibration_oxidizer.addWidget(self.btn_cal_oxidizer_min,
alignment=Qt.AlignTop)
self.layout_calibration_oxidizer.addWidget(self.btn_cal_oxidizer_max,
alignment=Qt.AlignTop)
self.layout_calibration = QVBoxLayout()
self.layout_calibration.addWidget(self.checkbox_calibration,
alignment=Qt.AlignTop)
self.layout_calibration.addLayout(self.layout_calibration_fuel)
self.layout_calibration.addLayout(self.layout_calibration_oxidizer)
self.tab_settings.layout.addLayout(self.layout_calibration)
# Tabs
self.tabs = QTabWidget()
self.tabs.resize(300, 200)
self.tabs.addTab(self.tab_main, "Main")
self.tabs.addTab(self.tab_settings, "Settings")
# Window
self.layout = QVBoxLayout(self)
self.layout.addWidget(self.tabs, alignment=Qt.AlignTop)
self.setLayout(self.layout)
self.setGeometry(500, 200, 900, 700)
self.setWindowTitle("Engine Control UI")
self.setWindowIcon(QIcon('icon.png'))
#Timer
self.timer.start()
#creates socket to receive messages
port = '733' + str(hedgehogId)
context = zmq.Context()
recvSocket = context.socket(zmq.PAIR)
recvSocket.bind('tcp://*:%s' % port)
#creates poller to create non blocking receive
poller = zmq.Poller()
poller.register(recvSocket, zmq.POLLIN)
controllerData = {}
#hedgehog data
if onHedgehog:
hedgehog = connect(emergency=15)
leftWheelPort = 0
rightWheelPort = 1
standardSpeed = 500
stopTimer = 2000
speeds = {'left': 0, 'right': 0}
#time deltas
lastControllerPacket = ''
lastReset = ''
def deltaTime(otherTime) -> int:
now = datetime.now()
if otherTime != '':
delta = now - otherTime
#creates socket to receive messages
port = '733' + str(hedgehogId)
context = zmq.Context()
recvSocket = context.socket(zmq.PAIR)
recvSocket.bind('tcp://*:%s' % port)
#creates poller to create non blocking receive
poller = zmq.Poller()
poller.register(recvSocket, zmq.POLLIN)
controllerData = {}
with ExitStack() as stack:
#hedgehog data
if onHedgehog:
hedgehog = stack.enter_context(connect(emergency=15))
leftWheelPort = 0
rightWheelPort = 1
standardSpeed = 1000
stopTimer = 2000
speeds = {'left':0,'right':0}
#time deltas
lastControllerPacket = ''
lastReset = ''
def deltaTime(otherTime) -> int:
now = datetime.now()
if otherTime != '':
delta = now - otherTime
delta = int(delta.total_seconds() * 1000)
def test_connect_with_emergency_shutdown(self):
ctx = zmq.Context()
with HedgehogServer(ctx, 'inproc://controller', handler()):
with connect('inproc://controller', emergency=0,
ctx=ctx) as client:
self.assertEqual(client.get_analog(0), 0)