def __init__(self):
# valori di velocità angolare e lineare precedenti , inizializzati a 0 e 0.1
self.previous_angular = 0
self.previous_linear = 0.1
rospy.Subscriber("cmd_vel_topic", Twist, self.cmd_vel_callback)
# creazione di un oggetto della classe MotorDriver
self.motion_driver = MotorDriver()
class RobotMover(object):
def __init__(self):
# valori di velocità angolare e lineare precedenti , inizializzati a 0 e 0.1
self.previous_angular = 0
self.previous_linear = 0.1
rospy.Subscriber("cmd_vel_topic", Twist, self.cmd_vel_callback)
# creazione di un oggetto della classe MotorDriver
self.motion_driver = MotorDriver()
# callback invocata ogni qual volta venga pubblicato qualcosa sul topic "cmd_vel_topic"
def cmd_vel_callback(self, data):
# velocità lineare e angolare pubblicate sul topic
linear_speed = data.linear.x
angular_speed = data.angular.z
# se sono dei valori di default, si passano a change speed le velocità precedenti, continua a fare quello che faceva
if (linear_speed == -1000) or (angular_speed == -1000) or (
linear_speed == 1000) or (angular_speed == 1000):
self.motion_driver.change_speed(self.previous_linear,
self.previous_angular)
print("{ROBOT_MOVER} Linear: " + str(self.previous_linear) +
", Angular: " + str(self.previous_angular))
else: # se le velocità sono cambiate, aggiorno i valori di quelle precedenti e li passo a change_speed
self.previous_linear = linear_speed
self.previous_angular = angular_speed
self.motion_driver.change_speed(linear_speed, angular_speed)
print("{ROBOT_MOVER} Linear: " + str(linear_speed) +
", Angular: " + str(angular_speed))
def run():
md = MotorDriver()
while True:
time.sleep(10)
md.motor_send(1, 10, 'fwd')
time.sleep(10)
class TantrumMotors:
"""
This class is designed to control the motors of a Tyco Tantrum
RC car. The Tantrum has a front motor controlling direction and
a rear motor for moving forwards and backwards.
"""
_front = 0 # Motor
_rear = 0 # Motor
_frontDriver = MotorDriver("P9_11", "P9_14")
_rearDriver = MotorDriver("P9_12", "P9_16")
# <public>
# Initialize motors to forward, stopped
def __init__(self):
self._front = Motor(self._frontDriver)
self._rear = Motor(self._rearDriver)
self._front.setDirCw()
self._front.setSpeed(0)
self._rear.setDirCcw()
self._rear.setSpeed(0)
# Set the speed for all motors, if a speed is negative then the
# motor is put in reverse.
def setSpeed(self, front, rear):
front = int(front)
rear = int(rear)
self._front.setSpeed(front)
if (front < 0):
self._front.setDirCcw()
else:
self._front.setDirCw()
self._rear.setSpeed(rear)
if (rear < 0):
self._rear.setDirCcw()
else:
self._rear.setDirCw()
def getFrontSpeed(self):
return self._front.getSpeed()
def getRearSpeed(self):
return self._rear.getSpeed()
def getFrontDirection(self):
return self._front.getDirection()
def getRearDirection(self):
return self._rear.getDirection()
def updateMotorState(self):
self._frontDriver.updateState()
self._rearDriver.updateState()
def __exit__(self, type, value, traceback):
self._frontDriver.shutdown()
self._rearDriver.shutdown()
def __init__(self):
rospy.Subscriber("/morpheus_bot/cmd_vel", Twist, self.cmd_vel_callback)
self.motor_driver = MotorDriver()
rospy.wait_for_service('/raspicam_node/start_capture')
start_cam = rospy.ServiceProxy('/raspicam_node/start_capture', Empty)
request_e = EmptyRequest()
start_cam(request_e)
rospy.loginfo("Started Camera")
def __init__(self, value_BASE_PWM, value_MULTIPLIER_STANDARD,
value_MULTIPLIER_PIVOT, value_simple_mode):
rospy.Subscriber("/mowgli/cmd_vel", Twist, self.cmd_vel_callback)
self.motor_driver = MotorDriver(
i_BASE_PWM=value_BASE_PWM,
i_MULTIPLIER_STANDARD=value_MULTIPLIER_STANDARD,
i_MULTIPLIER_PIVOT=value_MULTIPLIER_PIVOT,
simple_mode=value_simple_mode)
rospy.loginfo("RobotMover Started...")
def __init__(self, value_BASE_PWM, value_MULTIPLIER_STANDARD,
value_MULTIPLIER_PIVOT, value_simple_mode):
rospy.Subscriber("/morpheus_bot/cmd_vel", Twist, self.cmd_vel_callback)
self.motor_driver = MotorDriver(
i_BASE_PWM=value_BASE_PWM,
i_MULTIPLIER_STANDARD=value_MULTIPLIER_STANDARD,
i_MULTIPLIER_PIVOT=value_MULTIPLIER_PIVOT,
simple_mode=value_simple_mode)
rospy.wait_for_service('/raspicam_node/start_capture')
rospy.loginfo("RobotMover Started...")
class RobotMover(object):
def __init__(self):
rospy.Subscriber("wheel", Twist, self.cmd_vel_callback)
self.motion_driver = MotorDriver()
def cmd_vel_callback(self, data):
linear_speed = data.linear.x
angular_speed = data.angular.z
rospy.loginfo(data.linear)
self.motion_driver.change_speed(linear_speed, angular_speed)
def listener(self):
rospy.spin()
def __init__(self, robot_width=0.16, diameter=0.066, rmp_to_pwm_gain=0.15):
self.robot_width = robot_width
self.diameter = diameter
self.rmp_to_pwm_gain = rmp_to_pwm_gain
self.vx = 0.0
self.vth = 0.0
self.vx_max = 1.0
self.vth_max = 1.0
self.cmd_vel_sub = rospy.Subscriber("cmd_vel", Twist,
self.cmd_vel_callback)
self.lwheel_pub = rospy.Publisher('lwheel_rpm', Int32, queue_size=1)
self.rwheel_pub = rospy.Publisher('rwheel_rpm', Int32, queue_size=1)
self.current_time = None
self.last_cmd_time = None
self.motor = MotorDriver()
class RobotMover(object):
def __init__(self):
self.previous_angular = 0
self.previous_linear = 0.1
rospy.Subscriber("cmd_vel_topic", Twist, self.cmd_vel_callback2)
self.motion_driver = MotorDriver()
def cmd_vel_callback2(self, data):
linear_speed = data.linear.x
angular_speed = data.angular.z
if (linear_speed == -1000) and (angular_speed == -1000 or angular_speed == 1000):
self.motion_driver.change_speed(self.previous_linear, self.previous_angular)
print("{ROBOT_MOVER} Linear: " + str(self.previous_linear) + ", Angular: " + str(self.previous_angular))
else:
self.previous_linear = linear_speed
self.previous_angular = angular_speed
self.motion_driver.change_speed(linear_speed, angular_speed)
print("{ROBOT_MOVER} Linear: " + str(linear_speed) + ", Angular: " + str(angular_speed))
class RobotMover(object):
def __init__(self):
rospy.Subscriber("/morpheus_bot/cmd_vel", Twist, self.cmd_vel_callback)
self.motor_driver = MotorDriver()
rospy.wait_for_service('/raspicam_node/start_capture')
start_cam = rospy.ServiceProxy('/raspicam_node/start_capture', Empty)
request_e = EmptyRequest()
start_cam(request_e)
rospy.loginfo("Started Camera")
def cmd_vel_callback(self, msg):
linear_speed = msg.linear.x
angular_speed = msg.angular.z
# Decide Speed
self.motor_driver.set_cmd_vel(linear_speed, angular_speed)
def listener(self):
rospy.spin()
class RobotMover(object):
def __init__(self, value_BASE_PWM, value_MULTIPLIER_STANDARD,
value_MULTIPLIER_PIVOT, value_simple_mode):
rospy.Subscriber("/mowgli/cmd_vel", Twist, self.cmd_vel_callback)
self.motor_driver = MotorDriver(
i_BASE_PWM=value_BASE_PWM,
i_MULTIPLIER_STANDARD=value_MULTIPLIER_STANDARD,
i_MULTIPLIER_PIVOT=value_MULTIPLIER_PIVOT,
simple_mode=value_simple_mode)
rospy.loginfo("RobotMover Started...")
def cmd_vel_callback(self, msg):
linear_speed = msg.linear.x
angular_speed = msg.angular.z
# Decide Speed
self.motor_driver.set_cmd_vel(linear_speed, angular_speed)
def listener(self):
rospy.spin()
class RoverpiController:
def __init__(self, robot_width=0.16, diameter=0.066, rmp_to_pwm_gain=0.15):
self.robot_width = robot_width
self.diameter = diameter
self.rmp_to_pwm_gain = rmp_to_pwm_gain
self.vx = 0.0
self.vth = 0.0
self.vx_max = 1.0
self.vth_max = 1.0
self.cmd_vel_sub = rospy.Subscriber("cmd_vel", Twist,
self.cmd_vel_callback)
self.lwheel_pub = rospy.Publisher('lwheel_rpm', Int32, queue_size=1)
self.rwheel_pub = rospy.Publisher('rwheel_rpm', Int32, queue_size=1)
self.current_time = None
self.last_cmd_time = None
self.motor = MotorDriver()
def cmd_vel_callback(self, msg):
self.vx = max(min(msg.linear.x, self.vx_max), -self.vx_max)
self.vth = max(min(msg.angular.z, self.vth_max), -self.vth_max)
self.last_cmd_time = rospy.Time.now()
def run(self):
rate = rospy.Rate(10)
while not rospy.is_shutdown():
self.current_time = rospy.Time.now()
if self.last_cmd_time == None:
rospy.loginfo("Waiting for command.")
self.lwheel_pub.publish(0)
self.rwheel_pub.publish(0)
elif (self.current_time - self.last_cmd_time).to_sec() > 1.0:
rospy.loginfo("Did not get command for 1 second, stopping.")
try:
self.motor.e_stop()
self.lwheel_pub.publish(0)
self.rwheel_pub.publish(0)
except OSError as e:
rospy.logerr("Could not stop")
rospy.logdebug(e)
else:
if abs(self.vx) < 0.01 and abs(self.vth) < 0.01:
left_vel = right_vel = 0
elif abs(self.vx) < 0.01:
right_vel = self.vth * self.robot_width / 2.0
left_vel = -right_vel
elif abs(self.vth) < 0.01:
left_vel = right_vel = self.vx
else:
left_vel = self.vx - self.vth / 2.0
right_vel = self.vx + self.vth / 2.0
RPMleft = int((60 * left_vel) / (self.diameter * math.pi))
RPMright = int((60 * right_vel) / (self.diameter * math.pi))
self.lwheel_pub.publish(RPMleft)
self.rwheel_pub.publish(RPMright)
addon = 50.0 # dc to be added
if RPMleft > 0 and RPMright > 0:
self.rmp_to_pwm_gain = addon / (RPMleft + RPMright)
PWMleft = 50 + self.rmp_to_pwm_gain * RPMleft
PWMright = 50 + self.rmp_to_pwm_gain * RPMright
self.motor.left_wheel(PWMleft, 1000)
self.motor.right_wheel(PWMright, 1000)
elif RPMleft < 0 and RPMright < 0:
self.rmp_to_pwm_gain = -addon / (RPMleft + RPMright)
PWMleft = 50 + self.rmp_to_pwm_gain * -RPMleft
PWMright = 50 + self.rmp_to_pwm_gain * -RPMright
self.motor.left_wheel(PWMleft, 1000, reverse=True)
self.motor.right_wheel(PWMright, 1000, reverse=True)
elif RPMleft > 0 and RPMright < 0:
self.rmp_to_pwm_gain = addon / (RPMleft - RPMright)
PWMleft = 50 + self.rmp_to_pwm_gain * RPMleft
PWMright = 50 + self.rmp_to_pwm_gain * -RPMright
self.motor.left_wheel(PWMleft, 1000)
self.motor.right_wheel(PWMright, 1000, reverse=True)
elif RPMleft < 0 and RPMright > 0:
self.rmp_to_pwm_gain = addon / (-RPMleft + RPMright)
PWMleft = 50 + self.rmp_to_pwm_gain * -RPMleft
PWMright = 50 + self.rmp_to_pwm_gain * RPMright
self.motor.left_wheel(PWMleft, 1000, reverse=True)
self.motor.right_wheel(PWMright, 1000)
else:
self.motor.e_stop()
rate.sleep()
from motor_driver import MotorDriver
from servo_driver import ServoDriver
from arm_driver import ArmDriver
md = MotorDriver()
sd = ServoDriver()
ad = ArmDriver()
def drive():
for i in range(3):
md.motor_send(1, 75, 'fwd')
def turn():
for i in range(2):
md.motor_send(1, 90, 'right')
def pickup():
ad.pickup()
def up():
ad.up()
def down():
ad.down()
from motor_driver import MotorDriver from time import sleep motor = MotorDriver() motor.run(0, 0, 75) sleep(1) motor.run(0, 1, 75) sleep(1) motor.stop(0) motor.run(1, 0, 75) sleep(1) motor.run(1, 1, 75) sleep(1) motor.stop(1)
def main():
js = Joystick()
md = MotorDriver()
KEEP_DRIVING = True
try:
while KEEP_DRIVING:
js.get_event()
x, y = js.get_left_x_y()
x_r, y_r = js.get_right_x_y()
# Halt if Lstick in center
if y < EPSILON_FWD and y > -EPSILON_FWD\
and x < EPSILON_FWD and x > -EPSILON_FWD:
md.halt()
# Back/forward
if y > EPSILON_FWD: # y is positive when pushed down
md.go_back()
if x_r > EPSILON_FWD:
md.turn_1pt_back_right()
elif x_r < -EPSILON_FWD:
md.turn_1pt_back_left()
elif y < -EPSILON_FWD:
md.go_forward()
# 1 point Left/Right
if x_r > EPSILON_FWD:
md.turn_1pt_forward_right()
elif x_r < -EPSILON_FWD:
md.turn_1pt_forward_left()
# Spot Left/Spot Right
if x > EPSILON_FWD:
md.turn_right()
elif x < -EPSILON_FWD:
md.turn_left()
except KeyboardInterrupt as e:
KEEP_DRIVING = False
md.halt()
print("\nExiting...")
from fust_detection import FustDetector
from qr_code import QR
from fust_recognizer import FustRecognizer
from steppermotor_controller import StepperMotorController
from motor_driver import MotorDriver
from GUI import GUI
from crop_foto import crop_image
from PIL import Image
configLoader = ConfigLoader()
configLoader.load_configuration()
fust_detector = FustDetector()
qr = QR()
fust_recognizer = FustRecognizer()
steppermotor_controller = StepperMotorController()
motor_driver = MotorDriver()
gui = GUI()
def onDetection(image):
print('fust')
qr_data = qr.scanQrCode(image)
height = steppermotor_controller.getHeightForQRCode(qr_data)
motor_driver.rotate(height)
name = str(datetime.datetime.now()).replace(" ", "")
command = "fswebcam -d /dev/video0 -r 1600x1200 --rotate 90 " + name + ".jpg"
os.system(command)
img = Image.open(name + ".jpg")
crop_image(name + ".jpg")
img = img.resize((800, 800), Image.ANTIALIAS)
gui.setImage(img)
def __init__(self):
rospy.Subscriber("wheel", Twist, self.cmd_vel_callback)
self.motion_driver = MotorDriver()
#!/usr/bin/env python3
from ultrasonic_driver import UltrasonicDriver
from motor_driver import MotorDriver
import time
md = MotorDriver()
time.sleep(15)
for i in range(6):
print('sending', i)
md.motor_send(1,100,'fwd')
time.sleep(10)
print('stopping')
md.stop()
from motor_driver import MotorDriver
from arm_driver import ArmDriver
from servo_driver import ServoDriver
import time
import random as r
md = MotorDriver()
sd = ServoDriver()
ad = ArmDriver()
while True:
dist = r.randint(15, 50)
print("dist is", dist)
speed = 1
print("speed is ", speed)
md.motor_send(speed, dist, 'fwd')
time.sleep(dist * 0.1)
func = r.randint(1, 3)
if func == 1:
sub_func = r.randint(1, 2)
if sub_func == 1:
if ad.state == 'up':
ad.down()
else:
ad.up()
else:
ad.pickup()
elif func == 2:
sub_func = r.randint(1, 2)
def __init__(self):
self.previous_angular = 0
self.previous_linear = 0.1
rospy.Subscriber("cmd_vel_topic", Twist, self.cmd_vel_callback2)
self.motion_driver = MotorDriver()
elif line[2] == '1':
left_motor.spin(90)
right_motor.spin(100)
elif line[3] == '1':
left_motor.spin(100)
right_motor.spin(90)
elif line[4] == '1':
left_motor.spin(100)
right_motor.spin(40)
elif line[5] == '1':
left_motor.spin(100)
right_motor.spin(-50)
else:
left_motor.spin(20)
right_motor.spin(20)
# Press the green button in the gutter to run the script.
if __name__ == '__main__':
arduino = Arduino('ttyUSB0')
print("scan rfid")
arrived_guest = recognize_user(rfid_reader=arduino)
left_motor = MotorDriver(13, 5, 6)
right_motor = MotorDriver(12, 20, 16)
follow_line(arduino, left_motor, right_motor,
arrived_guest.get_destination())
arduino.close_connection()
echo_1, trigger_1 = 19, 13
echo_2, trigger_2 = 21, 20
echo_3, trigger_3 = 16, 12
distance_sensor_1 = DistanceSensor(echo=echo_1, trigger=trigger_1)
distance_sensor_2 = DistanceSensor(echo=echo_2, trigger=trigger_2)
distance_sensor_3 = DistanceSensor(echo=echo_3, trigger=trigger_3)
# d2
# d1----d3
# / \
# / \
threshold = 0.25
motor = MotorDriver()
classifier = PedestrianClassifier()
def move(direction, sleep_time):
if direction == "forward":
motor.run(0, 0, 50)
motor.run(1, 0, 50)
elif direction == "backward":
motor.run(0, 1, 50)
motor.run(1, 1, 50)
elif direction == "left":
motor.run(0, 0, 50)
motor.run(1, 1, 50)
elif direction == "right":
motor.run(0, 1, 50)
motor.run(1, 0, 50)
from motor_driver import MotorDriver
md = MotorDriver()
while True:
info = input("Enter Speed, Time, Direction:" )
input_dims = info.split()
speed = float(input_dims[0])
dur = float(input_dims[1])
dir = str(input_dims[2])
print(speed, dur, dir)
md.motor_send(speed, dur, dir)
def __init__(self):
super(Car, self).__init__()
self.m_drv = MotorDriver()
