readme extended

Steve/Scripts/Servo.py

@@ -0,0 +1,162 @@
+#!/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