readme extended

Steve/Scripts/Rotation.py

@@ -0,0 +1,125 @@
+#!/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 Rotation:
+    def __init__(self):
+        """ setup ROS """
+        rospy.init_node('Rotation')
+        self.pub = rospy.Publisher('rotation',
+                                    msg.String,
+                                    queue_size=2)
+        self.listener_test = rospy.Subscriber('tests',
+                                            msg.String,
+                                            self.callback)
+        self.listener_switch = rospy.Subscriber('forward_switch',
+                                                msg.String,
+                                                self.calibrate_forward)
+        self.listener_cmd = rospy.Subscriber('command',
+                                            msg.String,
+                                            self.callback)
+                                            
+        """ constants """
+        self.position = 0  # degrees
+        self.rot_speed = 0.081  # degrees per step
+        self.test = False
+        self.epsilon = 1  # degrees
+        
+        """ setup GPIO """
+        self.frequency = 100  # Hz = steps per second
+        self.dutycycle = 0  # % - 50 % is good
+
+        self.pi = pigpio.pi()
+
+        self.pi.set_mode(Pin.M_Rot_enable, pigpio.OUTPUT)  # high wenn aus, low wenn an
+        self.pi.set_mode(Pin.M_Rot_dir, pigpio.OUTPUT)
+        self.pi.set_mode(Pin.M_Rot_speed, pigpio.OUTPUT)
+        
+        self.pi.hardware_PWM(Pin.M_Rot_speed, 0, 0)  # frequency, dutycycle
+
+        while not rospy.is_shutdown():
+            self.pi.write(Pin.M_Rot_enable, 1)  # high wenn aus
+            rospy.spin()
+
+    def callback(self, data):
+        rospy.loginfo(rospy.get_caller_id() + ' I heard %s',
+                      data.data)
+        if data.data == 'STOP':
+            self.stop()
+        elif data.data == 'CENTER':
+            self.center_position()
+        elif 'ROTATE' in data.data:
+            try:
+                numbers = re.findall('-*[0-9]+', data.data)
+                self.go_to_degrees(int(numbers[0]))
+            except ValueError as E:
+                rospy.loginfo(E)
+
+    def calibrate_forward(self, data):
+        rospy.loginfo(' I heard %s', data.data)
+        if data.data == 'HIGH':
+            self.position = 0  # degrees
+
+    def center_position(self):
+        self.go_to_degrees(0)
+
+    def go_to_degrees(self, degrees):
+        start = time.time()
+        while not abs(self.position - degrees) <= self.epsilon:
+            then = time.time()
+            if self.position > degrees:
+                self.counterclockwise()
+                signum = -1
+            else:
+                self.clockwise()
+                signum = 1
+            self.rotate()
+            rospy.loginfo('rotating at %s degrees now' % self.position)
+            rospy.sleep(0.2)
+            now = time.time()
+            self.position = self.position + signum * self.frequency * self.rot_speed * (now - then)
+            self.pub.publish(str(self.position))
+        self.stop()
+
+    def counterclockwise(self):
+        self.pi.write(Pin.M_Rot_dir, 0)
+
+    def clockwise(self):
+        self.pi.write(Pin.M_Rot_dir, 1)
+            
+    def rotate(self):
+        self.pi.write(Pin.M_Rot_enable, 0)  # low wenn an
+        self.dutycycle = 50
+        self.pi.hardware_PWM(Pin.M_Rot_speed, self.frequency, self.dutycycle * 10000)
+        
+    def stop(self):
+        self.pi.hardware_PWM(Pin.M_Rot_speed, 0, 0)
+        self.pi.write(Pin.M_Rot_enable, 1)  # high wenn aus
+        
+    def disable(self):
+        self.pwm.stop()
+        self.pi.write(Pin.M_Rot_enable, 1)  # high wenn aus
+
+if __name__ == '__main__':
+    try:
+        Rotation()
+    except rospy.ROSInterruptException:
+        pass
+    # finally:
+        # GPIO.cleanup()