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