diff --git a/Server/Server-Code-2019/GPIO.py b/Server/Server-Code-2019/GPIO.py index 66c35ae..eb60616 100644 --- a/Server/Server-Code-2019/GPIO.py +++ b/Server/Server-Code-2019/GPIO.py @@ -1,5 +1,5 @@ import RPi.GPIO as GPIO -from time import * +import time pin = 20 @@ -12,6 +12,7 @@ def restart(): GPIOsetup() GPIO.output(pin,1) print("[Dynamixels]: OFF") + time.sleep(3) GPIO.output(pin,0) print("[Dynamixels]: ON") return diff --git a/Server/Server-Code-2019/NewRoboCupServer-Current.py b/Server/Server-Code-2019/NewRoboCupServer-Current.py new file mode 100644 index 0000000..0c37d6d --- /dev/null +++ b/Server/Server-Code-2019/NewRoboCupServer-Current.py @@ -0,0 +1,77 @@ +import socketserver as SocketServer +import sys +from GPIO import * +from emuBot import * +from xl320 import * + +numberOfActiveDynamixels = 13 + +def loopServoReset(setup): + for ID in range(1,numberOfActiveDynamixels+1): + try: + softwareResetServo(ID) + if ID == numberOfActiveDynamixels: + print("---------- // -----------") + if setup: + if (ID <= 4) or ID == 11: + wheelMode(ID) + elif ID == 12 or ID == 13: + cameraMode(ID) + else: + jointMode(ID) + except: + print("Setup of ID: " + str(ID) + " failed.") + +loopServoReset(False)#True) +cameraMode(12) +cameraMode(13) +lastValues = [0 for i in range(20)] + + +class MyUCPHandler(SocketServer.BaseRequestHandler): + def handle(self): + self.data = self.request[0].decode('utf-8').strip() + new = self.data.split('\n') + for i in new: + if i != "": + if 0: + print(new) + else: + if i == "restart": + restart() + elif i == "softwareResetServos": + loopServoReset(False) + else: + ID = int(i[0]+i[1]) + if (ID <= 4) or (ID == 11): + try: + if lastValues[ID] != int(i[2:]): + moveWheel(ID, int(i[2:])) + lastValues[ID] = int(i[2:]) + except: + print('brokeWheel ',ID) + else: + pos, speed = map(int, [round(float(x)) for x in i.split()[1:]]) + if ID == 12 or ID == 13: + try: + moveCamera(ID, pos) + except: + print("BrokeCamera", ID) + else: + try: + moveJoint(ID, pos, speed) + except: + print(i[2:-4]) + print('BrokeJoint ', ID) + # print(i) + # else: + # print(i) + +if __name__ == "__main__": + HOST, PORT = "", 9999 + +SocketServer.UDPServer.allow_reuse_address = True +server = SocketServer.UDPServer((HOST, PORT), MyUCPHandler) + +print('Dynamixel Server Started') +server.serve_forever() diff --git a/Server/Server-Code-2019/RoboCupServer-Current.py b/Server/Server-Code-2019/RoboCupServer-Current.py index 8bb2c98..76e6f31 100644 --- a/Server/Server-Code-2019/RoboCupServer-Current.py +++ b/Server/Server-Code-2019/RoboCupServer-Current.py @@ -21,7 +21,7 @@ def loopServoReset(setup): loopServoReset(True) -lastValues = [0 for i in range(5)] +lastValues = [0 for i in range(20)] class MyUCPHandler(SocketServer.BaseRequestHandler): @@ -44,8 +44,7 @@ def handle(self): if lastValues[ID] != int(i[2:]): moveWheel(ID, int(i[2:])) lastValues[ID] = int(i[2:]) - except e: - print(e) + except: print('brokeWheel ',ID) else: pos, speed = map(int, [round(float(x)) for x in i.split()[1:]]) diff --git a/Server/Server-Code-2019/__pycache__/GPIO.cpython-35.pyc b/Server/Server-Code-2019/__pycache__/GPIO.cpython-35.pyc index 0328bf7..1a1bb7c 100644 Binary files a/Server/Server-Code-2019/__pycache__/GPIO.cpython-35.pyc and b/Server/Server-Code-2019/__pycache__/GPIO.cpython-35.pyc differ diff --git a/Server/Server-Code-2019/__pycache__/emuBot.cpython-35.pyc b/Server/Server-Code-2019/__pycache__/emuBot.cpython-35.pyc index 2643818..ddbcb21 100644 Binary files a/Server/Server-Code-2019/__pycache__/emuBot.cpython-35.pyc and b/Server/Server-Code-2019/__pycache__/emuBot.cpython-35.pyc differ diff --git a/Server/Server-Code-2019/__pycache__/xl320.cpython-35.pyc b/Server/Server-Code-2019/__pycache__/xl320.cpython-35.pyc new file mode 100644 index 0000000..74dfc54 Binary files /dev/null and b/Server/Server-Code-2019/__pycache__/xl320.cpython-35.pyc differ diff --git a/Server/Server-Code-2019/camera.py b/Server/Server-Code-2019/camera.py new file mode 100644 index 0000000..95c11ae --- /dev/null +++ b/Server/Server-Code-2019/camera.py @@ -0,0 +1,129 @@ +import os, sys + +if os.name == 'nt': + import msvcrt + def getch(): + return msvcrt.getch().decode() +else: + import tty, termios + fd = sys.stdin.fileno() + old_settings = termios.tcgetattr(fd) + tty.setraw(sys.stdin.fileno()) + def getch(): + return sys.stdin.read(1) + +os.sys.path.append('../dynamixel_functions_py') # Path setting + +import dynamixel_functions as dynamixel # Uses Dynamixel SDK library + +# Control table address +ADDR_PRO_TORQUE_ENABLE = 24 # Control table address is different in Dynamixel model +ADDR_PRO_GOAL_POSITION = 30 +ADDR_PRO_PRESENT_POSITION = 37 + +# Protocol version +PROTOCOL_VERSION = 2 # See which protocol version is used in the Dynamixel + +# Default setting +DXL_ID = 13 # Dynamixel ID: 1 +BAUDRATE = 1000000 +DEVICENAME = "/dev/ttyUSB0".encode('utf-8') # Check which port is being used on your controller + # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" + +TORQUE_ENABLE = 1 # Value for enabling the torque +TORQUE_DISABLE = 0 # Value for disabling the torque +DXL_MINIMUM_POSITION_VALUE = 205 # Dynamixel will rotate between this value +DXL_MAXIMUM_POSITION_VALUE = 819 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) +DXL_MOVING_STATUS_THRESHOLD = 20 # Dynamixel moving status threshold + +ESC_ASCII_VALUE = 0x1b + +COMM_SUCCESS = 0 # Communication Success result value +COMM_TX_FAIL = -1001 # Communication Tx Failed + +# Initialize PortHandler Structs +# Set the port path +# Get methods and members of PortHandlerLinux or PortHandlerWindows +port_num = dynamixel.portHandler(DEVICENAME) + +# Initialize PacketHandler Structs +dynamixel.packetHandler() + +index = 0 +dxl_comm_result = COMM_TX_FAIL # Communication result +dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE] # Goal position + +dxl_error = 0 # Dynamixel error +dxl_present_position = 0 # Present position + +# Open port +if dynamixel.openPort(port_num): + print("Succeeded to open the port!") +else: + print("Failed to open the port!") + print("Press any key to terminate...") + getch() + quit() + +# Set port baudrate +if dynamixel.setBaudRate(port_num, BAUDRATE): + print("Succeeded to change the baudrate!") +else: + print("Failed to change the baudrate!") + print("Press any key to terminate...") + getch() + quit() + + +# Enable Dynamixel Torque +dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE) +if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS: + dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) +elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) != 0: + dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) +else: + print("Dynamixel has been successfully connected") + + +while 1: + print("Press any key to continue! (or press ESC to quit!)") + if getch() == chr(ESC_ASCII_VALUE): + break + + # Write goal position + dynamixel.write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_GOAL_POSITION, dxl_goal_position[index]) + if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS: + dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) + elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) != 0: + dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) + + while 1: + # Read present position + dxl_present_position = dynamixel.read4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_PRESENT_POSITION) + if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS: + dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) + elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) != 0: + dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) + + print("[ID:%03d] GoalPos:%03d PresPos:%03d" % (DXL_ID, dxl_goal_position[index], dxl_present_position)) + + if not (abs(dxl_goal_position[index] - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD): + break + + # Change goal position + if index == 0: + index = 1 + else: + index = 0 + + +# Disable Dynamixel Torque +dynamixel.write1ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE) +if dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION) != COMM_SUCCESS: + dynamixel.printTxRxResult(PROTOCOL_VERSION, dynamixel.getLastTxRxResult(port_num, PROTOCOL_VERSION)) +elif dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION) != 0: + dynamixel.printRxPacketError(PROTOCOL_VERSION, dynamixel.getLastRxPacketError(port_num, PROTOCOL_VERSION)) + + +# Close port +dynamixel.closePort(port_num) diff --git a/Server/Server-Code-2019/emuBot.py b/Server/Server-Code-2019/emuBot.py index 8a72317..97409de 100644 --- a/Server/Server-Code-2019/emuBot.py +++ b/Server/Server-Code-2019/emuBot.py @@ -5,7 +5,7 @@ BAUDRATE = 1000000 -portHandler = PortHandler("/dev/ttyUSB0") +portHandler = PortHandler("/dev/ttyUSB2") packetHandler = PacketHandler(1.0) if portHandler.openPort(): diff --git a/Server/Server-Code-2019/xl320.py b/Server/Server-Code-2019/xl320.py new file mode 100644 index 0000000..52ce1cb --- /dev/null +++ b/Server/Server-Code-2019/xl320.py @@ -0,0 +1,139 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +################################################################################ +# Copyright 2017 ROBOTIS CO., LTD. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +################################################################################ + +# Author: Ryu Woon Jung (Leon) + +# +# ********* Read and Write Example ********* +# +# +# Available Dynamixel model on this example : All models using Protocol 2.0 +# This example is designed for using a Dynamixel PRO 54-200, and an USB2DYNAMIXEL. +# To use another Dynamixel model, such as X series, see their details in E-Manual(emanual.robotis.com) and edit below variables yourself. +# Be sure that Dynamixel PRO properties are already set as %% ID : 1 / Baudnum : 1 (Baudrate : 57600) +# + +import os +from dynamixel_sdk import * # Uses Dynamixel SDK library + +# Control table address +ADDR_PRO_TORQUE_ENABLE = 24 # Control table address is different in Dynamixel model +ADDR_PRO_GOAL_POSITION = 30 +ADDR_PRO_PRESENT_POSITION = 37 + +# Protocol version +PROTOCOL_VERSION = 2.0 # See which protocol version is used in the Dynamixel + +# Default setting +DXL_ID = 13 # Dynamixel ID : 1 +BAUDRATE = 1000000 # Dynamixel default baudrate : 57600 +DEVICENAME = '/dev/ttyUSB0' # Check which port is being used on your controller + # ex) Windows: "COM1" Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*" + +TORQUE_ENABLE = 1 # Value for enabling the torque +TORQUE_DISABLE = 0 # Value for disabling the torque +DXL_MINIMUM_POSITION_VALUE = 205 # Dynamixel will rotate between this value +DXL_MAXIMUM_POSITION_VALUE = 819 # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.) +DXL_MOVING_STATUS_THRESHOLD = 20 # Dynamixel moving status threshold + +index = 0 +dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE] # Goal position + + +# Initialize cameraPortHandler instance +# Set the port path +# Get methods and members of cameraPortHandlerLinux or cameraPortHandlerWindows +cameraPortHandler = PortHandler(DEVICENAME) + +# Initialize PacketHandler instance +# Set the protocol version +# Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler +packetHandler = PacketHandler(PROTOCOL_VERSION) + +# Open port +if cameraPortHandler.openPort(): + #print("Succeeded to open the port") + pass +else: + print("Failed to open the port") + print("Press any key to terminate...") + getch() + quit() + + +# Set port baudrate +if cameraPortHandler.setBaudRate(BAUDRATE): + #print("Succeeded to change the baudrate") + pass +else: + print("Failed to change the baudrate") + print("Press any key to terminate...") + getch() + quit() + +ID = 12 +goal_position = 500 +def cameraMode(ID): + # Enable Dynamixel Torque + dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(cameraPortHandler, ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE) + if dxl_comm_result != COMM_SUCCESS: + print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) + elif dxl_error != 0: + print("%s" % packetHandler.getRxPacketError(dxl_error)) + else: + pass + #print("Dynamixel has been successfully connected") + +cameraMode(12) +cameraMode(13) +def moveCamera(ID, goal_position): + #print("Press any key to continue! (or press ESC to quit!)") + #if getch() == chr(0x1b): + #break + # Write goal position + dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(cameraPortHandler, ID, ADDR_PRO_GOAL_POSITION, goal_position) + if dxl_comm_result != COMM_SUCCESS: + print("111%s" % packetHandler.getTxRxResult(dxl_comm_result)) + elif dxl_error != 0: + print("222%s" % packetHandler.getRxPacketError(dxl_error)) + + # Read present position + dxl_present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(cameraPortHandler, ID, ADDR_PRO_PRESENT_POSITION) + #if dxl_comm_result != COMM_SUCCESS: + #print("333%s" % packetHandler.getTxRxResult(dxl_comm_result)) + if dxl_error != 0: + print("444%s" % packetHandler.getRxPacketError(dxl_error)) + + #print("555[ID:%03d] GoalPos:%03d PresPos:%03d" % (DXL_ID, dxl_goal_position[index], dxl_present_position)) + + # if not abs(goal_position - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD: + # print('Broken :(') + + +# moveCamera(12, 1023) +# moveCamera(13, 500) +# # Disable Dynamixel Torque +# dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(cameraPortHandler, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE) +# if dxl_comm_result != COMM_SUCCESS: +# print("666%s" % packetHandler.getTxRxResult(dxl_comm_result)) +# elif dxl_error != 0: +# print("777%s" % packetHandler.getRxPacketError(dxl_error)) + +# # Close port +# cameraPortHandler.closePort()