269 lines
9.7 KiB
Python
269 lines
9.7 KiB
Python
|
#!/usr/bin/env python
|
||
|
|
|
||
|
|
|
||
|
|
# -*- coding: utf-8 -*-
|
||
|
|
"""
|
||
|
|
Created on Mon Feb 10 11:22:52 2020
|
||
|
|
|
||
|
|
@author: Maxi
|
||
|
|
"""
|
||
|
|
|
||
|
|
import roslib
|
||
|
|
import rospy
|
||
|
|
import sys
|
||
|
|
import time
|
||
|
|
|
||
|
|
import std_msgs.msg as msg
|
||
|
|
|
||
|
|
import pigpio
|
||
|
|
import RPi.GPIO as GPIO
|
||
|
|
import Pin
|
||
|
|
|
||
|
|
|
||
|
|
class Elevation:
|
||
|
|
def __init__(self):
|
||
|
|
self.test = False
|
||
|
|
self.direction = None
|
||
|
|
self.destination = None
|
||
|
|
self.state = 'stowed'
|
||
|
|
self.angle = 0.0 # deg
|
||
|
|
self.speed = 0.05 # deg per second
|
||
|
|
self.instances = 0
|
||
|
|
|
||
|
|
""" setup ROS """
|
||
|
|
rospy.init_node('Elevation')
|
||
|
|
self.pub = rospy.Publisher('elevation',
|
||
|
|
msg.String,
|
||
|
|
queue_size=2)
|
||
|
|
self.listener_test = rospy.Subscriber('tests',
|
||
|
|
msg.String,
|
||
|
|
self.callback)
|
||
|
|
self.listener_cmd = rospy.Subscriber('command',
|
||
|
|
msg.String,
|
||
|
|
self.callback)
|
||
|
|
self.listener_stow = rospy.Subscriber('stowed_switch',
|
||
|
|
msg.String,
|
||
|
|
self.callback,
|
||
|
|
callback_args='stow')
|
||
|
|
self.listener_raise = rospy.Subscriber('raised_switch',
|
||
|
|
msg.String,
|
||
|
|
self.callback,
|
||
|
|
callback_args='raise')
|
||
|
|
self.listener_tilt = rospy.Subscriber('tilt_switch',
|
||
|
|
msg.String,
|
||
|
|
self.callback,
|
||
|
|
callback_args='tilt')
|
||
|
|
""" setup GPIO """
|
||
|
|
self.frequency = 500 # Hz
|
||
|
|
self.dutycycle = 0 # %
|
||
|
|
|
||
|
|
self.pi = pigpio.pi()
|
||
|
|
|
||
|
|
self.pi.set_mode(Pin.M_Elev_dir, pigpio.OUTPUT)
|
||
|
|
self.pi.set_mode(Pin.M_Elev_speed, pigpio.OUTPUT)
|
||
|
|
|
||
|
|
self.pi.set_PWM_dutycycle(Pin.M_Elev_speed, 0)
|
||
|
|
self.pi.set_PWM_frequency(Pin.M_Elev_speed, self.frequency)
|
||
|
|
|
||
|
|
# GPIO.setup(Pin.M_Elev, GPIO.OUT)
|
||
|
|
# self.pwm = GPIO.PWM(Pin.M_Cam, self.frequency)
|
||
|
|
# self.pwm.start(self.dutycycle)
|
||
|
|
|
||
|
|
""" keep node running """
|
||
|
|
while not rospy.is_shutdown():
|
||
|
|
rospy.spin()
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
def callback(self, data, switch=None):
|
||
|
|
rospy.loginfo(rospy.get_caller_id() + ' I heard %s from %s',
|
||
|
|
data.data, switch)
|
||
|
|
|
||
|
|
tilt_cmd_recognized = 'TILT' in data.data # true,false
|
||
|
|
|
||
|
|
if data.data == 'STOP':
|
||
|
|
self.state = None
|
||
|
|
self.stop()
|
||
|
|
|
||
|
|
elif data.data == 'HIGH' and switch == 'raise':
|
||
|
|
self.state = 'raised'
|
||
|
|
self.angle = 0
|
||
|
|
if self.destination == 'up':
|
||
|
|
rospy.sleep(0.01)
|
||
|
|
self.stop()
|
||
|
|
elif self.destination == 'tilt': # Achtung! Tilt ist End stop!
|
||
|
|
self.tilt()
|
||
|
|
|
||
|
|
elif data.data == 'LOW' and switch == 'stow':
|
||
|
|
self.stop()
|
||
|
|
self.state = 'stowed'
|
||
|
|
self.angle = 90
|
||
|
|
if not self.direction == 'forwards': # einklappen?
|
||
|
|
rospy.sleep(0.3)
|
||
|
|
self.stop()
|
||
|
|
if self.destination == 'up':
|
||
|
|
self.elevate()
|
||
|
|
pass
|
||
|
|
|
||
|
|
elif data.data == 'HIGH' and switch == 'stow':
|
||
|
|
pass
|
||
|
|
#if self.destination == 'down' and not self.state == 'moving':
|
||
|
|
# self.retract()
|
||
|
|
|
||
|
|
elif data.data == 'HIGH' and switch == 'tilt': # Tilt ist Elevation end stop
|
||
|
|
if self.state == "stowed" or self.state == None or self.state == "raised":
|
||
|
|
self.stop()
|
||
|
|
else:
|
||
|
|
self.state = 'tilted' # tilted backwards, touching end stop
|
||
|
|
self.angle = -30
|
||
|
|
self.stop()
|
||
|
|
if not self.direction == 'backwards':
|
||
|
|
self.stop()
|
||
|
|
if self.destination == 'up':
|
||
|
|
if not self.state == "raised": #pw
|
||
|
|
self.elevate()
|
||
|
|
|
||
|
|
elif data.data == 'ELEVATE' and not self.state == 'raised':
|
||
|
|
self.destination = 'up'
|
||
|
|
self.state = "moving" # pw
|
||
|
|
self.elevate()
|
||
|
|
elif data.data == 'STOW' and not self.state == 'stowed':
|
||
|
|
self.destination = 'down'
|
||
|
|
self.state = "moving" # pw
|
||
|
|
self.retract()
|
||
|
|
"""
|
||
|
|
elif data.data == 'TILT' and not self.state == 'tilted':
|
||
|
|
self.destination = 'tilt'
|
||
|
|
self.state == "moving" #pw
|
||
|
|
self.tilt() #pw
|
||
|
|
|
||
|
|
elif data.data == 'TILT_10' and not self.state == 'tilted':
|
||
|
|
self.destination = 'tilt'
|
||
|
|
self.state == "moving" #pw
|
||
|
|
self.tilt_variabletime(10) #pw
|
||
|
|
elif data.data == 'TILT_15' and not self.state == 'tilted':
|
||
|
|
self.destination = 'tilt'
|
||
|
|
self.state == "moving" #pw
|
||
|
|
self.tilt_variabletime(15) #pw
|
||
|
|
elif data.data == 'TILT_18' and not self.state == 'tilted':
|
||
|
|
self.destination = 'tilt'
|
||
|
|
self.state == "moving" #pw
|
||
|
|
self.tilt_variabletime(18) #pw
|
||
|
|
"""
|
||
|
|
#elif tilt_cmd_recognized == True and not self.state == 'tilted':
|
||
|
|
elif 'TILT' in data.data and not self.state == 'tilted':
|
||
|
|
self.destination = 'tilt'
|
||
|
|
self.state == "moving" #pw
|
||
|
|
|
||
|
|
# Extract info from string
|
||
|
|
start_marker = data.data.find('TILT_') + 5 #pw
|
||
|
|
tilt_goal = data.data[start_marker:-1]
|
||
|
|
for i in range(0,5):
|
||
|
|
print(">> Tilt Goal set: " + str(tilt_goal))
|
||
|
|
tilt_goal = int(tilt_goal)
|
||
|
|
for i in range(0,5):
|
||
|
|
print(">> Tilt Goal set: " + str(tilt_goal))
|
||
|
|
if tilt_goal < 21:
|
||
|
|
self.tilt_variabletime(tilt_goal) #pw
|
||
|
|
else:
|
||
|
|
print(">> Tilt Goal out of bounds!")
|
||
|
|
|
||
|
|
rospy.loginfo('STATE: %s', self.state)
|
||
|
|
rospy.loginfo('DIRECTION: %s', self.destination)
|
||
|
|
self.pub.publish(msg.String('STATE: %s' % self.state))
|
||
|
|
if not self.state == 'moving':
|
||
|
|
self.pub.publish(msg.String('ANGLE: %s' % self.angle))
|
||
|
|
rospy.loginfo('ANGLE: %s', self.angle)
|
||
|
|
|
||
|
|
def elevate(self):
|
||
|
|
if self.state == 'tilted':
|
||
|
|
self.pi.write(Pin.M_Elev_dir, 1)
|
||
|
|
self.direction = 'backwards'
|
||
|
|
rospy.loginfo('moving backwards')
|
||
|
|
else:
|
||
|
|
self.pi.write(Pin.M_Elev_dir, 0)
|
||
|
|
self.direction = 'forwards'
|
||
|
|
rospy.loginfo('moving forwards')
|
||
|
|
self.move()
|
||
|
|
self.pub.publish(msg.String('raising mast'))
|
||
|
|
|
||
|
|
def retract(self):
|
||
|
|
self.pi.write(Pin.M_Elev_dir, 1)
|
||
|
|
self.direction = 'forwards'
|
||
|
|
self.pub.publish(msg.String('retracting mast'))
|
||
|
|
then = time.time()
|
||
|
|
self.move() # self.state = "moving" in dieser funktion!
|
||
|
|
maxtime = 25.3 # max Zeit fuer retract von raised position
|
||
|
|
now = time.time()
|
||
|
|
while (now-then)<maxtime:
|
||
|
|
now = time.time()
|
||
|
|
self.stop()
|
||
|
|
self.state = 'stowed'
|
||
|
|
|
||
|
|
def tilt(self):
|
||
|
|
self.pi.write(Pin.M_Elev_dir, 1) # 1: forwards, 0: backwards, siehe oben
|
||
|
|
self.direction = 'forwards'
|
||
|
|
self.pub.publish(msg.String('tilting mast'))
|
||
|
|
then = time.time()
|
||
|
|
self.move()
|
||
|
|
maxtime = 10.0 # Zeit bis in tilt position
|
||
|
|
now = time.time()
|
||
|
|
while (now-then)<maxtime:
|
||
|
|
now = time.time()
|
||
|
|
self.stop()
|
||
|
|
self.state = None # do not set als tilted, tilted means end stop
|
||
|
|
|
||
|
|
def tilt_variabletime(self, dt_movement=0):
|
||
|
|
self.pi.write(Pin.M_Elev_dir, 1) # 1: forwards, 0: backwards, siehe oben
|
||
|
|
self.direction = 'forwards'
|
||
|
|
self.pub.publish(msg.String('tilting mast'))
|
||
|
|
then = time.time()
|
||
|
|
self.move()
|
||
|
|
maxtime = int(dt_movement) # Zeit bis in tilt position
|
||
|
|
now = time.time()
|
||
|
|
while (now-then)<maxtime:
|
||
|
|
now = time.time()
|
||
|
|
self.stop()
|
||
|
|
self.state = None # do not set als tilted, tilted means end stop
|
||
|
|
|
||
|
|
def move(self):
|
||
|
|
# self.instances = self.instances + 1
|
||
|
|
self.state = 'moving'
|
||
|
|
self.pub.publish(msg.String('STATE: %s' % self.state))
|
||
|
|
self.dutycycle = 160
|
||
|
|
self.pi.set_PWM_frequency(Pin.M_Elev_speed, self.frequency)
|
||
|
|
self.pi.set_PWM_dutycycle(Pin.M_Elev_speed, self.dutycycle)
|
||
|
|
rospy.loginfo("Moving at Frequency: %s, Dutycycle %s, Direction %s", self.frequency, self.dutycycle, self.direction)
|
||
|
|
|
||
|
|
# then = time.time()
|
||
|
|
# while self.state == 'moving':
|
||
|
|
# now = time.time()
|
||
|
|
# delta_t = now - then
|
||
|
|
# if self.direction == 'backwards':
|
||
|
|
# self.angle = self.angle + self.speed * delta_t
|
||
|
|
# else:
|
||
|
|
# self.angle = self.angle - self.speed * delta_t
|
||
|
|
# rospy.loginfo('ANGLE: %s', self.angle)
|
||
|
|
# self.pub.publish(msg.String('ANGLE: %s' % self.angle))
|
||
|
|
# rospy.sleep(0.2)
|
||
|
|
# if not self.instances == 1:
|
||
|
|
# self.instances = self.instances - 1
|
||
|
|
# break
|
||
|
|
|
||
|
|
def stop(self):
|
||
|
|
self.dutycycle = 0
|
||
|
|
self.pi.set_PWM_dutycycle(Pin.M_Elev_speed, 0)
|
||
|
|
self.pub.publish(msg.String('was moving in direction: %s' % self.direction))
|
||
|
|
self.pub.publish(msg.String('Stopped Moving'))
|
||
|
|
rospy.loginfo('State: %s' % self.state)
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == '__main__':
|
||
|
|
try:
|
||
|
|
e = Elevation()
|
||
|
|
except rospy.ROSInterruptException:
|
||
|
|
try:
|
||
|
|
e.disable()
|
||
|
|
except Exception as E:
|
||
|
|
print(E)
|