163 lines
5.1 KiB
Python
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
|