Skip to content

Commit

Permalink
Browse files Browse the repository at this point in the history
servos
  • Loading branch information
cavallialexander committed Jun 21, 2021
1 parent a772d5f commit a282342
Show file tree
Hide file tree
Showing 10 changed files with 654 additions and 0 deletions.
150 changes: 150 additions & 0 deletions configs/servo_config.json
@@ -0,0 +1,150 @@
{
"network": {
"ip": "*"
},
"control": {
"default_speed": 3
},
"motors": {
"type": "virtual"
},
"servos": {
"type": "maestro",
"instances": {
"0": {
"speed": 0.17,
"neutral": 0
},
"1": {
"speed": 0.17,
"neutral": "0"
},
"2": {
"speed": 0.17,
"neutral": 0
},
"3": {
"speed": 0.17,
"neutral": 0
}
}
},
"arm": {
"shoulder": "0",
"elbow": "1",
"wrist": "2",
"gripper": "3"
},
"interface": {
"notifications": {
"enabled": true,
"timeout": 7
},
"cameras": {
"front": {
"enabled": true,
"id": 1
},
"back": {
"enabled": false
},
"left": {
"enabled": false
},
"right": {
"enabled": false
}
},
"graphs": [
{
"uid": "cpu_temperature",
"type": "circle",
"enabled": true,
"location": "#left_view_sensors",
"title": "CPU Temp.",
"unit": "°C",
"unit_style": "font-size: 12px;"
},
{
"uid": "memory_usage",
"type": "circle",
"enabled": true,
"location": "#left_view_sensors",
"title": "RAM Usage",
"unit": " MB",
"unit_style": "font-size: 12px;"
},
{
"uid": "disk_usage",
"type": "circle",
"enabled": true,
"location": "#right_view_sensors",
"title": "Disk Usage",
"unit": " GB",
"unit_style": "font-size: 12px;"
},
{
"uid": "cpu_usage",
"type": "circle",
"enabled": true,
"location": "#right_view_sensors",
"title": "CPU Usage",
"unit": "%",
"unit_style": "font-size: 15px;"
},
{
"uid": "uptime",
"type": "uptime",
"enabled": true,
"location": "#textgroup_left",
"title": "Uptime"
}
],
"theme": {
"accent_colour": "#ff5a00"
}
},
"sensors": [
{
"enabled": true,
"type": "cpu_temp",
"name": "CPU Temperature",
"period": 1,
"display_on": [
"cpu_temperature"
]
},
{
"enabled": true,
"type": "memory",
"name": "RAM Usage",
"period": 1,
"display_on": [
"memory_usage"
]
},
{
"enabled": true,
"type": "cpu_usage",
"name": "CPU Usage",
"period": 1,
"display_on": [
"cpu_usage"
]
},
{
"enabled": true,
"type": "disk_usage",
"name": "Disk Usage",
"precision": 1,
"period": 60,
"display_on": [
"disk_usage"
]
}
],
"debug": {
"log_level": "info",
"print_messages": false
}
}
1 change: 1 addition & 0 deletions src/opt
Submodule opt added at 5c14a0
115 changes: 115 additions & 0 deletions src/servo_handler.py
@@ -0,0 +1,115 @@
#!/usr/bin/env python3
import logging
from time import perf_counter
import serial
import os
import traceback
from multiprocessing import Pipe
from websocket_process import WebSocketProcess
import asyncio as aio
from plugin_system import PluginManager
from servo_wrapper import ServoWrapper, ServoModel
from servos.virtual import VirtualConnection
from typing import List, Optional


class ServoBackgroundService(WebSocketProcess):
def __init__(self, mpid, pipe, connection, servos: List[ServoModel]):
WebSocketProcess.__init__(self, mpid, pipe, "src/configs/sights/minimal.json", 5557)
self.logger = logging.getLogger(__name__)
self.moving_servos = dict()
self.servos = dict([(s.channel, s) for s in servos])
self.connection = connection

def set_speed(self, channel, vel):
if vel == 0:
self.stop([channel])
else:
self.moving_servos[channel] = int(vel*self.servos[channel].speed)

def stop(self, channels=None):
if channels is None:
self.moving_servos = dict()
else:
for channel in channels:
self.moving_servos.pop(channel, None)

async def main(self, websocket, path):
self.t = -10000000000000000000000
# Enter runtime loop
while True:
if websocket.poll():
if message[0] == "SPEED":
self.logger.info(f"Set Speed: ch {message[1]} @ {message[2]}")
self.set_speed(message[1], message[2])
elif message[0] == "STOP":
self.logger.info("STOP")
self.stop(message[1])

if (perf_counter()-self.t)* 1_000 > 10:
self.logger.info(f"Updating {self.moving_servos}")
for channel, vel in self.moving_servos.items():
self.servos[channel].pos = self.connection.go_to(self.servos[channel].pos +
vel)
self.t = perf_counter()


class ServoHandler:
def __init__(self, config):
# Setup logger
self.logger = logging.getLogger(__name__)
# Create new plugin manager looking for subclasses of MotorWrapper in "src/motors/"
self.pm = PluginManager(ServoWrapper, os.getcwd() + "/src/servos")
# Load values from configuration file
self.type = config['servos']['type'].lower()
# Log loaded type
self.logger.info(f"Opening servo connection of type '{self.type}'")
try:
# Create servo connection (from a list loaded by the plugin manager) using
# class specified in the config
self.connection: ServoWrapper = self.pm.wrappers[self.type](config['servos'])
except Exception as e:
if isinstance(e, KeyError):
self.logger.error(f"Could not determine servo connection type "
f"'{self.type}'")
elif isinstance(e, serial.serialutil.SerialException):
self.logger.error(f"Could not open servo connection of type '{self.type}'")
else:
traceback.print_exc()
# Fall back to virtual connection common to all servo connection errors
self.logger.warning("Falling back to virtual connection")
self.connection = VirtualConnection(config['servos'])
self.type = 'virtual'
# Ensure Connection class has access to logging capabilities
self.connection.logger = self.logger
Servos = []
for servo, conf in config['servos']['instances'].items():
Servos.append(self.connection.create_servo_model(int(servo), conf))
piper, self.pipe = Pipe(duplex=False)
self.background_service = ServoBackgroundService(3, piper, self.connection, Servos)
# Start new processes
self.background_service.start()

def go_to_pos(self, channel, pos):
self.connection.go_to(channel, pos)

def move(self, channel, speed):
self.logger.info(f"Moving channel {channel} at {speed}")
self.pipe.send(["SPEED", channel, max(-1, min(1, speed))])

def stop(self, channel=None):
if channel is None:
# stop all servos
self.pipe.send(["STOP", None])
self.connection.stop()
else:
self.pipe.send(["STOP", channel])
self.connection.stop(channel)


def close(self):
self.logger.info("Closing servo connection")
# Set all servos to 0 and close connection
self.stop()
self.background_service.close()
self.connection.close()
25 changes: 25 additions & 0 deletions src/servo_wrapper.py
@@ -0,0 +1,25 @@
import logging

class ServoModel:
def __init__(self, channel, speed, neutral, pos=0):
self.channel = channel
self.speed = speed
self.neutral = neutral
self.pos = pos

class ServoWrapper:
def __init__(self, config):
# Setup logger
self.logger = logging.getLogger(__name__)

def create_servo_model(self, channel, config):
pass

def go_to(self, channel, pos):
pass

def stop(self, channel=None):
pass

def close(self):
pass
49 changes: 49 additions & 0 deletions src/servos/maestro.py
@@ -0,0 +1,49 @@
from servo_wrapper import ServoWrapper, ServoModel
import maestromaster.maestro as mae

class MaestroConnection(ServoWrapper):
type_ = 'maestro'
CENTRE = 6000
RANGE = 3600

def __init__(self, config):
ServoWrapper.__init__(self, config)
self.port = config.get('port', "/dev/ttyACM0")
self.Controller = mae.Controller(port=self.port)
self.Controller.getErrors()

def getPos(self, angle):
if angle > 90:
angle = 90
if angle < -90:
angle = -90
return self.CENTRE + int(self.RANGE * angle / 90)

@staticmethod
def calc_speed(speed): # s/60 deg
QuarterTickPer60Deg = 2000 / 4
TenMSPerSecond = 100
out = int(1 / (speed / QuarterTickPer60Deg * TenMSPerSecond))
print("Calc Speed: ", out)
return out

def create_servo_model(self, channel, config):
sm = ServoModel(channel, self.calc_speed(config["speed"]), config["neutral"],
self.Controller.getPosition(channel))
self.Controller.setRange(channel, self.CENTRE-self.RANGE, self.CENTRE+self.RANGE)
self.Controller.setSpeed(channel, sm.speed)
self.Controller.setAccel(channel, 0)
return sm

def go_to(self, channel, pos):
self.Controller.setTarget(channel, pos)
x = self.Controller.getPosition(channel)
while x != pos:
x = self.Controller.getPosition(channel)


def stop(self):
pass

def close(self):
self.Controller.close()
21 changes: 21 additions & 0 deletions src/servos/maestromaster/LICENSE.md
@@ -0,0 +1,21 @@
The MIT License (MIT)

Copyright (c) 2016 Steven L. Jacobs

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.

0 comments on commit a282342

Please sign in to comment.