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

126 lines
3.9 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 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()