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