max/masters-project
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

e

Browse Source
This commit is contained in:
server 2021-06-01 14:57:40 +01:00
parent 44b45f4318
commit 9b9132ba31
1 changed files with 33 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
odrive_server_main.py
View File

@ -2,6 +2,7 @@ import odrive
from odrive.enums import * from odrive.enums import *
import time import time
import socket import socket
import atexit
class Odrive: class Odrive:
def __init__(self) -> None: def __init__(self) -> None:
@ -18,6 +19,9 @@ class Odrive:
self.drive.axis1.motor.config.current_lim = 70 self.drive.axis1.motor.config.current_lim = 70
self.drive.axis0.motor.config.current_lim_margin = 30 self.drive.axis0.motor.config.current_lim_margin = 30
self.drive.axis1.motor.config.current_lim_margin = 30 self.drive.axis1.motor.config.current_lim_margin = 30
self.drive.axis0.controller.config.vel_limit = 10
self.drive.axis1.controller.config.vel_limit = 10
def set_zero(self) -> None: def set_zero(self) -> None:
self.drive.axis0.encoder.set_linear_count(0) self.drive.axis0.encoder.set_linear_count(0)
@ -40,15 +44,17 @@ class Odrive:
class Server: class Server:
def __init__(self) -> None: def __init__(self) -> None:
self.armed = False self.armed = False
self.armed = True
self.x_offset = 0 self.x_offset = 0
self.x_multiplier = -98/10 # NOTE: Remove the /10 self.x_multiplier = -98/1.1 # -98/10 # NOTE: Remove the /10
self.y_offset = 0 self.y_offset = 0
self.y_multiplier = 85/10 # NOTE: Remove the /10 self.y_multiplier = 85/1.1 # 85/10 # NOTE: Remove the /10
self.sock = socket.socket(socket.AF_INET,socket.SOCK_DGRAM) self.sock = socket.socket(socket.AF_INET,socket.SOCK_DGRAM)
self.udp_host = socket.gethostname() self.udp_host = socket.gethostname()
self.udp_port = 59200 self.udp_port = 59200
self.sock.bind((self.udp_host, self.udp_port)) self.sock.bind((self.udp_host, self.udp_port))
self.init_odrive() self.init_odrive()
atexit.register(self.at_exit)
self.control_loop() self.control_loop()
def init_odrive(self) -> None: def init_odrive(self) -> None:
@ -56,11 +62,26 @@ class Server:
def control_loop(self) -> None: def control_loop(self) -> None:
while True: while True:
data, _ = self.sock.recvfrom(1024) try:
print(f"Received Message: {data}, type: {type(data)}") data_encoded, _ = self.sock.recvfrom(1024)
if self.armed: data_dirty = data_encoded.decode()
angle_0, angle_1 = self.calculate_angles(data[0], data[1]) data_clean = data_dirty.strip('(').strip(')').split(',')
self.drive.goto_raw(angle_0, angle_1) target = [0, 0]
target[0] = float(data_clean[0])
target[1] = float(data_clean[1])
if target[0] > 1 or target[0] < 0 or target[1] > 1 or target[1] < 0:
print(f"{target} outside 0 to 1 range!")
exit(2)
print(f"Received Message: {data_clean}, type: {type(data_clean)}")
print(f"Parsed Message: {target}, type: {type(target)}")
angle_0, angle_1 = self.calculate_angles(target[0], target[1])
print(f"angles: {angle_0}, {angle_1}")
if self.armed:
self.drive.goto_raw(angle_0, angle_1)
except Exception as e:
print(f"Exception: {e}")
self.drive.set_idle()
exit(1)
def calculate_angles(self, target_pos_x, target_pos_y) -> float: def calculate_angles(self, target_pos_x, target_pos_y) -> float:
x = (target_pos_x + self.x_offset) * self.x_multiplier x = (target_pos_x + self.x_offset) * self.x_multiplier
@ -73,3 +94,8 @@ class Server:
target_angle_1 = (y / distance_per_revolution) + (x / distance_per_revolution) target_angle_1 = (y / distance_per_revolution) + (x / distance_per_revolution)
return target_angle_0, target_angle_1 return target_angle_0, target_angle_1
def at_exit(self):
self.drive.set_idle()
s = Server()