RoverLehre/STEVE_Cammast
2
0
Fork 0
Code Issues 3 Pull Requests Projects Releases Wiki Activity

readme extended

Browse Source
This commit is contained in:
winterhalderp
2021-04-08 13:04:58 +02:00
parent 4ebdee8f01
commit f3b3b9f358
54 changed files with 169206 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

124
Steve/Scripts/Commander.py Normal file
View File

@ -0,0 +1,124 @@
#!/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
from sensor_msgs.msg import JointState
import pigpio
import Pin
# Header header
# uint seq
# time stamp
# string frame_id
# string name
# float position
# float velocity
# float effort
class Commander:
def __init__(self):
self.test = False
""" constants """
self.rotate_old = self.rotate = 0 # rad
self.elevate_old = self.elevate = 0 # rad
self.servo_old = self.servo = 0 # rad
""" status """
self.rotate_enable = False
self.elevate_enable = True
self.servo_enable = False
""" setup ROS """
rospy.init_node('Commander')
self.pub = rospy.Publisher('command',
msg.String,
queue_size=10)
self.pub_gs = rospy.Publisher('groundstation_monitoring',
JointState,
queue_size=10)
self.listener_monitor = rospy.Subscriber('monitor',
msg.String,
self.callback)
self.listener_interrupt = rospy.Subscriber('interrupt',
msg.String,
self.callback)
self.ground_control = rospy.Subscriber('groundstation_command',
JointState,
self.callback_cmd)
""" keep node running """
while not rospy.is_shutdown():
rospy.spin()
def callback_cmd(self, cmd):
self.old_cmd()
rospy.loginfo(rospy.get_caller_id() + 'I heard the command \n\t Sequence: %s \n\t FrameID: %s \n\t Joint: %s \n\t Position: %s \n\t Velocity: %s \n\t Effort: %s',
cmd.header.seq, cmd.header.frame_id, cmd.name, cmd.position, cmd.velocity, cmd.effort)
# self.pub_gs.publish(cmd)
self.rotate = cmd.position[0]
self.elevate = cmd.position[1]
self.servo = cmd.position[2]
self.convert_command()
def old_cmd(self):
self.rotate_old = self.rotate
self.elevate_old = self.elevate
self.servo_old = self.servo
def convert_command(self):
if not self.rotate == self.rotate_old and self.rotate_enable:
self.rotate = int(2*3.1415*float(self.rotate)) # rad to deg
self.pub.publish(msg.String('ROTATE ' + str(self.rotate)))
if not self.elevate == self.elevate_old:
if self.elevate < -1.4 and self.elevate_enable:
self.pub.publish(msg.String('STOW'))
elif self.elevate >= -1.4 and self.elevate <= 0.4 and self.elevate_enable:
self.pub.publish(msg.String('ELEVATE'))
elif self.elevate > 0.4:
self.pub.publish(msg.String('TILT'))
if not self.servo == self.servo_old and self.servo_enable:
self.servo = int(2*3.1415*float(self.camera)) # rad to deg
self.pub.publish(msg.String('SERVO ' + str(self.servo)))
def callback(self, data):
rospy.loginfo(rospy.get_caller_id() + ' I heard %s',
data.data)
if 'ELEVATE' in data.data:
if 'ENABLE' in data.data:
self.elevate_enable = True
else:
self.elevate_enable = False
if 'ROTATE' in data.data:
if 'ENABLE' in data.data:
self.rotate_enable = True
else:
self.rotate_enable = False
if 'SERVO' in data.data:
if 'ENABLE' in data.data:
self.servo_enable = True
else:
self.servo_enable = False
if __name__ == '__main__':
try:
Commander()
except rospy.ROSInterruptException:
pass

268
Steve/Scripts/Elevation.py Normal file
View File

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

42
Steve/Scripts/ForwardSwitch.py Normal file
View File

@ -0,0 +1,42 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Created on Mon Feb 10 17:09:57 2020
@author: Maxi
"""
import roslib
import rospy
import sys
import std_msgs.msg as msg
from dynamic_reconfigure.server import Server as DynamicReconfigureServer
# Import custom message data and dynamic reconfigure variables.
from Steve.msg import node_example
from Steve.cfg import node_example_paramsConfig as ConfigType
import RPi.GPIO as GPIO
import Pin
from Switch import Switch
class ForwardSwitch(Switch):
def __init__(self):
self.pin = Pin.limit_forward
self.pin_out = Pin.limit_forward_out
self.event_based = True
self.setup_pwr_high()
self.SwitchPub = rospy.Publisher('forward_switch', msg.String, queue_size=1)
Switch.__init__(self)
if __name__ == '__main__': # name == main when ros opens ***.py
# instantiate class
try:
forward_switch = ForwardSwitch()
except rospy.ROSInterruptException:
pass

42
Steve/Scripts/LimitSwitch.py Normal file
View File

@ -0,0 +1,42 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Created on Mon Feb 10 17:09:57 2020
@author: Maxi
"""
import roslib
import rospy
import sys
import std_msgs.msg as msg
from dynamic_reconfigure.server import Server as DynamicReconfigureServer
# Import custom message data and dynamic reconfigure variables.
from Steve.msg import node_example
from Steve.cfg import node_example_paramsConfig as ConfigType
import RPi.GPIO as GPIO
import Pin
from Switch import Switch
class TiltSwitch(Switch):
def __init__(self):
self.pin = Pin.limit_tilt
self.pin_out = Pin.limit_tilt_out
self.event_based = True
self.setup_gnd_high()
self.SwitchPub = rospy.Publisher('tilt_switch', msg.String, queue_size=1)
Switch.__init__(self)
if __name__ == '__main__': # name == main when ros opens ***.py
# instantiate class
try:
tilt_switch = TiltSwitch()
except rospy.ROSInterruptException:
pass

174
Steve/Scripts/MastMonitor.py Normal file
View File

@ -0,0 +1,174 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Created on Mon Feb 10 17:09:57 2020
@author: Maxi
"""
import re
import roslib
import rospy
import sys
import std_msgs.msg as msg
from sensor_msgs.msg import JointState
from dynamic_reconfigure.server import Server as DynamicReconfigureServer
# Import custom message data and dynamic reconfigure variables.
from Steve.msg import node_example
from Steve.cfg import node_example_paramsConfig as ConfigType
class Monitor:
def __init__(self):
self.statusOK = True
self.counter = 0
""" status """
self.rotation = 0
self.elevation = 'stowed'
self.elev_angle = 90 # deg
self.servo = 0
""" ROS setup """
rospy.init_node('Monitor')
self.pub = rospy.Publisher('monitor',
msg.String,
queue_size=5)
self.InterruptPub = rospy.Publisher('interrupt',
msg.String,
queue_size=3)
self.pub_gs_monitor = rospy.Publisher('groundstation_monitoring',
JointState,
queue_size=1)
self.listener_gs_cmd = rospy.Subscriber('groundstation_command',
JointState,
callback=self.callback_cmd)
self.listener_gs_monitor = rospy.Subscriber('groundstation_monitoring',
JointState,
callback=self.callback_mon)
self.listener_ch = rospy.Subscriber('chatter',
node_example,
callback=self.callback_custom_msg)
self.listener_st = rospy.Subscriber('stowed_switch',
msg.String,
callback=self.callback_generic,
callback_args='stowed_switch')
self.listener_ra = rospy.Subscriber('raised_switch',
msg.String,
callback=self.callback_generic,
callback_args='raised_switch')
self.listener_lim = rospy.Subscriber('limit_switch',
msg.String,
callback=self.callback_generic,
callback_args='limit_switch')
self.listener_test = rospy.Subscriber('tests',
msg.String,
callback=self.callback_generic,
callback_args='tests')
self.listener_int = rospy.Subscriber('interrupt',
msg.String,
callback=self.callback_generic,
callback_args='interrupt')
self.listener_cmd = rospy.Subscriber('command',
msg.String,
callback=self.callback_generic,
callback_args='command')
self.listener_forw = rospy.Subscriber('forward_switch',
msg.String,
callback=self.callback_generic,
callback_args='forward_switch')
self.listener_rot = rospy.Subscriber('rotation',
msg.String,
callback=self.callback_generic,
callback_args='rotation')
self.listener_elev = rospy.Subscriber('elevation',
msg.String,
callback=self.callback_generic,
callback_args='elevation')
self.listener_servo = rospy.Subscriber('servo',
msg.String,
callback=self.callback_generic,
callback_args='servo')
while not rospy.is_shutdown():
self.enable_activities()
rospy.spin()
# time.sleep(0.1)
def callback_generic(self, data, topic):
rospy.loginfo('%s: sent me %s', topic, data.data)
if topic == 'rotation':
self.rotation = float(data.data)
elif topic == 'elevation':
if 'STATE' in data.data:
self.elevation = re.sub('STATE: ', '', data.data)
rospy.loginfo(self.elevation)
elif 'ANGLE' in data.data:
self.elev_angle = int(re.findall('-*[0-9]+', data.data)[0])
rospy.loginfo(self.elev_angle)
elif topic == 'servo':
self.servo = float(data.data)
if self.counter > 10:
self.enable_activities()
self.send_status()
self.counter = 0
self.counter += 1
def callback_custom_msg(self, data):
rospy.loginfo(rospy.get_caller_id() + ' I heard %s',
data.message)
def callback_cmd(self, cmd):
rospy.loginfo(rospy.get_caller_id() + 'I heard the command \n\t Sequence: %s \n\t FrameID: %s \n\t Joint: %s \n\t Position: %s \n\t Velocity: %s \n\t Effort: %s',
cmd.header.seq, cmd.header.frame_id, cmd.name, cmd.position, cmd.velocity, cmd.effort)
def callback_mon(self, cmd):
rospy.loginfo(rospy.get_caller_id() + 'Status was sent \n\t Sequence: %s \n\t FrameID: %s \n\t Joint: %s \n\t Position: %s \n\t Velocity: %s \n\t Effort: %s',
cmd.header.seq, cmd.header.frame_id, cmd.name, cmd.position, cmd.velocity, cmd.effort)
def enable_activities(self):
if abs(self.rotation) < 1.5:
self.InterruptPub.publish(msg.String('ELEVATE: ENABLE'))
else:
self.InterruptPub.publish(msg.String('ELEVATE: DISABLE'))
if self.elevation == 'raised':
self.InterruptPub.publish(msg.String('ROTATE: ENABLE'))
self.InterruptPub.publish(msg.String('SERVO: ENABLE'))
else:
self.InterruptPub.publish(msg.String('ROTATE: DISABLE'))
self.InterruptPub.publish(msg.String('SERVO: DISABLE'))
def send_status(self):
msg = JointState()
msg.name = ['J1_turntable', 'J2_fold', 'J3_nod']
msg.position = [self.deg_to_rad(self.rotation),
self.deg_to_rad(self.elev_angle),
self.deg_to_rad(self.servo)]
msg.velocity = []
msg.effort = []
self.pub_gs_monitor.publish(msg)
def deg_to_rad(self, degrees):
rad = degrees * 3.1415 / 180.0
return rad
def elev_to_rad(self, state):
if state == 'raised':
return 0.0
elif state == 'stowed':
return deg_to_rad(90)
elif state == 'tilted':
return deg_to_rad(-30)
else:
return
if __name__ == '__main__':
try:
monitor = Monitor()
except rospy.ROSInterruptException:
pass

38
Steve/Scripts/Pin.py Normal file
View File

@ -0,0 +1,38 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Created on Mon Feb 10 11:26:41 2020
@author: Maxi
"""
import RPi.GPIO as GPIO
# GPIO.setmode(GPIO.BOARD)
GPIO.setmode(GPIO.BCM)
"""
TODO
PWM1 und PWM0 beide freischalten
I2C aktivieren?
"""
# Pin Numbering a la GPIOxx (BCM)
# pwr33V = [1, 17]
# pwr5V = [2, 4]
# GND = [6, 9, 14, 20, 25, 30, 34, 39]
M_Elev_speed = sw_PWM = 4
M_Elev_dir = 14
M_Rot_speed = PWM1 = 18 # motor
M_Rot_dir = 15 # motor
M_Rot_enable = 24
M_Cam = PWM0 = 13 # motor
limit_stowed = 27
limit_stowed_out = 9
limit_raised = 17
limit_raised_out = 10
limit_forward = 22
limit_forward_out = 11
limit_tilt = 23
limit_tilt_out = 8
I2C = {'Data': 2, 'Clock': 3}

BIN
Steve/Scripts/Pin.pyc Normal file
View File

Binary file not shown.

42
Steve/Scripts/RaisedSwitch.py Normal file
View File

@ -0,0 +1,42 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Created on Mon Feb 10 17:09:57 2020
@author: Maxi
"""
import roslib
import rospy
import sys
import std_msgs.msg as msg
from dynamic_reconfigure.server import Server as DynamicReconfigureServer
# Import custom message data and dynamic reconfigure variables.
from Steve.msg import node_example
from Steve.cfg import node_example_paramsConfig as ConfigType
import RPi.GPIO as GPIO
import Pin
from Switch import Switch
class RaisedSwitch(Switch):
def __init__(self):
self.pin = Pin.limit_raised
self.pin_out = Pin.limit_raised_out
self.event_based = True
self.setup_pwr_high()
self.SwitchPub = rospy.Publisher('raised_switch', msg.String, queue_size=1)
Switch.__init__(self)
if __name__ == '__main__': # name == main when ros opens ***.py
# instantiate class
try:
raised_switch = RaisedSwitch()
except rospy.ROSInterruptException:
pass

125
Steve/Scripts/Rotation.py Normal file
View File

@ -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()

162
Steve/Scripts/Servo.py Normal file
View File

@ -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

47
Steve/Scripts/StowedSwitch.py Normal file
View File

@ -0,0 +1,47 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Created on Mon Feb 10 17:09:57 2020
@author: Maxi
"""
import roslib
import rospy
import sys
import std_msgs.msg as msg
from dynamic_reconfigure.server import Server as DynamicReconfigureServer
# Import custom message data and dynamic reconfigure variables.
from Steve.msg import node_example
from Steve.cfg import node_example_paramsConfig as ConfigType
import RPi.GPIO as GPIO
import Pin
from Switch import Switch
class StowedSwitch(Switch):
def __init__(self):
""" TODO : Flanke abstellen, nur auf HIGH / LOW abfragen """
self.pin = Pin.limit_stowed
self.pin_out = Pin.limit_stowed_out
self.event_based = False # detect only status changes
self.setup_pwr_low()
self.SwitchPub = rospy.Publisher('stowed_switch', msg.String, queue_size=1)
Switch.__init__(self)
if __name__ == '__main__': # name == main when ros opens ***.py
# initialize the node
# rospy.init_node('Switch')
# instantiate class
try:
# my_switch = StowedSwitch()
stow_switch = StowedSwitch()
# raised_switch = RaisedSwitch()
except rospy.ROSInterruptException:
pass

189
Steve/Scripts/Switch.py Normal file
View File

@ -0,0 +1,189 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Created on Mon Feb 10 17:09:57 2020
@author: Maxi
"""
import roslib
import rospy
import sys
import std_msgs.msg as msg
from dynamic_reconfigure.server import Server as DynamicReconfigureServer
# Import custom message data and dynamic reconfigure variables.
from Steve.msg import node_example
from Steve.cfg import node_example_paramsConfig as ConfigType
import RPi.GPIO as GPIO
import Pin
class Switch:
def __init__(self):
""" configure RPi, GPIO """
print(GPIO.getmode())
self.is_high = False
""" configure ROS """
self.node()
init_message = rospy.get_param('~message', 'hello')
rate = float(rospy.get_param('~rate', '1.0'))
topic = rospy.get_param('~topic', 'chatter')
rospy.loginfo('rate = %d', rate)
rospy.loginfo('topic = %s', topic)
# create a dynamic reconfigure server
self.server = DynamicReconfigureServer(ConfigType, self.reconfigure)
# create a publisher
self.pub = rospy.Publisher(topic, node_example, queue_size=10)
# set the message to publish
msg = node_example()
msg.a = 1
msg.b = 2
msg.message = init_message
""" start main processing loop """
while not rospy.is_shutdown():
# self.send_message()
self.detect_IO_change()
# if self.event_based:
# self.check_IO_value()
# sleep before next message
rospy.sleep(1/rate) # rate != 0
def reconfigure(self, config, level):
# fill in local variables from dynamic reconfigure items
self.message = config["message"]
self.a = config["a"]
self.b = config["b"]
rospy.loginfo('reconfiguring with %s' % config)
return config
""" trying to split stuff into functions """
def setup_pwr_low(self):
""" for switches connected to 3.3V --> will send 'LOW' when closing """
print("setting up GPIO")
print('setting PULL DOWN, expect to send LOW')
GPIO.setup(self.pin_out,
GPIO.OUT)
GPIO.output(self.pin_out,
1)
GPIO.setup(self.pin,
GPIO.IN,
pull_up_down=GPIO.PUD_DOWN)
GPIO.add_event_detect(self.pin,
GPIO.FALLING,
callback=self.send_if_low,
bouncetime=1000)
def setup_pwr_high(self):
""" for switches connected to 3.3V --> will send 'HIGH' when closing """
print("setting up GPIO")
print('setting PULL DOWN, expect to send LOW')
GPIO.setup(self.pin_out,
GPIO.OUT)
GPIO.output(self.pin_out,
1)
GPIO.setup(self.pin,
GPIO.IN,
pull_up_down=GPIO.PUD_UP)
GPIO.add_event_detect(self.pin,
GPIO.RISING,
callback=self.send_if_high,
bouncetime=1000)
def setup_gnd_low(self):
""" for switches connected to GND --> will send 'LOW' when triggered """
print("setting up GPIO")
print('setting PULL UP, expect to send HIGH')
GPIO.setup(self.pin_out,
GPIO.OUT)
GPIO.output(self.pin_out,
0)
GPIO.setup(self.pin,
GPIO.IN,
pull_up_down=GPIO.PUD_UP)
GPIO.add_event_detect(self.pin,
GPIO.FALLING,
callback=self.send_if_low,
bouncetime=1000)
def setup_gnd_high(self):
""" for switches connected to GND --> will send 'HIGH' when triggered """
print("setting up GPIO")
print('setting PULL UP, expect to send HIGH')
GPIO.setup(self.pin_out,
GPIO.OUT)
GPIO.output(self.pin_out,
0)
GPIO.setup(self.pin,
GPIO.IN,
pull_up_down=GPIO.PUD_UP)
GPIO.add_event_detect(self.pin,
GPIO.RISING,
callback=self.send_if_high,
bouncetime=1000)
def node(self):
# initialize the node
rospy.loginfo('launching node')
rospy.init_node('switch', anonymous=True)
def send_message(self):
# set the message to publish
msg = node_example()
msg.message = self.message
msg.a = self.a
msg.b = self.b
self.pub.publish(msg)
rospy.loginfo(rospy.get_caller_id() + ' sent message a: %s, b: %s' % (self.a, self.b))
def set_output(self):
GPIO.setup(self.output_pin,
GPIO.OUT)
def send_if_high(self, channel=None):
# if self.check_IO_value():
rospy.loginfo(rospy.get_caller_id() + 'Switch is HIGH == released')
my_msg = msg.String()
my_msg.data = 'HIGH'
self.SwitchPub.publish(my_msg)
def send_if_low(self, channel=None):
# if not self.check_IO_value():
rospy.loginfo(rospy.get_caller_id() + 'Switch is LOW == pressed')
my_msg = msg.String()
my_msg.data = 'LOW'
self.SwitchPub.publish(my_msg)
def check_IO_value(self):
if GPIO.input(self.pin) == GPIO.LOW:
rospy.loginfo('%s is LOW' % self.pin)
return False
elif GPIO.input(self.pin) == GPIO.HIGH:
rospy.loginfo('%s is HIGH' % self.pin)
return True
def detect_IO_change(self):
self.is_high_old = self.is_high
self.is_high = self.check_IO_value()
# if self.is_high_old and not self.is_high:
# self.send_if_low()
# elif not self.is_high_old and self.is_high:
# self.send_if_high()
# else:
# pass
if self.is_high:
self.send_if_high()
else:
self.send_if_low()
rospy.sleep(0.3)

BIN
Steve/Scripts/Switch.pyc Normal file
View File

Binary file not shown.

7
Steve/Scripts/__init__.py Normal file
View File

@ -0,0 +1,7 @@
# -*- coding: utf-8 -*-
"""
Created on Mon Feb 10 11:22:10 2020
@author: Maxi
"""

8
Steve/Scripts/runpigpiod.sh Normal file
View File

@ -0,0 +1,8 @@
#!/bin/bash
# just print this out
echo "Starting GPIO Deamon (sudo pigpiod)"
sudo pigpiod
# exit gracefully by returning a status
exit 0

216
Steve/Scripts/tests.py Normal file
View File

@ -0,0 +1,216 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Created on Mon Feb 10 12:10:34 2020
@author: Maxi
"""
import roslib
import rospy
import sys
import time
import std_msgs.msg as msg
class Test:
def __init__(self):
""" setup ROS """
rospy.init_node('Tests')
self.pub = rospy.Publisher('tests',
msg.String,
queue_size=10)
self.CmdPub = rospy.Publisher('command',
msg.String,
queue_size=10)
self.InterruptPub = rospy.Publisher('interrupt',
msg.String,
queue_size=10)
self.RaisedSwitchPub = rospy.Publisher('raised_switch',
msg.String,
queue_size=10)
self.ForwardSwitchPub = rospy.Publisher('forward_switch',
msg.String,
queue_size=10)
self.StowedSwitchPub = rospy.Publisher('stowed_switch',
msg.String,
queue_size=10)
self.RotatePub = rospy.Publisher('rotation',
msg.String,
queue_size=10)
self.ElevatePub = rospy.Publisher('elevation',
msg.String,
queue_size=10)
self.ServoPub = rospy.Publisher('servo',
msg.String,
queue_size=10)
# while not rospy.is_shutdown():
# rospy.spin()
def test_elevate(self):
self.pub.publish(msg.String('Test 0'))
rospy.sleep(1.0)
self.StowedSwitchPub.publish(msg.String('LOW'))
rospy.sleep(1.0)
self.CmdPub.publish(msg.String('ELEVATE'))
rospy.sleep(10.0) # seconds
# self.RaisedSwitchPub.publish(msg.String('HIGH'))
def test_retract(self):
self.pub.publish(msg.String('Test 0'))
# rospy.sleep(1.0)
# self.RaisedSwitchPub.publish(msg.String('HIGH'))
rospy.sleep(1.0)
self.CmdPub.publish(msg.String('SERVO 68'))
rospy.sleep(5.0)
self.CmdPub.publish(msg.String('STOW'))
rospy.sleep(20.0) # seconds
# self.StowedSwitchPub.publish(msg.String('LOW'))
def test_tilt(self):
self.pub.publish(msg.String('Test 0'))
rospy.sleep(1.0)
self.CmdPub.publish(msg.String('TILT'))
rospy.sleep(10.0) # seconds
def test_sweep(self):
self.pub.publish(msg.String('Test 0'))
rospy.sleep(1.0)
self.CmdPub.publish('ROTATE %s' % str(180))
rospy.sleep(45.0)
self.CmdPub.publish('ROTATE %s' % str(-180))
rospy.sleep(45.0)
self.CmdPub.publish('CENTER')
rospy.sleep(30.0)
def test_center(self):
self.pub.publish(msg.String('Test 0'))
rospy.sleep(1.0)
# self.ForwardSwitchPub.publish('HIGH')
# rospy.sleep(1.0)
self.CmdPub.publish('ROTATE %s' % str(10))
rospy.sleep(15.0)
self.CmdPub.publish('ROTATE %s' % str(-10))
rospy.sleep(15.0)
self.CmdPub.publish('CENTER')
rospy.sleep(30.0)
# self.CmdPub.publish('ROTATE %s' % str(-80))
# rospy.sleep(10.0)
# self.CmdPub.publish('CENTER')
# rospy.sleep(20.0)
# rospy.sleep(1.0)
# self.ForwardSwitchPub.publish('HIGH')
def test_rotate(self, angle):
self.pub.publish(msg.String('Test 0'))
rospy.sleep(1.0)
self.CmdPub.publish('ROTATE %s' % str(angle))
# speed = 8 # grad pro sekunde
# rospy.sleep(angle/speed)
rospy.sleep(5)
def test_elev_switch(self):
self.pub.publish(msg.String('Test 0'))
rospy.sleep(1.0)
self.CmdPub.publish(msg.String('ELEVATE'))
rospy.sleep(5.0)
self.pub.publish(msg.String('Test 0'))
rospy.sleep(1.0)
self.CmdPub.publish(msg.String('STOW'))
def test_servo(self, angle):
self.pub.publish(msg.String('Test 0'))
rospy.sleep(0.2)
# self.CmdPub.publish(msg.String('SERVO 0'))
# rospy.sleep(5.0)
self.CmdPub.publish(msg.String('SERVO ' + str(angle)))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 20'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 30'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 40'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 50'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 55'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 60'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 62'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 64'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 66'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 68'))
# rospy.sleep(5.0)
def test1(self):
for t in range(40):
my_msg = msg.String()
my_msg.data = str(t)
rospy.loginfo('sent %s' % my_msg.data)
pub.publish(my_msg)
if rospy.is_shutdown():
return
rospy.Rate(0.8).sleep()
if __name__ == '__main__':
try:
t = Test()
rospy.sleep(2.0)
# First Run of tests.py should start with:
#t.test_elevate()
#t.test_sweep() # Search Forward Position
#t.test_center() # Move to Rearward Position
#t.test_retract()
#t.test_rotate(180)
# t.test_center()
# rospy.sleep(10.0)
#t.test_tilt()
# rospy.sleep(10.0)
# t.test_center()
# rospy.sleep(10.0)
# rospy.sleep(30.0)
# t.test_retract()
# rospy.sleep(30.0)
#t.test_elevate() # Aufrichten in die Vertikale
# rospy.sleep(10.0)
# t.test_sweep()
# rospy.sleep(30.0)
# t.test_center()
# rospy.sleep(30.0)
# t.test_retract()
# rospy.sleep(10.0)
# t.test_elevate()
# rospy.sleep(10.0)
# # rospy.sleep(30.0)
# t.test_tilt() # Tilt until Limit Switch pressed
# rospy.sleep(20.0)
# t.test_elevate()
# rospy.sleep(30.0)
#for angle in range(20, 30, 2):
## angle = 40 - angle
#t.test_servo(angle)
#rospy.sleep(0.6)
t.test_servo(20)
# rospy.sleep(1.0)
# t.test_rotate(-90)
# rospy.sleep(10.0).
# t.test_rotate(-270)
# rospy.sleep(10.0)
# t.test_rotate(-180)
# rospy.sleep(5.0)
# t.test_rotate(0)
except rospy.ROSInterruptException:
pass

304
Steve/Scripts/tests_cli.py Normal file
View File

@ -0,0 +1,304 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
Created on Mon Feb 10 12:10:34 2020
@author: Maxi
@edit: PatrickW
"""
import roslib
import rospy
import sys
import time
import std_msgs.msg as msg
class Test:
def __init__(self):
""" setup ROS """
rospy.init_node('Tests')
self.pub = rospy.Publisher('tests',
msg.String,
queue_size=10)
self.CmdPub = rospy.Publisher('command',
msg.String,
queue_size=10)
self.InterruptPub = rospy.Publisher('interrupt',
msg.String,
queue_size=10)
self.RaisedSwitchPub = rospy.Publisher('raised_switch',
msg.String,
queue_size=10)
self.ForwardSwitchPub = rospy.Publisher('forward_switch',
msg.String,
queue_size=10)
self.StowedSwitchPub = rospy.Publisher('stowed_switch',
msg.String,
queue_size=10)
self.RotatePub = rospy.Publisher('rotation',
msg.String,
queue_size=10)
self.ElevatePub = rospy.Publisher('elevation',
msg.String,
queue_size=10)
self.ServoPub = rospy.Publisher('servo',
msg.String,
queue_size=10)
# while not rospy.is_shutdown():
# rospy.spin()
def test_elevate(self):
self.pub.publish(msg.String('Test 0'))
rospy.sleep(1.0)
self.StowedSwitchPub.publish(msg.String('LOW'))
rospy.sleep(1.0)
self.CmdPub.publish(msg.String('ELEVATE'))
rospy.sleep(2.0) # seconds
# self.RaisedSwitchPub.publish(msg.String('HIGH'))
def test_retract(self):
self.pub.publish(msg.String('Test 0'))
# rospy.sleep(1.0)
# self.RaisedSwitchPub.publish(msg.String('HIGH'))
rospy.sleep(1.0)
#self.CmdPub.publish(msg.String('SERVO 68'))
#rospy.sleep(5.0)
self.CmdPub.publish(msg.String('STOW'))
#rospy.sleep(20.0) # seconds
# self.StowedSwitchPub.publish(msg.String('LOW'))
def test_tilt(self, tilt_state=""):
self.pub.publish(msg.String('Test 0'))
rospy.sleep(1.0)
string = 'TILT' + str(tilt_state)
self.CmdPub.publish(msg.String(string))
#self.CmdPub.publish(msg.String('TILT'))
rospy.sleep(10.0) # seconds
def test_sweep(self):
self.pub.publish(msg.String('Test 0'))
rospy.sleep(1.0)
self.CmdPub.publish('ROTATE %s' % str(180))
rospy.sleep(45.0)
self.CmdPub.publish('ROTATE %s' % str(-180))
rospy.sleep(45.0)
self.CmdPub.publish('CENTER')
rospy.sleep(30.0)
def test_center(self):
self.pub.publish(msg.String('Test 0'))
rospy.sleep(1.0)
# self.ForwardSwitchPub.publish('HIGH')
# rospy.sleep(1.0)
self.CmdPub.publish('ROTATE %s' % str(10))
rospy.sleep(15.0)
self.CmdPub.publish('ROTATE %s' % str(-10))
rospy.sleep(15.0)
self.CmdPub.publish('CENTER')
rospy.sleep(30.0)
# self.CmdPub.publish('ROTATE %s' % str(-80))
# rospy.sleep(10.0)
# self.CmdPub.publish('CENTER')
# rospy.sleep(20.0)
# rospy.sleep(1.0)
# self.ForwardSwitchPub.publish('HIGH')
def test_rotate(self, angle):
self.pub.publish(msg.String('Test 0'))
rospy.sleep(1.0)
self.CmdPub.publish('ROTATE %s' % str(angle))
# speed = 8 # grad pro sekunde
# rospy.sleep(angle/speed)
rospy.sleep(10)
def test_elev_switch(self):
self.pub.publish(msg.String('Test 0'))
rospy.sleep(1.0)
self.CmdPub.publish(msg.String('ELEVATE'))
rospy.sleep(5.0)
self.pub.publish(msg.String('Test 0'))
rospy.sleep(1.0)
self.CmdPub.publish(msg.String('STOW'))
def test_servo(self, angle):
self.pub.publish(msg.String('Test 0'))
rospy.sleep(0.2)
# self.CmdPub.publish(msg.String('SERVO 0'))
# rospy.sleep(5.0)
self.CmdPub.publish(msg.String('SERVO ' + str(angle)))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 20'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 30'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 40'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 50'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 55'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 60'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 62'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 64'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 66'))
# rospy.sleep(5.0)
# self.CmdPub.publish(msg.String('SERVO 68'))
# rospy.sleep(5.0)
def test1(self):
for t in range(40):
my_msg = msg.String()
my_msg.data = str(t)
rospy.loginfo('sent %s' % my_msg.data)
pub.publish(my_msg)
if rospy.is_shutdown():
return
rospy.Rate(0.8).sleep()
def print_menu():
print("--- STEVE Console Interface ---")
print 5*"-", "Choose Command", 5*"-"
print "1. Raise from Storage" # Raise Mast (test_elevate)
print "2. Initiate Azimuth" # Initiate Az (find forward switch) (test_sweep)
print "3. Rotate Azimuth"
print "4. Rotate Azimuth to Forward Position (180 deg)"
print "5. Rotate Pitch Servo -SERVO NOT CONNECTED-"
print "6. Move to Tilted Position" # Rotate Elevation to tilted
print "7. Move to storage Azimuth (0 deg)" # Move to storage Az (test_center)
print "8. Retract (Caution: Center Az first)" # test_retract
return
if __name__ == '__main__':
t = Test()
rospy.sleep(1.0)
rate=rospy.Rate(10)
print_limit = 8
number_of_commands = 0
print_menu()
while not rospy.is_shutdown():
if number_of_commands>=print_limit:
print_menu()
number_of_commands = 0
try:
choice = input("Command [1-8]: ")
choice = int(choice)
if choice == 1:
print("Raising Mast...")
t.test_elevate()
print("Finished :)")
elif choice == 2:
choice = raw_input("Caution: Is Mast raised? [y/n] ")
choice = str(choice)
if choice == "y":
print("Initiating: Finding Switch...")
t.test_sweep()
print("Finished :)")
else:
print("Cancelling...")
elif choice == 3:
set_angle_az = int(input("Enter set value [-359,359] -> [cw>0; ccw<0]: "))
if set_angle_az >-359 and set_angle_az < 359:
print("Moving Base...")
t.test_rotate(set_angle_az)
print("Finished :)")
else:
print("Azimuth Set Value out of bounds!")
elif choice == 4:
choice = raw_input("Rotation cw/ccw? [cw: 1; ccw: 2]: ")
choice = int(choice)
if choice == 1:
print("Moving to Forward Direction (cw)...")
set_angle_az = 180
t.test_rotate(set_angle_az)
print("Finished :)")
elif choice == 2:
print("Moving to Forward Direction (cw)...")
set_angle_az = -180
t.test_rotate(set_angle_az)
print("Finished :)")
else:
print("Cancelling...")
elif choice== 5:
set_angle_servo = input("Enter set value [0,68]: ")
if set_angle_servo>68 or set_angle_servo<0:
print("\n Error: Value out of bounds \n")
else:
print("Moving Servo...")
t.test_servo(set_angle_servo)
print("Finished :)")
elif choice == 6:
time = [0,18]
tilt_choice = input("Choose tilt stage " + str(time) + ": ")
tilt_choice = int(tilt_choice)
if tilt_choice>=time[0] and tilt_choice<=time[1]:
choice = raw_input("Caution: Is Mast raised? [y/n] ")
choice = str(choice)
if choice == "y":
print("Moving to Tilted Elevation...")
goal = "_" + str(tilt_choice) + "_"
t.test_tilt(tilt_state = goal)
t.CmdPub.publish(msg.String('STOP'))
print("Finished :)")
else:
print("Cancelling...")
else:
print("Entry out of bounds")
print("Cancelling...")
elif choice == 7:
print("Moving to storage Azimuth...")
t.test_center()
print("Finished :)")
elif choice == 8:
choice = raw_input("Caution: Is Mast centered && raised? [y/n] ")
choice = str(choice)
if choice == "y":
print("Retracting Mast...")
t.test_retract()
print("Finished :)")
else:
print("Cancelling...")
else:
print("Unkown Command! Enter any key to try again..")
except ValueError:
print("ValueError:\tinvalid... try again..")
except NameError as e:
print(e)
print("NameError:\tinvalid... try again..")
except KeyboardInterrupt:
print("KeyboardInterrupt")
t.pub.publish(msg.String('STOP'))
t.CmdPub.publish(msg.String('STOP'))
sys.exit()
except rospy.ROSInterruptException:
pass
except:
print("invalid... try again..")
finally:
number_of_commands += 1
print("")
#rospy.spin()