def __init__(self):
self.conf = ServoConfig()
self.mg996r = ServoMotor(0, self.conf.getNeutral('steering'), self.conf.getMin('steering'), self.conf.getMax('steering'), 5)
# Initialize message variables.
self.enable = False
self.pwm = self.conf.getNeutral('steering')
self.mg996r.set_current_pwm(self.pwm)
self.robot_host = re.sub("-", "_", socket.gethostname())
self.steerSub = rospy.Subscriber(self.robot_host + '/steer/set', Steer, self.steerCallback)
self.steerPWMSub = rospy.Subscriber(self.robot_host + '/steer/set/pwm', Steer, self.steerCallback)
self.steerDegressSub = rospy.Subscriber(self.robot_host + '/steer/set/degree', ServoDeg, self.steerDegreeCallback)
self.steerResetSub = rospy.Subscriber(self.robot_host + '/steer/reset', Bool, self.steerResetCallback)
self.steerPub = rospy.Publisher(self.robot_host + '/steer/get', Steer, queue_size=10)
self.steerPWMPub = rospy.Publisher(self.robot_host + '/steer/get/pwm', Steer, queue_size=10)
self.steerDegreePub = rospy.Publisher(self.robot_host + '/steer/get/degree', ServoDeg, queue_size=10)
self.steerIntervallPub = rospy.Publisher(self.robot_host + '/steer/get/intervall', ServoIntervall, queue_size=10)
self.steerIntervallPWMPub = rospy.Publisher(self.robot_host + '/steer/get/intervall/pwm', ServoIntervall, queue_size=10)
self.steerIntervallDegreePub = rospy.Publisher(self.robot_host + '/steer/get/intervall/degree', ServoIntervallDeg, queue_size=10)
self.steerChannelPub = rospy.Publisher(self.robot_host + '/steer/get/channel', Int8, queue_size=10)
self.steerMinPub = rospy.Publisher(self.robot_host + '/steer/get/min', Steer, queue_size=10)
self.steerMinPWMPub = rospy.Publisher(self.robot_host + '/steer/get/min/pwm', Steer, queue_size=10)
self.steerMinDegreePub = rospy.Publisher(self.robot_host + '/steer/get/min/degree', ServoDeg, queue_size=10)
self.steerMaxPub = rospy.Publisher(self.robot_host + '/steer/get/max', Steer, queue_size=10)
self.steerMaxPWMPub = rospy.Publisher(self.robot_host + '/steer/get/max/pwm', Steer, queue_size=10)
self.steerMaxDegreePub = rospy.Publisher(self.robot_host + '/steer/get/max/degree', ServoDeg, queue_size=10)
self.steerNeutralPub = rospy.Publisher(self.robot_host + '/steer/get/neutral', Steer, queue_size=10)
self.steerNeutralPWMPub = rospy.Publisher(self.robot_host + '/steer/get/neutral/pwm', Steer, queue_size=10)
self.steerNeutralDegreePub = rospy.Publisher(self.robot_host + '/steer/get/neutral/degree', ServoDeg, queue_size=10)
self.steerRangePub = rospy.Publisher(self.robot_host + '/steer/get/range', ServoRange, queue_size=10)
self.steerRangePWMPub = rospy.Publisher(self.robot_host + '/steer/get/range/pwm', ServoRange, queue_size=10)
self.steerRangeDegreePub = rospy.Publisher(self.robot_host + '/steer/get/range/degree', ServoRangeDeg, queue_size=10)
self.rate = rospy.Rate(10) # 10hz
if self.enable:
self.start()
else:
self.stop()
def __init__(self, x=0, y=0, width=200):
self.left_pulley_moving = False
self.right_pulley_moving = False
self.x = x
self.y = y
self.width = width
self.left_pulley_position = hypotenuse(x, y)
self.right_pulley_position = hypotenuse(width - x, y)
self.left_pulley = PulleyMotor(parent=self,
direction_pin=7,
step_pin=8,
backwards=True)
self.right_pulley = PulleyMotor(parent=self,
direction_pin=24,
step_pin=23)
self.pen = ServoMotor()
self.pulley_queue = multiprocessing.Queue()
self.points_to_draw = []
def __init__(self, x=0, y=0, width=200):
self.left_pulley_moving = False
self.right_pulley_moving = False
self.x = x
self.y = y
self.width = width
self.left_pulley_position = hypotenuse(x, y)
self.right_pulley_position = hypotenuse(width-x, y)
self.left_pulley = PulleyMotor(parent=self, direction_pin=7, step_pin=8, backwards=True)
self.right_pulley = PulleyMotor(parent=self, direction_pin=24, step_pin=23)
self.pen = ServoMotor()
self.pulley_queue = multiprocessing.Queue()
self.points_to_draw = []
class StippleManager(object):
"""
Current hardware configuration:
For x and y coordinates, 500 = 1/4 inch
For width, 38000 = 19"
"""
def __init__(self, x=0, y=0, width=200):
self.left_pulley_moving = False
self.right_pulley_moving = False
self.x = x
self.y = y
self.width = width
self.left_pulley_position = hypotenuse(x, y)
self.right_pulley_position = hypotenuse(width - x, y)
self.left_pulley = PulleyMotor(parent=self,
direction_pin=7,
step_pin=8,
backwards=True)
self.right_pulley = PulleyMotor(parent=self,
direction_pin=24,
step_pin=23)
self.pen = ServoMotor()
self.pulley_queue = multiprocessing.Queue()
self.points_to_draw = []
def add_points(self, points):
self.points_to_draw += points
def get_coords(self):
return "%s, %s" % (self.x, self.y)
def start_drawing(self):
drawing_thread = threading.Thread(target=self.draw)
def stop_drawing(self):
self._stop_drawing = True
def draw(self):
if self._stop_drawing:
# We received a message to stop drawing, so stop drawing dots and reset the message
self._stop_drawing = False
else:
# Pop a point off the front of our points array
if self.points_to_draw:
next_point = self.points_to_draw.pop(0)
new_x = next_point[0]
new_y = next_point[1]
# Calculate pulley lengths for new coords
new_left_pulley_position = hypotenuse(new_x, new_y)
new_right_pulley_position = hypotenuse(self.width - new_x,
new_y)
# Figure out diff between old and new pulley lengths now so
# that properties on self can be set before pulleys are moved
diff_lpp = new_left_pulley_position - self.left_pulley_position
diff_rpp = new_right_pulley_position - self.right_pulley_position
print "Going to move %s %s" % (diff_lpp, diff_rpp)
# Setting properties before actually moving pulleys
self.left_pulley_position = new_left_pulley_position
self.right_pulley_position = new_right_pulley_position
self.x = new_x
self.y = new_y
if new_x > 0 and new_x < self.width and new_y > 0:
self.left_pulley_moving = True
self.right_pulley_moving = True
multiprocessing.Process(
target=self.left_pulley.move,
args=[self.pulley_queue, diff_lpp, True,
'first']).start()
multiprocessing.Process(
target=self.right_pulley.move,
args=[self.pulley_queue, diff_rpp, True,
'second']).start()
print "Waiting for result..."
result = self.pulley_queue.get(
) # waits until any of the proccess have `.put()` a result
result2 = self.pulley_queue.get()
print result
print result2
self.pen.tap()
# Draw next point
self.draw()
def setServo(self, servoName):
servo_neutral = self.getNeutral(servoName)
servo_min = self.getMin(servoName)
servo_max = self.getMax(servoName)
servo = self.getServo(servoName)
servo_pwm = servo_neutral
intervall = 5
if servoName == "steering":
charMax = "d"
charMin = "a"
turn = "turn right / left"
servoChannel = 0
elif servoName == "pan":
charMax = "a"
charMin = "d"
turn = "turn left / right"
servoChannel = 4
elif servoName == "tilt":
charMax = "s"
charMin = "w"
turn = "turn down / up"
servoChannel = 3
# channel, pwm_neutral, pwm_min, pwm_max, intervall
servoMotor = ServoMotor(servoChannel, servo_neutral, servo_min,
servo_max, intervall)
servoMotor.set_current_pwm(servo_neutral)
for current in servo:
if (current == 'neutral'):
servo_pwm = servo_neutral
elif (current == 'min'):
servo_pwm = servo_min
elif (current == 'max'):
servo_pwm = servo_max
self.__printScreen(charMin, charMax, turn, servoName, current,
servo_pwm)
# pwm.set_pwm(servoChannel, 0, servo_pwm)
servoMotor.set_current_pwm(servo_pwm)
while True:
char = self.__getch()
if (char == charMin):
servo_pwm = servo_pwm - intervall
servoMotor.set_current_pwm(servo_pwm)
self.__printScreen(charMin, charMax, turn, servoName,
current, servo_pwm)
if (char == charMax):
servo_pwm = servo_pwm + intervall
servoMotor.set_current_pwm(servo_pwm)
self.__printScreen(charMin, charMax, turn, servoName,
current, servo_pwm)
if (char == "x"):
print("x pressed")
self.setServoValue(servoName, current, servo_pwm)
self.writeConifgFile()
servoMotor.set_current_pwm(servo_neutral)
break
print("Exit program...")
char = ""
print("w/s: turn up / down")
print("a/d: turn left / right")
print("x: Exit program")
print("=== PWM value of the servo motors ===")
print("PWM value pan servo: ", pan.get_current_pwm())
print("PWM value tilt servo: ", tilt.get_current_pwm())
#servo2 pan
#servo1 tilt
if __name__ == "__main__":
conf = ServoConfig()
# channel, pwm_neutral, pwm_min, pwm_max, intervall
mg996r = ServoMotor(0, conf.getNeutral('steering'),
conf.getMin('steering'), conf.getMax('steering'), 5)
pan = ServoMotor(4, conf.getNeutral('pan'), conf.getMin('pan'),
conf.getMax('pan'), 5, -90.0, 90.0)
tilt = ServoMotor(3, conf.getNeutral('tilt'), conf.getMin('tilt'),
conf.getMax('tilt'), 5, -45.0, 45.0)
print("w/s: turn up / down")
print("a/d: turn left / right")
print("x: Exit program")
while True:
char = getch()
# The Pan-Tilt-Kit turns up.
if (char == "w"):
import os
import sys
import getpass
env = os.path.expanduser(
os.path.expandvars('/home/' + getpass.getuser() + '/robotcar/config'))
sys.path.insert(0, env)
from servo_config import ServoConfig
env2 = os.path.expanduser(
os.path.expandvars('/home/' + getpass.getuser() +
'/robotcar/driver/actor'))
sys.path.insert(1, env2)
from servo import ServoMotor
if __name__ == "__main__":
conf = ServoConfig()
# channel, pwm_neutral, pwm_min, pwm_max, intervall
mg996r = ServoMotor(0, conf.getNeutral('steering'),
conf.getMin('steering'), conf.getMax('steering'), 5)
print("a/d: turn left / right")
print("x: Exit program")
mg996r.test_servo()
class StippleManager(object):
"""
Current hardware configuration:
For x and y coordinates, 500 = 1/4 inch
For width, 38000 = 19"
"""
def __init__(self, x=0, y=0, width=200):
self.left_pulley_moving = False
self.right_pulley_moving = False
self.x = x
self.y = y
self.width = width
self.left_pulley_position = hypotenuse(x, y)
self.right_pulley_position = hypotenuse(width-x, y)
self.left_pulley = PulleyMotor(parent=self, direction_pin=7, step_pin=8, backwards=True)
self.right_pulley = PulleyMotor(parent=self, direction_pin=24, step_pin=23)
self.pen = ServoMotor()
self.pulley_queue = multiprocessing.Queue()
self.points_to_draw = []
def add_points(self, points):
self.points_to_draw += points
def get_coords(self):
return "%s, %s" % (self.x, self.y)
def start_drawing(self):
drawing_thread = threading.Thread(target=self.draw)
def stop_drawing(self):
self._stop_drawing = True
def draw(self):
if self._stop_drawing:
# We received a message to stop drawing, so stop drawing dots and reset the message
self._stop_drawing = False
else:
# Pop a point off the front of our points array
if self.points_to_draw:
next_point = self.points_to_draw.pop(0)
new_x = next_point[0]
new_y = next_point[1]
# Calculate pulley lengths for new coords
new_left_pulley_position = hypotenuse(new_x, new_y)
new_right_pulley_position = hypotenuse(self.width-new_x, new_y)
# Figure out diff between old and new pulley lengths now so
# that properties on self can be set before pulleys are moved
diff_lpp = new_left_pulley_position - self.left_pulley_position
diff_rpp = new_right_pulley_position - self.right_pulley_position
print "Going to move %s %s" % (diff_lpp, diff_rpp)
# Setting properties before actually moving pulleys
self.left_pulley_position = new_left_pulley_position
self.right_pulley_position = new_right_pulley_position
self.x = new_x
self.y = new_y
if new_x > 0 and new_x < self.width and new_y > 0:
self.left_pulley_moving = True
self.right_pulley_moving = True
multiprocessing.Process(target=self.left_pulley.move, args=[self.pulley_queue, diff_lpp, True, 'first']).start()
multiprocessing.Process(target=self.right_pulley.move, args=[self.pulley_queue, diff_rpp, True, 'second']).start()
print "Waiting for result..."
result = self.pulley_queue.get() # waits until any of the proccess have `.put()` a result
result2 = self.pulley_queue.get()
print result
print result2
self.pen.tap()
# Draw next point
self.draw()
from motioncontrol import MotionControl
from motionsensor import MotionSensor
from servo import ServoMotor
from ultrasonic import UltrsonicSensor
if __name__ == "__main__":
config = {}
with open('config.json') as f:
config = json.load(f)
motionControl = MotionControl(config['MultiStepperController'])
motionControl.setSpeed(1.5)
motionsensor = MotionSensor()
ultrasonicSensorFront = UltrsonicSensor(config["FrontUltrsonicSensor"])
ultrasonicSensorBack = UltrsonicSensor(config["BackUltrsonicSensor"])
servo = ServoMotor(config['HeadMountServo'])
servoAngle = 90
servo.rotate(servoAngle)
try:
while 1:
events = get_gamepad()
for event in events:
# print(event.ev_type, event.code, event.state)
if event.ev_type == 'Key' and event.code == 'BTN_NORTH' and event.state == 0:
print('Moving forward')
motionControl.move(0.1)
elif event.ev_type == 'Key' and event.code == 'BTN_EAST' and event.state == 0:
print('Moving backward')
motionControl.move(-0.1)
elif event.ev_type == 'Key' and event.code == 'BTN_Z' and event.state == 0:
import time
import sys
import json
from servo import ServoMotor
from ultrasonic import UltrsonicSensor
from motioncontrol import MotionControl
from motionsensor import MotionSensor
config = {}
with open('config.json') as f:
config = json.load(f)
ultrasonicSensorFront = UltrsonicSensor(config["FrontUltrsonicSensor"])
motionControl = MotionControl(config['MultiStepperController'])
servo = ServoMotor(config['HeadMountServo'])
servo.rotate(0)
motionSensor = MotionSensor()
def calibrateMovement(dist: float):
startAvg = 0
for i in range(10):
distance = ultrasonicSensorFront.getDistance()
time.sleep(0.1)
startAvg += distance
startAvg /= 10
motionControl.move(dist)
time.sleep(1)
def test2(photo):
from time import time
# t1 = time()
# draw_photo1(photo)
# t2 = time()
# print('time1: ' , round(t2 - t1, 2))
t1 = time()
draw_photo2(photo)
t2 = time()
print('time2: ', round(t2 - t1, 2))
if __name__ == '__main__':
stepper = StepperMotor()
servo = ServoMotor()
boundaries = Boundaries(left_down_corner=(-4.7, 5.85),
right_down_corner=(4.7, 5.85),
left_up_corner=(-4.7, 15.35),
right_up_corner=(4.7, 15.35))
pen = Pen()
photo = Photo('star')
test2(photo)
# test() #simple test
sleep(5)
# explicit deletation is need in order to call destructor.
# in destructors we reset positions of motors to 0 degrees and clean GPIO
del stepper
del servo
#!/usr/bin/env python
import os
import sys
import getpass
from config.servo_config import ServoConfig
env = os.path.expanduser(
os.path.expandvars('/home/' + getpass.getuser() +
'/robotcar/driver/actor'))
sys.path.insert(0, env)
from servo import ServoMotor
# Main function.
if __name__ == "__main__":
conf = ServoConfig()
steering = ServoMotor(0, conf.getNeutral('steering'),
conf.getMin('steering'), conf.getMax('steering'), 5)
pan = ServoMotor(4, conf.getNeutral('pan'), conf.getMin('pan'),
conf.getMax('pan'), 5)
tilt = ServoMotor(3, conf.getNeutral('tilt'), conf.getMin('tilt'),
conf.getMax('tilt'), 5)
steering.reset()
pan.reset()
tilt.reset()
class SteerNode(object):
"""Node example class."""
def __init__(self):
self.conf = ServoConfig()
self.mg996r = ServoMotor(0, self.conf.getNeutral('steering'), self.conf.getMin('steering'), self.conf.getMax('steering'), 5)
# Initialize message variables.
self.enable = False
self.pwm = self.conf.getNeutral('steering')
self.mg996r.set_current_pwm(self.pwm)
self.robot_host = re.sub("-", "_", socket.gethostname())
self.steerSub = rospy.Subscriber(self.robot_host + '/steer/set', Steer, self.steerCallback)
self.steerPWMSub = rospy.Subscriber(self.robot_host + '/steer/set/pwm', Steer, self.steerCallback)
self.steerDegressSub = rospy.Subscriber(self.robot_host + '/steer/set/degree', ServoDeg, self.steerDegreeCallback)
self.steerResetSub = rospy.Subscriber(self.robot_host + '/steer/reset', Bool, self.steerResetCallback)
self.steerPub = rospy.Publisher(self.robot_host + '/steer/get', Steer, queue_size=10)
self.steerPWMPub = rospy.Publisher(self.robot_host + '/steer/get/pwm', Steer, queue_size=10)
self.steerDegreePub = rospy.Publisher(self.robot_host + '/steer/get/degree', ServoDeg, queue_size=10)
self.steerIntervallPub = rospy.Publisher(self.robot_host + '/steer/get/intervall', ServoIntervall, queue_size=10)
self.steerIntervallPWMPub = rospy.Publisher(self.robot_host + '/steer/get/intervall/pwm', ServoIntervall, queue_size=10)
self.steerIntervallDegreePub = rospy.Publisher(self.robot_host + '/steer/get/intervall/degree', ServoIntervallDeg, queue_size=10)
self.steerChannelPub = rospy.Publisher(self.robot_host + '/steer/get/channel', Int8, queue_size=10)
self.steerMinPub = rospy.Publisher(self.robot_host + '/steer/get/min', Steer, queue_size=10)
self.steerMinPWMPub = rospy.Publisher(self.robot_host + '/steer/get/min/pwm', Steer, queue_size=10)
self.steerMinDegreePub = rospy.Publisher(self.robot_host + '/steer/get/min/degree', ServoDeg, queue_size=10)
self.steerMaxPub = rospy.Publisher(self.robot_host + '/steer/get/max', Steer, queue_size=10)
self.steerMaxPWMPub = rospy.Publisher(self.robot_host + '/steer/get/max/pwm', Steer, queue_size=10)
self.steerMaxDegreePub = rospy.Publisher(self.robot_host + '/steer/get/max/degree', ServoDeg, queue_size=10)
self.steerNeutralPub = rospy.Publisher(self.robot_host + '/steer/get/neutral', Steer, queue_size=10)
self.steerNeutralPWMPub = rospy.Publisher(self.robot_host + '/steer/get/neutral/pwm', Steer, queue_size=10)
self.steerNeutralDegreePub = rospy.Publisher(self.robot_host + '/steer/get/neutral/degree', ServoDeg, queue_size=10)
self.steerRangePub = rospy.Publisher(self.robot_host + '/steer/get/range', ServoRange, queue_size=10)
self.steerRangePWMPub = rospy.Publisher(self.robot_host + '/steer/get/range/pwm', ServoRange, queue_size=10)
self.steerRangeDegreePub = rospy.Publisher(self.robot_host + '/steer/get/range/degree', ServoRangeDeg, queue_size=10)
self.rate = rospy.Rate(10) # 10hz
if self.enable:
self.start()
else:
self.stop()
def start(self):
"""Turn on publisher and subscriber."""
self.enable = True
self.steerSub = rospy.Subscriber(self.robot_host + '/steer/set', Steer, self.steerCallback)
self.steerPWMSub = rospy.Subscriber(self.robot_host + '/steer/set/pwm', Steer, self.steerCallback)
self.steerDegressSub = rospy.Subscriber(self.robot_host + '/steer/set/degree', ServoDeg, self.steerDegreeCallback)
self.steerResetSub = rospy.Subscriber(self.robot_host + '/steer/reset', Bool, self.steerResetCallback)
self.steerPub = rospy.Publisher(self.robot_host + '/steer/get', Steer, queue_size=10)
self.steerPWMPub = rospy.Publisher(self.robot_host + '/steer/get/pwm', Steer, queue_size=10)
self.steerDegreePub = rospy.Publisher(self.robot_host + '/steer/get/degree', ServoDeg, queue_size=10)
self.steerIntervallPub = rospy.Publisher(self.robot_host + '/steer/get/intervall', ServoIntervall, queue_size=10)
self.steerIntervallPWMPub = rospy.Publisher(self.robot_host + '/steer/get/intervall/pwm', ServoIntervall, queue_size=10)
self.steerIntervallDegreePub = rospy.Publisher(self.robot_host + '/steer/get/intervall/degree', ServoIntervallDeg, queue_size=10)
self.steerChannelPub = rospy.Publisher(self.robot_host + '/steer/get/channel', Int8, queue_size=10)
self.steerMinPub = rospy.Publisher(self.robot_host + '/steer/get/min', Steer, queue_size=10)
self.steerMinPWMPub = rospy.Publisher(self.robot_host + '/steer/get/min/pwm', Steer, queue_size=10)
self.steerMinDegreePub = rospy.Publisher(self.robot_host + '/steer/get/min/degree', ServoDeg, queue_size=10)
self.steerMaxPub = rospy.Publisher(self.robot_host + '/steer/get/max', Steer, queue_size=10)
self.steerMaxPWMPub = rospy.Publisher(self.robot_host + '/steer/get/max/pwm', Steer, queue_size=10)
self.steerMaxDegreePub = rospy.Publisher(self.robot_host + '/steer/get/max/degree', ServoDeg, queue_size=10)
self.steerNeutralPub = rospy.Publisher(self.robot_host + '/steer/get/neutral', Steer, queue_size=10)
self.steerNeutralPWMPub = rospy.Publisher(self.robot_host + '/steer/get/neutral/pwm', Steer, queue_size=10)
self.steerNeutralDegreePub = rospy.Publisher(self.robot_host + '/steer/get/neutral/degree', ServoDeg, queue_size=10)
self.steerRangePub = rospy.Publisher(self.robot_host + '/steer/get/range', ServoRange, queue_size=10)
self.steerRangePWMPub = rospy.Publisher(self.robot_host + '/steer/get/range/pwm', ServoRange, queue_size=10)
self.steerRangeDegreePub = rospy.Publisher(self.robot_host + '/steer/get/range/degree', ServoRangeDeg, queue_size=10)
while not rospy.is_shutdown():
steerMsg = Steer()
steerDegMsg = ServoDeg()
steerIntervallMsg = ServoIntervall()
steerIntervallDegMsg = ServoIntervallDeg()
steerChannelMsg = Int8()
steerMinMsg = Steer()
steerMinDegMsg = ServoDeg()
steerMaxMsg = Steer()
steerMaxDegMsg = ServoDeg()
steerNeutralMsg = Steer()
steerNeutralDegMsg = ServoDeg()
steerRangeMsg = ServoRange()
steerRangeDegMsg = ServoRangeDeg()
steerMsg.pwm = self.mg996r.get_current_pwm()
steerDegMsg.angle = self.mg996r.get_current_degree()
steerIntervallMsg.intervall_pwm = self.mg996r.get_intervall_pwm()
steerIntervallDegMsg.intervall_angle = self.mg996r.get_intervall_degree()
steerChannelMsg.data = self.mg996r.get_channel()
steerMinMsg.pwm = self.mg996r.get_servo_min()
steerMinDegMsg.angle = self.mg996r.get_servo_min_degree()
steerMaxMsg.pwm = self.mg996r.get_servo_max()
steerMaxDegMsg.angle = self.mg996r.get_servo_max_degree()
steerNeutralMsg.pwm = self.mg996r.get_servo_neutral()
steerNeutralDegMsg.angle = 0.0
steerRangeMsg.range_pwm = self.mg996r.get_servo_range_pwm()
steerRangeDegMsg.range_angle = self.mg996r.get_servo_range_degree()
self.steerPub.publish(steerMsg)
self.steerPWMPub.publish(steerMsg)
self.steerDegreePub.publish(steerDegMsg)
self.steerIntervallPub.publish(steerIntervallMsg)
self.steerIntervallPWMPub.publish(steerIntervallMsg)
self.steerIntervallDegreePub.publish(steerIntervallDegMsg)
self.steerChannelPub.publish(steerChannelMsg)
self.steerMinPub.publish(steerMinMsg)
self.steerMinPWMPub.publish(steerMinMsg)
self.steerMinDegreePub.publish(steerMinDegMsg)
self.steerMaxPub.publish(steerMaxMsg)
self.steerMaxPWMPub.publish(steerMaxMsg)
self.steerMaxDegreePub.publish(steerMaxDegMsg)
self.steerNeutralPub.publish(steerNeutralMsg)
self.steerNeutralPWMPub.publish(steerNeutralMsg)
self.steerNeutralDegreePub.publish(steerNeutralDegMsg)
self.steerRangePub.publish(steerRangeMsg)
self.steerRangePWMPub.publish(steerRangeMsg)
self.steerRangeDegreePub.publish(steerRangeDegMsg)
self.rate.sleep()
def stop(self):
"""Turn off publisher and subscriber."""
self.enable = False
# self.mg996r.reset()
self.steerSub.unregister()
self.steerPWMSub.unregister()
self.steerDegressSub.unregister()
self.steerResetSub.unregister()
self.steerPub.unregister()
self.steerPWMPub.unregister()
self.steerDegreePub.unregister()
self.steerIntervallPub.unregister()
self.steerIntervallPWMPub.unregister()
self.steerIntervallDegreePub.unregister()
self.steerChannelPub.unregister()
self.steerMinPub.unregister()
self.steerMinPWMPub.unregister()
self.steerMinDegreePub.unregister()
self.steerMaxPub.unregister()
self.steerMaxPWMPub.unregister()
self.steerMaxDegreePub.unregister()
self.steerNeutralPub.unregister()
self.steerNeutralPWMPub.unregister()
self.steerNeutralDegreePub.unregister()
self.steerRangePub.unregister()
self.steerRangePWMPub.unregister()
self.steerRangeDegreePub.unregister()
def steerCallback(self, data):
"""Handle subscriber data."""
self.pwm = data.pwm
self.mg996r.set_current_pwm(int(self.pwm))
rospy.loginfo(rospy.get_caller_id() + ' Set steering angle to %s', self.pwm)
def steerDegreeCallback(self, data):
"""Handle subscriber data."""
self.mg996r.set_current_degree(data.angle) # set degree
self.pwm = self.mg996r.get_current_pwm() # get pwm
rospy.loginfo(rospy.get_caller_id() + ' Got steering angle degree %s and set angle to %s' % (data.angle, self.pwm))
def steerResetCallback(self, data):
"""Handle subscriber data."""
if(data.data == True):
self.pwm = self.conf.getNeutral('steering')
self.mg996r.set_current_pwm(self.pwm)
rospy.loginfo(rospy.get_caller_id() + ' Steering reseted to angle %s', self.pwm)