STEVE_Cammast/Steve/Scripts/Servo.py
2021-04-08 13:04:58 +02:00

163 lines
5.1 KiB
Python

#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Created on Mon Feb 10 12:10:34 2020
@author: Maxi
"""
import re
import roslib
import rospy
import sys
import time
import std_msgs.msg as msg
import pigpio
import Pin
class Servo:
def __init__(self):
""" setup ROS """
rospy.init_node('Servo')
self.pub = rospy.Publisher('servo',
msg.String,
queue_size=10)
self.listener_test = rospy.Subscriber('tests',
msg.String,
self.callback)
self.listener_cmd = rospy.Subscriber('command',
msg.String,
self.callback)
""" constants """
# TODO ggf. konstanten tauschen
self.position = 0 # degrees
self.t_max = 1500E-6 # ms == 0 deg
self.t_min = 500E-6 # ms == 180 deg
self.alpha_max = 0 # deg
self.alpha_min = 90 # deg
""" setup GPIO """
self.frequency = 50.0 # Hz
self.dutycycle = 0 # %
self.pi = pigpio.pi()
#self.pi.set_mode(Pin.M_Cam, pigpio.OUTPUT) # eher ALT1=PWM1 fuer bcm13
#self.pi.set_mode(Pin.M_Cam, pigpio.ALT1)
#print(">> GPIO Mode: " + str(self.pi.get_mode(Pin.M_Cam)) + " (1: OUTPUT; 5: ALT1=PWM1)")
#self.pi.hardware_PWM(Pin.M_Cam, 0, 0)
#self.pi.set_PWM_dutycycle(Pin.M_Cam, 0)
#self.pi.set_PWM_frequency(Pin.M_Cam, self.frequency)
## Ersatz GPIO: BCM19
self.pi.set_mode(19, pigpio.ALT5)
self.pi.hardware_PWM(19, 400, 0)
# Test Servo Motion
# CAUTION: Make sure servo movement is possible, not on storage position
print(">> Servo Test ...")
test_servo = False
if test_servo == True:
self.test_servo_motion()
print(">> Test Done")
while not rospy.is_shutdown():
rospy.spin()
def callback(self, data):
rospy.loginfo(rospy.get_caller_id() + ' I heard %s',
data.data)
if data.data == 'STOP':
self.disable()
elif 'SERVO' in data.data:
try:
numbers = re.findall('[0-9]+', data.data)
angle = int(numbers[0])
rospy.loginfo('Angle %s', angle)
if angle >= 0 and angle <= 75:
self.go_to_degrees(int(angle))
else:
rospy.loginfo('angle %s not possible', angle)
except Exception as E:
rospy.loginfo(E)
rospy.loginfo('unrecognised message format %s', data.data)
def go_to_degrees(self, degrees):
self.step_calc(degrees)
if self.stepwidth <= 1500E-6 and self.stepwidth >=740E-6:
# Catching runtime errors
if self.dutycycle < 0:
self.dutycycle = 0
if self.dutycycle > 255:
self.dutycycle = 255
self.pi.hardware_PWM(19, 400, self.calc_dutycycle(degrees))
#self.pi.hardware_PWM(Pin.M_Cam, self.frequency, self.dutycycle)
#self.pi.set_PWM_dutycycle(Pin.M_Cam, self.dutycycle)
rospy.loginfo('rotating at %s degrees now', degrees)
else:
rospy.loginfo('stepwidth %s not possible', self.stepwidth)
self.pub.publish(str(degrees))
def disable(self):
self.dutycycle = 0
self.pi.hardware_PWM(19, 0, 0)
#self.pi.hardware_PWM(Pin.M_Cam, self.frequency, 0)
#self.pi.set_PWM_dutycycle(Pin.M_Cam, self.dutycycle)
def step_calc(self, angle):
# linearer Zusammenhang
self.stepwidth = ((self.t_max - self.t_min)/(self.alpha_max - self.alpha_min)) * (angle - self.alpha_min) + self.t_min
self.dutycycle = self.calculate_pwm(self.stepwidth)
def calculate_pwm(self, stepwidth):
time_per_tick = 1 / self.frequency
pwm_fraction = stepwidth / time_per_tick
pwm_percent = pwm_fraction * 100
#pwm_set = pwm_percent * 10000
pwm_set = pwm_percent
rospy.loginfo('stepwidth: %s sec\nticktime: %s sec\npwm_set: %s %%', stepwidth, time_per_tick, pwm_set)
return pwm_set
def test_servo_motion(self):
for i in range(0,65,5):
self.go_to_degrees(i)
rospy.sleep(3.0)
self.go_to_degrees(0)
def calc_dutycycle(angle):
frequency = 400
steps_max = 250000000/frequency
t_servo_max = 2500 # us
t_servo_min = 500 # us
angle_max = 100 # enstpricht pwm > 50 percent
angle_min = -100 # enspricht pwm = 0
steps_min = (t_servo_min / t_servo_max) * steps_max
y0 = (steps_max-steps_min)/2
duty = (steps_max-steps_min)/(angle_max-angle_min) * angle + y0
print("\nDutycycle: " + str(duty) + "\n")
return duty
if __name__ == '__main__':
try:
s = Servo()
except rospy.ROSInterruptException:
pass
finally:
try:
s.disable()
except:
pass