class HQControlServer():

    def __init__(self):
        self.controller = Controller()
        self.coef = 58823530
        self.port = "50060"
        if len(sys.argv) > 1:
            self.port = sys.argv[1]
            int(self.port)

        self.context = zmq.Context()
        self.socket = self.context.socket(zmq.REP)
        self.socket.bind("tcp://*:%s" % self.port)

    def listen_commands(self):
        print "server is ready for commands"
        while True:
            #  Wait for next request from client
            message = self.socket.recv()

            direction, velocity = json.loads(message)

            if direction == 'forward':
                self.handleButtonFWD(velocity)
            if direction == 'back':
                self.handleButtonBack(velocity)
            if direction == 'left':
                self.handleButtonL(velocity)
            if direction == 'right':
                self.handleButtonR(velocity)
            if direction == 'stop':
                self.handleButtonSTP()
            self.socket.send("OK")

    def from_real_to_drive_vel(self, real):
            return float(real) * self.coef

    def handleButtonFWD(self, vel):
        self.controller.set_global_velocity(self.from_real_to_drive_vel(vel))
        self.controller.go_forward()

    def handleButtonBack(self, vel):
        self.controller.set_global_velocity(self.from_real_to_drive_vel(vel))
        self.controller.go_back()

    def handleButtonR(self, vel):
        self.controller.set_global_velocity(self.from_real_to_drive_vel(vel))
        self.controller.turn_right()

    def handleButtonL(self, vel):
        self.controller.set_global_velocity(self.from_real_to_drive_vel(vel))
        self.controller.turn_left()

    def handleButtonSTP(self):
        self.controller.stop_motor()
Beispiel #2
0
    def test_Controller(self, vel=5000000):
        test_object = Controller(self.port)
        self.start_info(test_object)

        test_object.set_global_velocity(vel/2)
        test_object.go_forward()
        time.sleep(2)
        test_object.set_global_velocity(vel/2)
        test_object.go_back()
        time.sleep(2)
        test_object.set_global_velocity(vel/5)
        test_object.turn_right()
        time.sleep(5)
        test_object.turn_left()
        time.sleep(5)
        test_object.stop_motor()
Beispiel #3
0
class RobotHQ(QtGui.QWidget):
    # TODO: 5 buttons, 2 text fields
    # GUI (forward/back/right/left/stop, velocity, distance) -> cmd
    def __init__(self, port=1, server=None):
        self.controller = Controller(port)
        self.server = server
        self.coef = 58823530
        self.app = QtGui.QApplication(sys.argv)
        super(RobotHQ, self).__init__()
        self.setGeometry(500, 500, 700, 300)
        self.setWindowTitle("Mobile Robot control")
        self.init_ui()
        self.center()
        if server:
            self.zmq_port = "50060"
            self.context = zmq.Context()
            print "Connecting to server..."
            self.socket = self.context.socket(zmq.REQ)
            self.socket.connect("tcp://%s:%s" % (self.server, self.zmq_port))

    def init_ui(self):
        btn_fwd = QtGui.QPushButton("Move Forward", self)
        btn_fwd.clicked.connect(self.handleButtonFWD)
        btn_fwd.resize(btn_fwd.sizeHint())
        btn_fwd.move(115, 10)
        btn_fwd.setFixedWidth(150)

        btn_back = QtGui.QPushButton("Back", self)
        btn_back.clicked.connect(self.handleButtonBack)
        btn_back.resize(btn_back.sizeHint())
        btn_back.move(115, 65)
        btn_back.setFixedWidth(150)

        btn_stop = QtGui.QPushButton("Stop", self)
        btn_stop.clicked.connect(self.handleButtonSTP)
        btn_stop.resize(btn_stop.sizeHint())
        btn_stop.move(115, 95)
        btn_stop.setFixedWidth(150)

        btn_left = QtGui.QPushButton("Turn Left", self)
        btn_left.clicked.connect(self.handleButtonL)
        btn_left.resize(btn_stop.sizeHint())
        btn_left.move(10, 37)
        btn_left.setFixedWidth(150)

        btn_right = QtGui.QPushButton("Turn Right", self)
        btn_right.clicked.connect(self.handleButtonR)
        btn_right.resize(btn_stop.sizeHint())
        btn_right.move(210, 37)
        btn_right.setFixedWidth(150)

        lb_vel = QtGui.QLabel("Velocity [m/s]", self)
        lb_vel.move(15, 135)
        self.le_vel = QtGui.QLineEdit("0.02", self)
        self.le_vel.move(10, 150)
        self.le_vel.setFixedWidth(150)

        lb_dis = QtGui.QLabel("Position", self)
        lb_dis.move(220, 135)
        self.le_pos = QtGui.QLineEdit(self)
        self.le_pos.move(215, 150)
        self.le_pos.setFixedWidth(150)

    def from_real_to_drive_vel(self, real):
        return float(real) * self.coef

    def handleButtonFWD(self):
        if self.server:
            cmd = []
            cmd.append("forward")
            cmd.append(str(self.le_vel.text()))
            v_send = json.dumps(cmd)
            self.socket.send(v_send)
            #  Get the reply.
            message = self.socket.recv()
            print message
        else:
            self.controller.set_global_velocity(self.from_real_to_drive_vel(self.le_vel.text()))
            if self.le_pos.text() != "":
                # TODO: implement self.controller.set_possition(int(self.le_pos.text()))
                pass
            self.controller.go_forward()

    def handleButtonBack(self):
        if self.server:
            cmd = []
            cmd.append("back")
            cmd.append(str(self.le_vel.text()))
            v_send = json.dumps(cmd)
            self.socket.send(v_send)
            #  Get the reply.
            message = self.socket.recv()
            print message
        else:
            self.controller.set_global_velocity(self.from_real_to_drive_vel(self.le_vel.text()))
            self.controller.go_back()

    def handleButtonR(self):
        if self.server:
            cmd = []
            cmd.append("right")
            cmd.append(str(self.le_vel.text()))
            v_send = json.dumps(cmd)
            self.socket.send(v_send)
            #  Get the reply.
            message = self.socket.recv()
            print message
        else:
            self.controller.set_global_velocity(self.from_real_to_drive_vel(self.le_vel.text()))
            self.controller.turn_right()

    def handleButtonL(self):
        if self.server:
            cmd = []
            cmd.append("left")
            cmd.append(str(self.le_vel.text()))
            v_send = json.dumps(cmd)
            self.socket.send(v_send)
            #  Get the reply.
            message = self.socket.recv()
            print message
        else:
            self.controller.set_global_velocity(self.from_real_to_drive_vel(self.le_vel.text()))
            self.controller.turn_left()

    def handleButtonSTP(self):
        if self.server:
            cmd = []
            cmd.append("stop")
            cmd.append(str(self.le_vel.text()))
            v_send = json.dumps(cmd)
            self.socket.send(v_send)
            #  Get the reply.
            message = self.socket.recv()
            print message
        else:
            self.controller.stop_motor()

    def closeEvent(self, event):
        reply = QtGui.QMessageBox.question(
            self, "Message", "Are you sure to quit?", QtGui.QMessageBox.Yes | QtGui.QMessageBox.No, QtGui.QMessageBox.No
        )

        if reply == QtGui.QMessageBox.Yes:
            event.accept()
        else:
            event.ignore()

    def center(self):
        qr = self.frameGeometry()
        cp = QtGui.QDesktopWidget().availableGeometry().center()
        qr.moveCenter(cp)
        self.move(qr.topLeft())

    def start_gui(self):
        self.show()
        sys.exit(self.app.exec_())