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