コード例 #1
0
ファイル: roboarm.py プロジェクト: tinkerfuroc/tk2_control
class RoboArm(object):
    def __init__(self):
        self.joints = [
            PWMJoint(
                5, BASE_MIN_ANGLE, BASE_MIN_PWM, BASE_MAX_ANGLE, 
                BASE_MAX_PWM, BASE_COF1, BASE_COF0),
            PWMJoint(
                6, SHOULDER_MIN_ANGLE, SHOULDER_MIN_PWM, SHOULDER_MAX_ANGLE, 
                SHOULDER_MAX_PWM, SHOULDER_COF1, SHOULDER_COF0),
            PWMJoint(
                7, ELBOW_MIN_ANGLE, ELBOW_MIN_PWM, ELBOW_MAX_ANGLE, 
                ELBOW_MAX_PWM, ELBOW_COF1, ELBOW_COF0),
            DyMiJoint(
                1, WRIST_MIN_ANGLE, WRIST_MIN_PWM, WRIST_MAX_ANGLE, 
                WRIST_MAX_PWM, WRIST_COF1, WRIST_COF0)]
        self.motor = Motor(4)

    def move_arm(self, out_pos):
        self.motor.set_vel(out_pos.pos7)
        arm_pos = [0.] * 6
        for i in range(0, 4):
            legal, arm_pos[i] = joints[i].get_pos(arm_pose[i])
            if not legal:
                rospy.logerr('angle over bound for joint %d' % i)
            joints[i].set_pos(arm_pos[i])
    
    def close(self):
        for joint in self.joints:
            joint.close()
コード例 #2
0
ファイル: pathfinder.py プロジェクト: LukasOtis/steppingMotor
def main():
    log_file = "main_log.log"
    create_timed_rotating_log("log/" + log_file)
    logger = logging.getLogger("BasicLogger")
    logger.info("----- Starting Logging Session -----")
    config = configparser.ConfigParser()
    config.read('config.ini')

    # How to use the config values. REMOVE when done with setup of this!
    print(config.sections())
    print('scale of this whole thing from config file is: ' + config['grid']['scale'])
    # To get the values as integers:
    i = int(config['grid']['max_position_z'])
    print(i+2)

    # subprocess.call("../gcodepull.sh", shell=True)

    #opens the file named in the varibles file
    length = range(FileOperator.OpenFile()- 3)
    Motor.setup()
    start = 2
    for row in length:
            # for the appropiated length each row is worked through 
            # and the needet steps are sent to the stepper motors
            next_row = row + start
            delta_step = FileOperator.NextMove(next_row)
            corrected_coords = FileOperator.MoveCorrect(delta_step)
            Motor.move(corrected_coords)
            print('finished')
    GPIO.cleanup()
コード例 #3
0
ファイル: navigation.py プロジェクト: HugoCMU/SolarTree
def navigate():
	'''
		Function will call exploration() function to move robot, but will keep
		track of previous moves
	'''

	# Test output
	print("Navigating ...")

	# Create motor and buzzer objects
	motor = Motor()
	buzzer = Buzzer()

	# Beep to indicate begining of navigate step
	buzzer.play(4)

	try:
		# Enter the exploration loop
		for i in xrange(params.p['MAX_ITER']):

			# Execute explore function and save results
			explore(motor, buzzer)

			# Wait between moves
			time.sleep(params.p['WAIT_TIME'])
	except Exception:
		pass
	finally:
		motor.stop_bot()
コード例 #4
0
ファイル: 6.1.py プロジェクト: patrik-nilsson/Python
def main():
	Volvomotor=Motor(0,100,0,"Volvo","Stop")
	state=raw_input("Motor state: ")
	Volvomotor.changestate(state)
	Volvomotor.accelerate()
	print "Current speed: "+str(Volvomotor.rpm)+"rpm"
	return 0
コード例 #5
0
ファイル: tantrum_motors.py プロジェクト: Grated/Beaglebone
 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)
コード例 #6
0
ファイル: control.py プロジェクト: lewisling/yarapibabot
    def __init__(self, config):
        Thread.__init__(self)
        self.angle = 0
        self.K = 0.98
        self.logDataSetBuffer = []

        self.Kp = config.config.as_float('Kp')
        self.Ki = config.config.as_float('Ki')
        self.Kd = config.config.as_float('Kd')
        self.set_point = config.config.as_float('set_point')
        self.disable_motors = config.config.as_bool('disable_motors')
        
        self.integral_error = 0
        self.last_error = 0
        self.motor_left = Motor("L", port_motor_left_pwm, port_motor_left_backward, port_motor_left_forward)
        self.motor_right = Motor("R", port_motor_right_pwm, port_motor_right_backward, port_motor_right_forward)
        
        self.accelerometer = MPU6050()

        self.last_time = time()

        self.logger = logging.getLogger(__name__)

        self.setDaemon(True)
        self.latest_sensor = 0
        
        self.logger.info('Initialized ControlThread with the following settings')
        self.logger.info('disable_motors={}'.format(self.disable_motors))
        self.logger.info('set_point={:1.2f}'.format(self.set_point))
        self.logger.info('Kp={:1.1f}'.format(self.Kp))
        self.logger.info('Ki={:1.1f}'.format(self.Ki))
        self.logger.info('Kd={:1.1f}'.format(self.Kd))
コード例 #7
0
ファイル: motor_conductor.py プロジェクト: clubcapra/capra
    def __init__(self):
        self.motor_front_left = Motor(MotorConductor.FRONT, MotorConductor.LEFT)
        self.motor_front_right = Motor(MotorConductor.FRONT, MotorConductor.RIGHT)

        for ndx, motor in enumerate(self.get_motors()):
            motor.gear_ratio = Config.get_gear_ratio()
            motor.wheel_diameter = Config.get_wheel_diameter()
            motor.position_ratio = Config.get_position_ratio()[ndx]
            motor.speed_ratio = Config.get_speed_ratio()[ndx]
コード例 #8
0
ファイル: car_server.py プロジェクト: adityakamath/RobotBrain
class CarServer(object):

    def __init__(self, motor_pins=(24, 25), servo_pin=0, port=2012):
        self.port = port

        # The motor and servo for driving
        self.motor = Motor(*motor_pins)
        self.servo = Servo(servo_pin)
        
        # The most recent coordinates from the accelerameter
        self.coords = (0, 0, 0)

        # Whether or not to continue running the server
        self._run = True

        self.start()

    def start(self):
        """ Initialize and start threads. """

        self._server_thread = threading.Thread(target=self._server_worker)
        self._server_thread.start()
        self._control_thread = threading.Thread(target=self._control_worker)
        self._control_thread.start()

    def stop(self):
        """ Shut down server and control threads. """
        self._run = False

    def _server_worker(self):
        HOST = ''  # Symbolic name meaning all available interfaces
        PORT = self.port
        s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        s.bind((HOST, PORT))
        s.listen(1)
        conn, addr = s.accept()

        print 'Connected by', addr
        while self._run:
            data = conn.recv(1024)
            if data:
                coords = data[1:].split(',')
                x, y, z = [float(n) for n in coords]
                self.coords = (x, y, z)
            conn.sendall(data)
        conn.close()

    def _control_worker(self):
        while self._run:
            x, y, z = self.coords
            forward_speed = -y/10
            turning_power = (x+10)/20

            self.motor.drive(forward_speed)
            self.servo.set(turning_power)
コード例 #9
0
    def __init__(self, verbose=False):

        self.verbose = verbose

        capture = cv2.VideoCapture(0)
        capture.set(cv2.CAP_PROP_FRAME_WIDTH, 960)
        capture.set(cv2.CAP_PROP_FRAME_HEIGHT, 500)
        capture.set(cv2.CAP_PROP_BUFFERSIZE, 3)
        capture.set(cv2.CAP_PROP_FPS, 60)
        self.__capture_manager = CaptureManager(capture, 10)

        self.__io_manager = IOManager(IO_PINS, verbose)

        self.__image_analyzer = ImageAnalyzer(self.__capture_manager.current_frame_color, verbose)

        self.__coordinates_translator = CoordinatesTranslator(self.__image_analyzer, CONVEYOR_WIDTH)

        self.__packager = Packager(TRAY_SIZE, PADDING, verbose)

        self.__item_movement_motor = Motor(MOTOR_PINS['item_movement_control'],
                                           MOTOR_PINS['item_movement_direction'],
                                           ITEM_MOVEMENT_CONVEYOR_STEP_INTERVAL,
                                           ITEM_MOVEMENT_CONVEYOR_STEP_LIMIT,
                                           MOTOR_PINS['item_movement_limit_switch'],
                                           0.15,
                                           10,
                                           0.15,
                                           10,
                                           self.__io_manager,
                                           verbose)

        self.__io_manager.protect_pin(MOTOR_PINS['item_movement_control'])
        self.__io_manager.protect_pin(MOTOR_PINS['item_movement_direction'])
        self.__io_manager.protect_pin(MOTOR_PINS['item_movement_limit_switch'])

        self.__tray_movement_motor = Motor(MOTOR_PINS['tray_movement_control'],
                                           MOTOR_PINS['tray_movement_direction'],
                                           TRAY_MOVEMENT_CONVEYOR_STEP_INTERVAL,
                                           None,
                                           MOTOR_PINS['tray_movement_limit_switch'],
                                           0.15,
                                           10,
                                           0.15,
                                           10,
                                           self.__io_manager,
                                           verbose)

        self.__io_manager.protect_pin(MOTOR_PINS['tray_movement_control'])
        self.__io_manager.protect_pin(MOTOR_PINS['tray_movement_direction'])
        self.__io_manager.protect_pin(MOTOR_PINS['tray_movement_limit_switch'])

	self.current_loop_done = 1
	self.__calibrate_motors()
コード例 #10
0
ファイル: Frank.py プロジェクト: godiard/frank
class Frank:

    def __init__(self):
        print "Set mode BCM"
        GPIO.setmode(GPIO.BCM)
        self.motorX = Motor([6, 13, 19, 26])
        #self.motorY = Motor([])
        #self.sensor = Sensor(4) 
        self.setUpScreen()

    def setUpScreen(self):
        self.screen = curses.initscr()
        # turn off input echoing
        curses.noecho()
        # respond to keys immediately (don't wait for enter)
        curses.cbreak()
        # map arrow keys to special values
        self.screen.keypad(True)

    def isArrowKey(self, char):
        return char in [curses.KEY_RIGHT,curses.KEY_LEFT, curses.KEY_UP, curses.KEY_DOWN]

    def start(self):
        try:
            char = self.screen.getch()
            while True :
                
                while self.isArrowKey():
                    if char == curses.KEY_RIGHT:
                        print 'right'
                        self.motorX.moveTo(Motor.RIGHT)
                    elif char == curses.KEY_LEFT:
                        print 'left '
                        self.motorX.moveTo(Motor.LEFT)
                    elif char == curses.KEY_UP :
                        print 'up'
                    elif char == curses.KEY_DOWN :
                        print 'down'
                    char = self.screen.getch()
                if char == ord('q'):
                    self.cleanUp()
                    break
                else:
                    char = self.screen.getch()

        finally:
            # shut down cleanly
            curses.nocbreak(); self.screen.keypad(0); curses.echo()
            curses.endwin()

    def cleanUp(self):
        GPIO.cleanup()
コード例 #11
0
ファイル: robot.py プロジェクト: mparlaktuna/robot_face_v2
 def add_motor(self, line):
     """
     Add a new motor to the motor list
     :param line: motor parameters line as string from config file
     motor_number, motor_name, start_position, min_limit, max_limit
     :return:
     """
     line_list = line.split()
     new_motor = Motor(line_list[1])
     new_motor.motor_number = int(line_list[0])
     new_motor.goal_position = int(line_list[2])
     new_motor.min_limit = int(line_list[3])
     new_motor.max_limit = int(line_list[4])
     self.motor_list[line_list[1]] = new_motor
コード例 #12
0
ファイル: square.py プロジェクト: godiard/frank
    def __init__(self):
        print "Set mode BCM"
        config = json.load(open("config.json"))
        GPIO.setmode(GPIO.BCM)
        self.motorX = Motor(config["motor_x"]["pins"])
        self.motorX.name = "X"
        self.motorX.delay = config["motor_x"]["delay"]
        self.motorX.steps_by_mm = config["motor_x"]["steps_by_mm"]

        self.motorY = Motor(config["motor_y"]["pins"])
        self.motorY.name = "Y"
        self.motorY.delay = config["motor_y"]["delay"]
        self.motorY.steps_by_mm = config["motor_y"]["steps_by_mm"]

        self.laser = Laser(config["laser"]["pin"])
コード例 #13
0
ファイル: servidor.py プロジェクト: israelps/telescopioRpi
    def handle_read(self):
       
        data0 = self.recv(160);
        if data0:            
            data = ConstBitStream(bytes=data0, length=160)
            # print "All: %s" % data.bin
 
            msize = data.read('intle:16')
            mtype = data.read('intle:16')
            mtime = data.read('intle:64')
 
            # RA: 
            ant_pos = data.bitpos
            ra = data.read('hex:32')
            data.bitpos = ant_pos
            ra_uint = data.read('uintle:32')
 
            # DEC:
            ant_pos = data.bitpos
            dec = data.read('hex:32')
            data.bitpos = ant_pos
            dec_int = data.read('intle:32')
             
            logging.debug("Size: %d, Type: %d, Time: %d, RA: %d (%s), DEC: %d (%s)" % (msize, mtype, mtime, ra_uint, ra, dec_int, dec))
                       
            (ra, dec, time) = coords.int_2_rads(ra_uint, dec_int, mtime)

            x = transformar_coordenadas(dec, ra)
            az,alt = x.get_azi_alt()

            #instancia o motor de azimute nos pinos 12, 16, 20 e 21 do RPi
            motor_az = Motor([31,33,35,37])
            motor_az.rpm = 5
            motor_az.mode = 2
            motor_az.move_to(az-self.az_anterior)
            self.az_anterior = az

            #instancia o motor de azimute nos pinos 32, 36, 38 e 40 do RPi
            motor_alt = Motor([32,36,38,40])
            motor_alt.rpm = 5
            motor_alt.mode = 2
            motor_alt.move_to(alt-self.alt_anterior)
            self.alt_anterior = alt

            logging.debug("Azimute: %d, Altitude: %d" % (az,alt))
            
            # envia as cordenadas para o Stellarium
            self.act_pos(ra, dec)
コード例 #14
0
    def __init__(self, mode):
        self.mode = mode
        adafruitLoader = AdafruitLoader(self.mode)
        self.pwm = adafruitLoader.getPwmModule()
        self.bno = adafruitLoader.getBNO055Module()

        if not self.bno.begin():
            raise RuntimeError('Failed to initialize BNO055! Is the sensor connected?')

        # Set frequency to 60hz, good for servos.
        self.pwm.set_pwm_freq(60)

        self.motor1 = Motor(0, self.pwm, 276,457)
        self.motor2 = Motor(1, self.pwm, 276,457)
        self.motor3 = Motor(14, self.pwm, 276,457)
        self.motor4 = Motor(15, self.pwm, 276,457)
コード例 #15
0
ファイル: Frank.py プロジェクト: godiard/frank
 def __init__(self):
     print "Set mode BCM"
     GPIO.setmode(GPIO.BCM)
     self.motorX = Motor([6, 13, 19, 26])
     #self.motorY = Motor([])
     #self.sensor = Sensor(4) 
     self.setUpScreen()
コード例 #16
0
ファイル: robot.py プロジェクト: johandry/RosPi
	def __init__(self, debugMode = False):
		GPIO.setmode(GPIO.BOARD)
		GPIO.setwarnings(False)

		if debugMode:
			level = logging.DEBUG
		else:
			level = logging.CRITICAL
		logging.basicConfig(stream=sys.stderr, level=level)

		self.motor 			= Motor(GPIO, logging)
		self.panandtilt = PanAndTilt(logging)
		self.distance 	= Distance(GPIO, logging)

		self.MoveDirectionsOptions = {
			'fwd': self.motor.forward,
			'stp': self.motor.stop,
			'lft': self.motor.leftTurn,
			'rgt': self.motor.rightTurn,
			'bwd': self.motor.backward,
			'lfm': self.motor.left,
			'rgm': self.motor.right,
		}

		self.LookDirectionsOptions = {
			'fnt': self.panandtilt.front,
			'lft': self.panandtilt.left,
			'rgt': self.panandtilt.right,
			'up': self.panandtilt.up,
			'dwn': self.panandtilt.down,
		}		
コード例 #17
0
ファイル: scan.py プロジェクト: jkyl/xhorn
def go(min = 20, max = 50, n_steps = 5, zenith = 0, samp_rate = 4400, acc_len = 1, n_accs = 10,
       port = '/dev/ttyUSB0', ip = '128.135.52.192', home=True, docal=True, indef=True):
    '''
    Main function that creates motor, spec, and hdf5 objects, calculates the computer's offset
    from ntp time, and calls snap_and_move() in order to sweep the horn through a range of
    elevations and write accumulations and metadata to disk. Closes file and creates a new file
    after each return to zenith.

    Inputs:
        Step size in degrees, zenith angle in degrees, min and max angles in degrees, sample 
        rate in MHz, accumulation length in seconds, number of accumulations, /dev address of 
        motor controller, output filename, and ip address of roach board. 

    Outputs: 
        None,  writes to disk and std out.
    '''
    m = Motor(port = port)
    angles = np.sign(min)*scan_range(min, max, n_steps)

    # Flag calibration data if not scanning indefinitely
    if not indef:
        calext='_cal'
    else:
        calext='_scan'

    while True:
        while True:
            try:
                s = Spec(ip = ip, samp_rate = samp_rate, acc_len = acc_len) #re-initialize roach
                break
            except Exception:
                pass
            
        dt = 0 #ts.offset()
        fname = '/'.join(os.path.abspath(io.__file__).split('/')[:-2])\
                + '/output/' + ts.true_time(dt) + calext + '.h5'
        if home:
            #print('Homing')
            m.abst(0)
            m.home()
        if docal:
            move_and_snap(m, s, fname, zenith, CALIBRATOR_POSITION + zenith, acc_len, n_accs, dt)
        for destination in tqdm.tqdm(angles, unit = 'steps'):
            move_and_snap(m, s, fname, zenith, destination, acc_len, n_accs, dt)
        if not indef:
            break
コード例 #18
0
ファイル: zmqmotor.py プロジェクト: nukeop/Ci40Drone
 def __init__(self, address, pin, maxthrottle=200, minthrottle=500, name="motor"):
     self.name = name
     self.address = address
     self.server = ZMQServer(address)
     self.motor = Motor(pin, minthrottle, maxthrottle)
     self.motor.setmin()
     self.quit = False
     self.engage()
コード例 #19
0
ファイル: motor_conductor.py プロジェクト: clubcapra/capra
class MotorConductor:
    FRONT = "front"
    LEFT = "left"
    RIGHT = "right"

    def __init__(self):
        self.motor_front_left = Motor(MotorConductor.FRONT, MotorConductor.LEFT)
        self.motor_front_right = Motor(MotorConductor.FRONT, MotorConductor.RIGHT)

        for ndx, motor in enumerate(self.get_motors()):
            motor.gear_ratio = Config.get_gear_ratio()
            motor.wheel_diameter = Config.get_wheel_diameter()
            motor.position_ratio = Config.get_position_ratio()[ndx]
            motor.speed_ratio = Config.get_speed_ratio()[ndx]

    def get_motors(self):
        return [self.motor_front_left, self.motor_front_right]

    def set_velocity(self, linear_velocity, angular_velocity):
        width = Config.get_robot_width()

        # Calculate front motors velocity
        # Apply linear speed
        # On the left: substract arc velocity (rad * radius = arc)
        # On the right: add arc velocity (rad * radius = arc)
        # Considering radians increase counterclockwise and robot front is pointing toward x+
        # right wheel needs to travel more distance (and thus go faster) when the robot turns counterclockwise
        front_left_speed = linear_velocity - angular_velocity * width / 2.0
        front_right_speed = linear_velocity + angular_velocity * width / 2.0

        self.motor_front_left.set_velocity(front_left_speed)
        self.motor_front_right.set_velocity(front_right_speed)

    def measured_position(self):
        pass


    #Set the mode of the command
    #int8 MODE_STOPPED=-1
    #int8 MODE_VELOCITY=0 (DEFAULT)
    #int8 MODE_POSITION=1
    def set_motors_mode(self, mode):
        for motor in self.get_motors():
            motor.set_mode(mode)
コード例 #20
0
ファイル: robot.py プロジェクト: patengelbert/robotics-CO333
	def __init__(self):
		self.logging = False
		self.setDefaults()
		self.interface = brickpi.Interface()
		self.interface.initialize()
		self.events = Events()
		self.motorL = Motor(self.interface, self.events, 0)
		self.motorR = Motor(self.interface, self.events, 1)
		self.initMotorParams(self.motorL.motorParams)
		self.initMotorParams(self.motorR.motorParams)

		self.initConfig()
		self.touchSensorL = PushSensor('left',  self.interface, 0, self.events, brickpi.SensorType.SENSOR_TOUCH)
		self.touchSensorR = PushSensor('right', self.interface, 1, self.events, brickpi.SensorType.SENSOR_TOUCH)
		Bumper(self.events)
		self.ultraSonic = UltraSonicSensor(self.interface, 2, self.events, brickpi.SensorType.SENSOR_ULTRASONIC)
		self.setPID(self.pidk_p, self.pidk_i, self.pidk_d)

		self.events.add(EventType.SENSOR_TOUCH, self.sensorAction)
コード例 #21
0
ファイル: clock.py プロジェクト: gregmajor/slotobahn
    def __init__(self, configuration):
        """Create a new instance of the clock class.
        """
        self._configuration = configuration
        self._logger = logging.getLogger(__name__)

        self._motor = Motor(self._configuration)
        self._blinker = Blinker(self._configuration)
        self._display = Display(self._configuration)
        self._database = Database(self._configuration)
        self._consumer = Consumer(self._configuration, self.on_message)
コード例 #22
0
ファイル: car_server.py プロジェクト: adityakamath/RobotBrain
    def __init__(self, motor_pins=(24, 25), servo_pin=0, port=2012):
        self.port = port

        # The motor and servo for driving
        self.motor = Motor(*motor_pins)
        self.servo = Servo(servo_pin)
        
        # The most recent coordinates from the accelerameter
        self.coords = (0, 0, 0)

        # Whether or not to continue running the server
        self._run = True

        self.start()
コード例 #23
0
ファイル: roboarm.py プロジェクト: tinkerfuroc/tk2_control
 def __init__(self):
     self.joints = [
         PWMJoint(
             5, BASE_MIN_ANGLE, BASE_MIN_PWM, BASE_MAX_ANGLE, 
             BASE_MAX_PWM, BASE_COF1, BASE_COF0),
         PWMJoint(
             6, SHOULDER_MIN_ANGLE, SHOULDER_MIN_PWM, SHOULDER_MAX_ANGLE, 
             SHOULDER_MAX_PWM, SHOULDER_COF1, SHOULDER_COF0),
         PWMJoint(
             7, ELBOW_MIN_ANGLE, ELBOW_MIN_PWM, ELBOW_MAX_ANGLE, 
             ELBOW_MAX_PWM, ELBOW_COF1, ELBOW_COF0),
         DyMiJoint(
             1, WRIST_MIN_ANGLE, WRIST_MIN_PWM, WRIST_MAX_ANGLE, 
             WRIST_MAX_PWM, WRIST_COF1, WRIST_COF0)]
     self.motor = Motor(4)
コード例 #24
0
ファイル: stoerkoerpermessung.py プロジェクト: chrisieh/SKM
 def init_motor(self):
     # Abkürzung
     settings = self.config["MOTOR"]
     
     # Serial-Config
     port = settings["Port"]
     baudrate = int(settings["Baudrate"])
     serial_timeout = float(settings["SerialTimeout"])
     
     self.motor_serial = Serial(port, baudrate, timeout=serial_timeout)
     
     # Motor-Config
     motor_name = settings["Name"]
     motor_config = settings["Config"]
     
     self.motor = Motor(self.motor_serial, motor_name)
     self.motor.load_motor_config(motor_config)
     self.motor.scaling = float(settings["Scaling"])
コード例 #25
0
	def __init__(self, name, ammo):
		self.name = name
		self.current_degree = 0
		self.ammunition_count = ammo
		self.degrees = range(0, 101, 10)
		self.serial_connection =  serial.Serial("/dev/ttyACM0", 9600)


	
		self.seven_seg_one = Display(SDI=11, RCLK=12, SRCLK=13)
		self.seven_seg_two = Display(SDI=33, RCLK=32, SRCLK=35)
		
		self.x_axis = Servo(0, "X Axis", self)
		self.y_axis = Servo(1, "Y Axis", self)		
		self.release= Servo(2, "Release", self)
		
		self.sonic = Sonic(16, 18)
	
		self.servos = [self.x_axis, self.y_axis]
 
		self.motor = Motor(37, 38, 40)
コード例 #26
0
ファイル: motorpot.py プロジェクト: sbelectronics/pi-stereo
    def __init__(self, bus, adc_addr=0x48, motor_pin1=L293_1, motor_pin2=L293_2, motor_enable = STEREO_L293_ENABLE, dirmult=1, verbose=False):
        Thread.__init__(self)

        self.motor = Motor(pin1=motor_pin1, pin2=motor_pin2, enable = motor_enable)
        self.motor.set_speed(0)

        self.adc = ADS1015(bus, adc_addr)

        self.adc.write_config(MUX_AIN0 | PGA_4V | MODE_CONT | DATA_1600 | COMP_MODE_TRAD | COMP_POL_LOW | COMP_NON_LAT | COMP_QUE_DISABLE)

        self.dirmult = dirmult

        self.setPoint = None
        self.newSetPoint = False
        self.moving = False

        self.daemon = True
        self.verbose = verbose

        self.lastStopTime = time.time()

        self.start()
コード例 #27
0
ファイル: zmqmotor.py プロジェクト: nukeop/Ci40Drone
class ZMQMotor:
    def __init__(self, address, pin, maxthrottle=200, minthrottle=500, name="motor"):
        self.name = name
        self.address = address
        self.server = ZMQServer(address)
        self.motor = Motor(pin, minthrottle, maxthrottle)
        self.motor.setmin()
        self.quit = False
        self.engage()
        
    def engage(self):
        self.mainloop()
    
    def mainloop(self):
        while not self.quit:
            try:
                message = self.server.receive()
                self.process_msg(message)
                self.reply(message)
                time.sleep(1)
            except ZMQError as e:
                pass
        self.destroy()
            
    def process_msg(self, message):
        match = THROTTLE_REGEX.match(message)
        print "Received message: " + message
        if message == "laputanmachine":
            self.quit=True
        elif match:
            throttle = int(match.group('throttle'))
            self.motor.set_throttle(throttle)
        else:
            print message
            
    def reply(self, message):
        self.server.reply(message)
    
    def destroy(self):
        self.motor.stop()
        self.server.destroy()
コード例 #28
0
ファイル: adjust.py プロジェクト: Time1ess/VES
# Author: David
# Email: [email protected]
# Created: 2016-04-08 05:38
# Last modified: 2016-04-08 05:43
# Filename: adjust.py
# Description:

import sys
import time

from motor import Motor


try:
    index = int(sys.argv[1])
    rot = True if sys.argv[2] == '1' else False
    num = int(sys.argv[3])

    m = Motor()
    
    for i in xrange(num):
        m.adjust(index, rot)
        time.sleep(0.08)
    
    m.exit()
except Exception, e:
    print 'ERROR:', e
except KeyboardInterrupt, e:
    print 'EXIT.'

コード例 #29
0
def addMotor(sid, data):
    motors.append(Motor(pi, data['name'], data['pin'], config))
    sendMotors()
コード例 #30
0
import sys

sys.path.insert(1, './Library/scripts')

from RTIMUScripts import get_heading
from motor import Motor
from infraredSensor import InfraredSensor
from ultrasonic import Ultrasonic

motor = Motor()
right_ir_sensor = InfraredSensor(16)
left_ir_sensor = InfraredSensor(12)
right_front_ir_sensor = InfraredSensor(20)
left_front_ir_sensor = InfraredSensor(21)
ultrasonicSensor = Ultrasonic()

components = [
    motor, right_ir_sensor, left_ir_sensor, right_front_ir_sensor,
    left_front_ir_sensor, ultrasonicSensor
]

car_stopped = False


def normalizeDegrees(degrees):
    if 0 <= degrees < 360:
        return degrees
    elif degrees < 0:
        return degrees + 360
    elif degrees >= 360:
        return degrees - 360
コード例 #31
0
#!/usr/bin/env python

import rospy
import RPi.GPIO as GPIO
from std_msgs.msg import Float32
from motor import Motor

if __name__ == "__main__":
    power_motor = Motor("power_motor", 22, 17, 27)
コード例 #32
0
class MotorMatrix(object):
    # NOTE: this could a config file or whatever, coded in this class for now
    # Todo: get pinouts decided
    FRONT_LEFT_PIN = 12
    FRONT_RIGHT_PIN = 18
    BACK_LEFT_PIN = 13
    BACK_RIGHT_PIN = 19

    TRANSLATION_GAIN = 1 / 10

    FL = 0
    FR = 1
    BL = 2
    BR = 3

    ALL = [FL, FR, BL, BR]
    FRONT_ARRAY = [FL, FR]
    BACK_ARRAY = [BL, BR]
    LEFT_ARRAY = [FL, BL]
    RIGHT_ARRAY = [FR, BR]

    CLOCKWISE_ARRAY = [FR, BL]  # Clockwise rotating motors
    ANTICLOCKWISE_ARRAY = [FL, BR]

    def __init__(self, motor_definition: MotorDefinition = None):
        #NOTE: this is very static code, may be better to loop and calculuate these, or subclass to isolate motor

        if not motor_definition:
            self.front_left = Motor(MotorMatrix.FRONT_LEFT_PIN)
            self.front_right = Motor(MotorMatrix.FRONT_RIGHT_PIN)
            self.back_left = Motor(MotorMatrix.BACK_LEFT_PIN)
            self.back_right = Motor(MotorMatrix.BACK_RIGHT_PIN)
        else:
            self.front_left = motor_definition.front_left
            self.front_right = motor_definition.front_right
            self.back_left = motor_definition.back_left
            self.back_right = motor_definition.back_right

        # Must be aligned with MotorMatrix.ALL
        self.all = [
            self.front_left,
            self.front_right,
            self.back_left,
            self.back_right,
        ]

    def start_your_engines(self):

        for motor in self.all:
            motor.start()

    def set_platform_controls(self, rise_normalized: float,
                              forward_normalized: float,
                              roll_right_normalized: float,
                              yaw_clockwise_normalized: float):

        buffer_speeds = [0.0, 0.0, 0.0, 0.0]

        for motor_index in MotorMatrix.ALL:
            buffer_speeds[motor_index] = rise_normalized

        for motor_index in MotorMatrix.FRONT_ARRAY:
            buffer_speeds[
                motor_index] -= MotorMatrix.TRANSLATION_GAIN * forward_normalized
        for motor_index in MotorMatrix.BACK_ARRAY:
            buffer_speeds[
                motor_index] += MotorMatrix.TRANSLATION_GAIN * forward_normalized

        for motor_index in MotorMatrix.RIGHT_ARRAY:
            buffer_speeds[
                motor_index] -= MotorMatrix.TRANSLATION_GAIN * roll_right_normalized
        for motor_index in MotorMatrix.LEFT_ARRAY:
            buffer_speeds[
                motor_index] += MotorMatrix.TRANSLATION_GAIN * roll_right_normalized

        for motor_index in MotorMatrix.CLOCKWISE_ARRAY:
            buffer_speeds[
                motor_index] -= MotorMatrix.TRANSLATION_GAIN * yaw_clockwise_normalized
        for motor_index in MotorMatrix.ANTICLOCKWISE_ARRAY:
            buffer_speeds[
                motor_index] += MotorMatrix.TRANSLATION_GAIN * yaw_clockwise_normalized

        logger.critical("Buffer speeds FL {} FR {} BR {} BL {}".format(
            buffer_speeds[0],
            buffer_speeds[1],
            buffer_speeds[2],
            buffer_speeds[3],
        ))

        for motor_index in MotorMatrix.ALL:
            motor = self.all[motor_index]
            desired_speed = buffer_speeds[motor_index]
            desired_speed = min(1.0, max(-1.0, desired_speed))

            translated_postive = desired_speed + 1
            scaled_to_percent = (translated_postive / 2) * 100
            logger.critical("Motor speed index {} value of {}".format(
                motor_index, scaled_to_percent))
            motor.update_speed(scaled_to_percent)

    def direct_test(
        self,
        fl_normalized: float,
        fr_normalized: float,
        br_normalized: float,
        bl_normalized: float,
    ):

        self.front_left.update_speed(fl_normalized * 100)
        self.front_right.update_speed(fr_normalized * 100)
        self.back_right.update_speed(br_normalized * 100)
        self.back_left.update_speed(bl_normalized * 100)

    def cleanup(self):

        for motor in self.all:
            motor.stop()

        Motor.cleanup_all_motors()
コード例 #33
0
from m5stack import lcd
from machine import I2C, Pin
from motor import Motor
from time import sleep_ms

lcd.clear()
lcd.setCursor(0, 0)
lcd.setColor(lcd.WHITE)
lcd.print("Hello World!")

i2c = I2C(scl=Pin(22), sda=Pin(21))

m = Motor(i2c=i2c, address=0x0F)

while True:
    m.setMotorSpeed(100, 100)
    m.setDirection(0b0000)
    sleep_ms(1000)
    m.setDirection(0b0101)
    sleep_ms(1000)
    m.setDirection(0b1010)
    sleep_ms(1000)
    m.setDirection(0b1111)
    sleep_ms(1000)
コード例 #34
0
ファイル: main.py プロジェクト: krismsd/robot-arm-control
import curses
import RPi.GPIO as GPIO
import smbus

from motor import Motor
from stepper import StepperMotor
from i2cencoder import I2CEncoder
from ArmJoint import ArmJoint

try:
    GPIO.setmode(GPIO.BCM)

    bus = smbus.SMBus(1)

    motor1 = Motor(6, 13)
    motor2 = Motor(19, 26)

    stepper1 = StepperMotor(21, 20)

    encoderConnection = I2CEncoder(bus, 0x08)

    jointAngleRatio = 54000 * 2 * -1  # 54000 measured position manually over 1/2th of full rotation
    armJoint1 = ArmJoint(motor1, encoderConnection, 0, 180, jointAngleRatio)
    armJoint2 = ArmJoint(motor2, encoderConnection, 1, 180, jointAngleRatio)

    with stepper1, encoderConnection, armJoint1, armJoint2:

        def updateScreen(screen):
            screen.clear()
            screen.addstr(1, 0, str(stepper1))
コード例 #35
0
def main(io):
    #GPIO 14,15,18,23; PIN  8,10,12,16
    #GPIO 24,25, 8, 7; PIN 18,22,24,26
    #GPIO 1 ,12,16,20; PIN 28,32,26,38
    #GPIO 6 ,13,19,26; PIN 31,33,35,37 (Left turn)
    #GPIO 10, 9,11, 5; PIN 19,21,23,29 (
    #GPIO 17,27, 3, 4; PIN 11,13,5 ,7
    #22 doesn't work

    #rpi2

    global rack_l, motor_l, rack_b, motor_b
    global is_f_upright, is_b_upright, is_l_upright, is_r_upright
    rack_l = RackMotor("rl", 6, 13, 19, 26, io)
    motor_l = Motor("ml", 1, 21, 16, 20, io)
    rack_b = RackMotor("rb", 10, 9, 11, 5, io)
    motor_b = Motor("mb", 24, 25, 8, 7, io)
    comms = serial_comm()
    '''
    rack_l.move_deg(L_RACK_OUT)
    motor_l.move_deg(L_FWD)
    motor_l.move_deg(L_FWD)
    motor_l.move_deg(L_REV)
    motor_l.move_deg(L_FWD)
    motor_l.move_deg(L_REV)
    motor_l.move_deg(L_REV)
    rack_l.move_deg(L_RACK_IN)
    
    return
    '''

    #solution = "L' F R F' B' F B".split(" ")
    #solution = "B R' R L B' F B".split(" ")
    #solution = "R' F' B R U' F' U' U' B' B' L D F U' U' F' F' D' D' F' L' L' F' F' B R' R' F".split()
    solution = "F' F' B U' F' L' D F' B B L B B D F F U' D D R R U U R R F F U' R R B'".split(
    )
    solution = "F L B B L U B' D' L' B U' F F B B D R R D B B U' D D B B".split(
    )
    #solution = "U U' D D' L L' R R' B B' F F'".split()
    for step in solution:
        print(step)
        if step == "L":
            if not is_f_upright:
                comms.write_serial(SRL_F_RACK_OUT)
                comms.read_serial()
                comms.write_serial(SRL_F_FWD)
                comms.read_serial()
                comms.write_serial(SRL_F_RACK_IN)
                comms.read_serial()
                is_f_upright = True
            if not is_b_upright:
                rack_b.move_set_power(B_RACK_OUT)
                motor_b.move_deg(B_FWD)
                rack_b.move_set_power(B_RACK_IN)
                is_b_upright = True
            motor_l.move_deg(L_FWD)
            is_l_upright = not is_l_upright
        elif step == "L'":
            if not is_f_upright:
                comms.write_serial(SRL_F_RACK_OUT)
                comms.read_serial()
                comms.write_serial(SRL_F_FWD)
                comms.read_serial()
                comms.write_serial(SRL_F_RACK_IN)
                comms.read_serial()
                is_f_upright = True
            if not is_b_upright:
                rack_b.move_set_power(B_RACK_OUT)
                motor_b.move_deg(B_FWD)
                rack_b.move_set_power(B_RACK_IN)
                is_b_upright = True
            motor_l.move_deg(L_REV)
            is_l_upright = not is_l_upright
        elif step == "R":
            if not is_f_upright:
                comms.write_serial(SRL_F_RACK_OUT)
                comms.read_serial()
                comms.write_serial(SRL_F_FWD)
                comms.read_serial()
                comms.write_serial(SRL_F_RACK_IN)
                comms.read_serial()
                is_f_upright = True
            if not is_b_upright:
                rack_b.move_set_power(B_RACK_OUT)
                motor_b.move_deg(B_FWD)
                rack_b.move_set_power(B_RACK_IN)
                is_b_upright = True
            comms.write_serial(SRL_R_FWD)
            comms.read_serial()
            is_r_upright = not is_r_upright
        elif step == "R'":
            if not is_f_upright:
                comms.write_serial(SRL_F_RACK_OUT)
                comms.read_serial()
                comms.write_serial(SRL_F_FWD)
                comms.read_serial()
                comms.write_serial(SRL_F_RACK_IN)
                comms.read_serial()
                is_f_upright = True
            if not is_b_upright:
                rack_b.move_set_power(B_RACK_OUT)
                motor_b.move_deg(B_FWD)
                rack_b.move_set_power(B_RACK_IN)
                is_b_upright = True
            comms.write_serial(SRL_R_REV)
            comms.read_serial()
            is_r_upright = not is_r_upright
        elif step == "F":
            if not is_l_upright:
                rack_l.move_set_power(L_RACK_OUT)
                motor_l.move_deg(L_FWD)
                rack_l.move_set_power(L_RACK_IN)
                is_l_upright = True
            if not is_r_upright:
                comms.write_serial(SRL_R_RACK_OUT)
                comms.read_serial()
                comms.write_serial(SRL_R_FWD)
                comms.read_serial()
                comms.write_serial(SRL_R_RACK_IN)
                comms.read_serial()
                is_r_upright = True
            comms.write_serial(SRL_F_FWD)
            comms.read_serial()
            is_f_upright = not is_f_upright
        elif step == "F'":
            if not is_l_upright:
                rack_l.move_set_power(L_RACK_OUT)
                motor_l.move_deg(L_FWD)
                rack_l.move_set_power(L_RACK_IN)
                is_l_upright = True
            if not is_r_upright:
                comms.write_serial(SRL_R_RACK_OUT)
                comms.read_serial()
                comms.write_serial(SRL_R_FWD)
                comms.read_serial()
                comms.write_serial(SRL_R_RACK_IN)
                comms.read_serial()
                is_r_upright = True
            comms.write_serial(SRL_F_REV)
            comms.read_serial()
            is_f_upright = not is_f_upright
        elif step == "B":
            if not is_l_upright:
                rack_l.move_set_power(L_RACK_OUT)
                motor_l.move_deg(L_FWD)
                rack_l.move_set_power(L_RACK_IN)
                is_l_upright = True
            if not is_r_upright:
                comms.write_serial(SRL_R_RACK_OUT)
                comms.read_serial()
                comms.write_serial(SRL_R_FWD)
                comms.read_serial()
                comms.write_serial(SRL_R_RACK_IN)
                comms.read_serial()
                is_r_upright = True
            motor_b.move_deg(B_FWD)
            is_b_upright = not is_b_upright
        elif step == "B'":
            if not is_l_upright:
                rack_l.move_set_power(L_RACK_OUT)
                motor_l.move_deg(L_FWD)
                rack_l.move_set_power(L_RACK_IN)
                is_l_upright = True
            if not is_r_upright:
                comms.write_serial(SRL_R_RACK_OUT)
                comms.read_serial()
                comms.write_serial(SRL_R_FWD)
                comms.read_serial()
                comms.write_serial(SRL_R_RACK_IN)
                comms.read_serial()
                is_r_upright = True
            motor_b.move_deg(B_REV)
            is_b_upright = not is_b_upright
        else:  #U, U', D, D'
            if (step == 'U' or step == "U'"):
                is_up = True
            elif (step == 'D' or step == "D'"):
                is_up = False
            else:
                print('Flying kite')
                raise Exception('flying kite')

            if is_l_upright:
                if not is_f_upright:
                    comms.write_serial(SRL_F_RACK_OUT)
                    comms.read_serial()
                    comms.write_serial(SRL_F_FWD)
                    comms.read_serial()
                    comms.write_serial(SRL_F_RACK_IN)
                    comms.read_serial()
                    is_f_upright = True
                if not is_b_upright:
                    rack_b.move_set_power(B_RACK_OUT)
                    motor_b.move_deg(B_FWD)
                    rack_b.move_set_power(B_RACK_IN)
                    is_b_upright = True
                rack_l.move_set_power(L_RACK_OUT)
                motor_l.move_deg(L_FWD)
                is_l_upright = False
                rack_l.move_set_power(L_RACK_IN)
            if is_r_upright:
                if not is_f_upright:
                    comms.write_serial(SRL_F_RACK_OUT)
                    comms.read_serial()
                    comms.write_serial(SRL_F_FWD)
                    comms.read_serial()
                    comms.write_serial(SRL_F_RACK_IN)
                    comms.read_serial()
                    is_f_upright = True
                if not is_b_upright:
                    rack_b.move_set_power(B_RACK_OUT)
                    motor_b.move_deg(B_FWD)
                    rack_b.move_set_power(B_RACK_IN)
                    is_b_upright = True
                comms.write_serial(SRL_R_RACK_OUT)
                comms.read_serial()
                comms.write_serial(SRL_R_FWD)
                comms.read_serial()
                is_r_upright = False
                comms.write_serial(SRL_R_RACK_IN)
                comms.read_serial()

            # retract front and back
            comms.write_serial(SRL_F_RACK_OUT)
            rack_b.move_set_power(B_RACK_OUT)
            comms.read_serial()

            # rotate cube's U to F claw
            if is_up:
                comms.write_serial(SRL_R_REV)
                motor_l.move_deg(L_FWD)
                comms.read_serial()
            else:
                comms.write_serial(SRL_R_FWD)
                motor_l.move_deg(L_REV)
                comms.read_serial()

            # rotate using F claw
            comms.write_serial(SRL_F_RACK_IN)
            rack_b.move_set_power(B_RACK_IN)
            comms.read_serial()
            if step == "U" or step == "D":
                comms.write_serial(SRL_F_FWD)
                comms.read_serial()
            elif step == "U'" or step == "D'":
                comms.write_serial(SRL_F_REV)
                comms.read_serial()
            is_f_upright = not is_f_upright

            comms.write_serial(SRL_F_RACK_OUT)
            rack_b.move_set_power(B_RACK_OUT)
            comms.read_serial()
            if not is_f_upright:  # make it upright to prevent collision when rotating back
                comms.write_serial(SRL_F_FWD)
                comms.read_serial()
                is_f_upright = True

            if not is_b_upright:  # make it upright to prevent collision when rotating back
                motor_b.move_deg(B_FWD)
                is_b_upright = True

            # rotate back
            if is_up:
                comms.write_serial(SRL_R_FWD)
                motor_l.move_deg(L_REV)
                comms.read_serial()
            else:
                comms.write_serial(SRL_R_REV)
                motor_l.move_deg(L_FWD)
                comms.read_serial()

            # reengage motors
            comms.write_serial(SRL_F_RACK_IN)
            rack_b.move_set_power(B_RACK_IN)
            comms.read_serial()

            is_l_upright = False
            is_r_upright = False
    comms.write_serial(SRL_STOP)
    return
    '''
    if result != "":
        to_send = f"messaged about {result} received"
        comms.write_serial(to_send)
        print("sending confirmation")
    '''
    """
コード例 #36
0
import pigpio
import time
from pigpioFIFO import pigpioFIFO
from motor import Motor, Integrator, Command
from profiler import print_prof_data

motor = Motor(17)
integrator = Integrator(motor)

fifo = pigpioFIFO(2000, 0.1)
fifo.callBack = integrator.integrate

#Command(pos, vel)
integrator.g00(1600)
integrator.g00(16000)

time.sleep(1)

while fifo.pi.wave_tx_at() != 9999:
    #print_prof_data()
    time.sleep(1)

time.sleep(5)

fifo.pi.wave_tx_stop()
fifo.pi.stop()
コード例 #37
0
def main():
    # Local functions in main function
    global nextLocationIndex
    global locations
    global robotMotor
    botCentered = False
    locationAccuracy = 0.000005

    print("Setting devices...")
    compass = Compass("/dev/ttyACM0")
    robotMotor = Motor("/dev/ttyACM1")
    try:
        robotGps = RobotGps()
    except OSError:
        print("GPSD not started!")
        exit(-1)
    robotMotor.moveMotor('stop')
    print("All device set!")
    sleep(1)
    robotMotor.moveMotor('stop')
    # Set the bot to point at next location
    while not (170 < compass.getCompassAngle()
               and compass.getCompassAngle() < 180):
        print(compass.getCompassAngle())

    # os.system("clear")
    botCentered = False

    # Rotate Rover Once
    # angle = compass.getCompassAngle()-100
    # while compass.getCompassAngle()>angle or compass.getCompassAngle()<angle:
    # 	print("Calibration!")
    # 	roverMotor.moveMotor('left')

    robotMotor.moveMotor("stop")
    os.system("clear")
    print("Rover centered!")
    robotMotor.moveMotor('stop')
    sleep(2)
    # roverMotor.resetAllMotors()
    print("Locations:", locations)
    # input('Press anything to continue!')

    # =========
    # Main Loop
    # =========
    while nextLocationIndex < len(locations):
        # roverMotor.resetAllMotors()
        try:
            currentLocation = robotMotor.getGpsData()
            # print(roverGps)
        except ValueError:
            print("GPS not set")
            continue
        if currentLocation[0] == None:
            print("GPS no location")
            continue

        if abs(currentLocation[0] -
               locations[nextLocationIndex][0]) < locationAccuracy and abs(
                   currentLocation[1] -
                   locations[nextLocationIndex][1]) < locationAccuracy:
            robotMotor.moveMotor("stop")
            nextLocationIndex += 1
            if nextLocationIndex >= len(locations):
                break
            print(locations)
            print("Location Reached!", currentLocation)
            print("Press any key to continue")
            input()
            # sleep(2)
            # Center bot to north
            botCentered = False
            while not botCentered:
                # os.system("clear")

                if centerBot(compass, 0, robotGps, robotMotor, 20):
                    print()
                    botCentered = True
            botCentered = False
            print("Continue to location", locations[nextLocationIndex])
            sleep(1)
            # input()
            continue

        slope = getSlope(currentLocation)
        # Move the rover to this slope
        while not botCentered:
            # os.system("clear")
            slope = getSlope(currentLocation)

            if centerBot(compass, slope, robotGps, robotMotor, 10):
                botCentered = True
            # sleep(0.5)
        if not centerBot(compass, slope, robotGps, robotMotor, 10):
            print()
            botCentered = False

        # # Move bot forward
        # # os.system("clear")
        try:
            print("Move Forward",
                  robotGps.getGpsData(), locations[nextLocationIndex], slope,
                  compass.getCompassAngle())
            # roverMotor.moveMotor('forward')
            robotMotor.moveMotor('forward')
        except ValueError:
            print("Compass Value error")
    robotMotor.moveMotor('stop')
    print("Finished!")
コード例 #38
0
from motor import Motor
from microbit import *
import music

m = Motor(pin0, pin12, pin13)


def count_ntimes(n):
    for _ in range(n):
        display.show(_)
        sleep(1000)


def play_beep():
    music.play(music.BA_DING, pin=pin8)


display.scroll("waiting...", wait=False)
while True:
    if button_a.is_pressed():
        m.start_forward(speed=500)
        count_ntimes(5)
        m.stop()
        play_beep()
        display.clear()
    if button_b.is_pressed():
        m.start_backward(speed=500)
        count_ntimes(5)
        m.stop()
        play_beep()
        display.clear()
コード例 #39
0
ファイル: wheelie.py プロジェクト: Mayakshanesht/robots
    def __init__(self,
                 name,
                 pinRightFwd,
                 pinRightRev,
                 pinLeftFwd,
                 pinLeftRev,
                 wheel_diameter=.065,
                 wheel_base=0.15,
                 left_max_rpm=200.0,
                 right_max_rpm=200.0,
                 frequency=20):
        """
        Parameters
        ----------
        name: str
            The node name that will be used for this robot; defaults
            to "wheelie"
        pinRightFwd : int
            The RaspPi GPIO pin that goes high to create forward motion
            on the right wheel
        pinRightRev : int
            The RaspPi GPIO pin that goes high to create reverse motion
            on the right wheel
        pinLeftFwd : int
            The RaspPi GPIO pin that goes high to create forward motion
            on the right wheel
        pinLeftRev : int
            The RaspPi GPIO pin that goes high to create reverse motion
            on the right wheel
        wheel_diameter : float
            The diameter of the wheels in meters
        wheel_base : float
            The distance between the center of the wheels in meters
        left_max_rpm : float
            The number of revolutions per minute made by the left motor
            when running at 100% power
        right_max_rpm : float
            The number of revolutions per minute made by the left motor
            when running at 100% power
        frequency : int
            The frequency in Hz used to control the PWM motors
        """

        super().__init__(name)

        self._frequency = frequency
        self._left_max_rpm = left_max_rpm
        self._right_max_rpm = right_max_rpm
        self._wheel_diameter = wheel_diameter
        self._wheel_base = wheel_base
        self._rightWheel = Motor(pinRightFwd, pinRightRev, frequency)
        self._leftWheel = Motor(pinLeftFwd, pinLeftRev, frequency)

        self.speed = 0.0
        self.spin = 0.0
        self.close = 0.30  # start slowing down when this close
        self.tooclose = 0.10  # no forward motion when this close
        self.distance = 100.0

        self._command_subscription = self.create_subscription(
            String, 'command', self._command_callback, 10)

        self._cmd_vel_subscription = self.create_subscription(
            Twist, 'cmd_vel', self._cmd_vel_callback, 2)

        self._joy_subscription = self.create_subscription(
            Joy, 'joy', self._joy_callback, 5)

        self._range_subscription = self.create_subscription(
            Range, 'range', self._range_callback, 5)
コード例 #40
0
ファイル: d2_with_od.py プロジェクト: tmdorny/School-Work
from __future__ import print_function
#
# Sample demo for obstacle detector (od)
#

import sys
import time
import RPi.GPIO as GPIO

sys.path.append('..')

from ObstacleDetector import ObstacleDetector
from car_config import *
from motor import Motor
import time
left = Motor(LFP, LBP, LEP)
right = Motor(RFP, RBP, REP)


def stop(distance):
    print("Distance is {0}".format(distance))
    left.off()
    right.off()
    time.sleep(1)
    left.forward(60)
    right.backward(60)
    time.sleep(1)
    left.forward(50)
    right.forward(50)
    return
コード例 #41
0
ファイル: wheelie.py プロジェクト: Mayakshanesht/robots
class Wheelie(Node):
    '''Wheelie node suitable for a RPi robot with two PWM driven motors

    Creates listener on /command to accept string-style commands.
    Creates listener on /cmd_vel to accept twist messages.
    Creates listener on /joy to accept xbox joystick input.

    Attributes
    ----------
    speed : float
        Speed along the X axis in meters per second; positive is
        forward and negative is backward
    spin : float
        Rotation about the pivot point in radians per second; positive
        is clockwise when viewed from above (right spin)

    Methods
    -------
    stop()
        Stop all movement of the wheelie

    '''
    def __init__(self,
                 name,
                 pinRightFwd,
                 pinRightRev,
                 pinLeftFwd,
                 pinLeftRev,
                 wheel_diameter=.065,
                 wheel_base=0.15,
                 left_max_rpm=200.0,
                 right_max_rpm=200.0,
                 frequency=20):
        """
        Parameters
        ----------
        name: str
            The node name that will be used for this robot; defaults
            to "wheelie"
        pinRightFwd : int
            The RaspPi GPIO pin that goes high to create forward motion
            on the right wheel
        pinRightRev : int
            The RaspPi GPIO pin that goes high to create reverse motion
            on the right wheel
        pinLeftFwd : int
            The RaspPi GPIO pin that goes high to create forward motion
            on the right wheel
        pinLeftRev : int
            The RaspPi GPIO pin that goes high to create reverse motion
            on the right wheel
        wheel_diameter : float
            The diameter of the wheels in meters
        wheel_base : float
            The distance between the center of the wheels in meters
        left_max_rpm : float
            The number of revolutions per minute made by the left motor
            when running at 100% power
        right_max_rpm : float
            The number of revolutions per minute made by the left motor
            when running at 100% power
        frequency : int
            The frequency in Hz used to control the PWM motors
        """

        super().__init__(name)

        self._frequency = frequency
        self._left_max_rpm = left_max_rpm
        self._right_max_rpm = right_max_rpm
        self._wheel_diameter = wheel_diameter
        self._wheel_base = wheel_base
        self._rightWheel = Motor(pinRightFwd, pinRightRev, frequency)
        self._leftWheel = Motor(pinLeftFwd, pinLeftRev, frequency)

        self.speed = 0.0
        self.spin = 0.0
        self.close = 0.30  # start slowing down when this close
        self.tooclose = 0.10  # no forward motion when this close
        self.distance = 100.0

        self._command_subscription = self.create_subscription(
            String, 'command', self._command_callback, 10)

        self._cmd_vel_subscription = self.create_subscription(
            Twist, 'cmd_vel', self._cmd_vel_callback, 2)

        self._joy_subscription = self.create_subscription(
            Joy, 'joy', self._joy_callback, 5)

        self._range_subscription = self.create_subscription(
            Range, 'range', self._range_callback, 5)

    def stop(self):
        self._leftWheel.stop()
        self._rightWheel.stop()

    def max_speed(self):
        '''Speed in meters per second at maximum RPM'''
        rpm = (self._left_max_rpm + self._right_max_rpm) / 2.0
        mps = rpm * math.pi * self._wheel_diameter / 60.0
        return mps

    def max_twist(self):
        '''Rotation in radians per second at maximum RPM'''
        return self.max_speed() / self._wheel_diameter

    def _forward(self, speed=100):
        self._rightWheel.forward(speed)
        self._leftWheel.forward(speed)

    def _reverse(self, speed=100):
        self._rightWheel.reverse(speed)
        self._leftWheel.reverse(speed)

    def _left(self, speed=100):
        self._rightWheel.reverse(speed)
        self._leftWheel.forward(speed)

    def _right(self, speed=100):
        self._rightWheel.forward(speed)
        self._leftWheel.reverse(speed)

    def _command_callback(self, msg):
        command = msg.data
        if command == 'forward':
            self._forward()
        elif command == 'reverse':
            self._reverse()
        elif command == 'left':
            self._left()
        elif command == 'right':
            self._right()
        elif command == 'stop':
            self.stop()
        else:
            print('Unknown command, stopping instead')
            self.stop()

    def _joy_callback(self, msg):
        '''Translate XBox buttons into speed and spin

        Just use the left joystick (for now):
        LSB left/right  axes[0]     +1 (left) to -1 (right)
        LSB up/down     axes[1]     +1 (up) to -1 (back)
        LB              buttons[5]  1 pressed, 0 otherwise
        '''

        if abs(msg.axes[0]) > 0.10:
            self.spin = msg.axes[0]
        else:
            self.spin = 0.0

        if abs(msg.axes[1]) > 0.10:
            self.speed = msg.axes[1]
        else:
            self.speed = 0.0

        if msg.buttons[5] == 1:
            self.speed = 0
            self.spin = 0

        self._set_motor_speeds()

    def _cmd_vel_callback(self, msg):
        self.speed = msg.linear.x
        self.spin = msg.angular.z
        self._set_motor_speeds()

    def _range_callback(self, msg):
        self.distance = msg.range
        self._set_motor_speeds()

    def _set_motor_speeds(self):
        # TODO: inject a stop() if no speeds seen for a while
        #
        # Scary math ahead.
        #
        # First figure out the speed of each wheel based on spin: each wheel
        # covers self._wheel_base meters in one radian, so the target speed
        # for each wheel in meters per sec is spin (radians/sec) times
        # wheel_base divided by wheel_diameter
        #
        right_twist_mps = self.spin * self._wheel_base / self._wheel_diameter
        left_twist_mps = -1.0 * self.spin * \
            self._wheel_base / self._wheel_diameter
        #
        # Now add in forward motion.
        #
        left_mps = self.speed + left_twist_mps
        right_mps = self.speed + right_twist_mps
        #
        # Convert meters/sec into RPM: for each revolution, a wheel travels
        # pi * diameter meters, and each minute has 60 seconds.
        #
        left_target_rpm = (left_mps * 60.0) / (math.pi * self._wheel_diameter)
        right_target_rpm = (right_mps * 60.0) / \
            (math.pi * self._wheel_diameter)
        #
        left_percentage = (left_target_rpm / self._left_max_rpm) * 100.0
        right_percentage = (right_target_rpm / self._right_max_rpm) * 100.0
        #
        # clip to +- 100%
        left_percentage = max(min(left_percentage, 100.0), -100.0)
        right_percentage = max(min(right_percentage, 100.0), -100.0)
        #
        # Add in a governor to cap forward motion when we're about
        # to collide with something (but still backwards motion)
        governor = 1.0
        if self.distance < self.tooclose:
            governor = 0.0
        elif self.distance < self.close:
            governor = (self.distance - self.tooclose) / \
                (self.close - self.tooclose)
        if right_percentage > 0:
            right_percentage *= governor
        if left_percentage > 0:
            left_percentage *= governor
        #
        self._rightWheel.run(right_percentage)
        self._leftWheel.run(left_percentage)
コード例 #42
0
class Car:
    min_stop_sensor_dist = 25
    """Distance given in centimeters"""
    def __init__(self, m1_forward, m1_backward, m2_forward, m2_backward,
                 m3_forward, m3_backward, m4_forward, m4_backward, front_trig,
                 front_echo, side_trig, side_echo, back_trig, back_echo):
        GPIO.setmode(GPIO.BCM)

        self.motor1 = Motor(m1_forward, m1_backward)
        self.motor2 = Motor(m2_forward, m2_backward)
        self.motor3 = Motor(m3_forward, m3_backward)
        self.motor4 = Motor(m4_forward, m4_backward)

        self.front_sensor = DistSensor(front_trig, front_echo)
        self.side_sensor = DistSensor(side_trig, side_echo)
        self.back_sensor = DistSensor(back_trig, back_echo)

        self.running = False

        # Pool size = number of distance sensors
        dist_sensor_count = 3
        self.pool = ThreadPool(dist_sensor_count)

    def move_forward(self):
        print('Moving the car forward ...')
        self.motor1.move_forward()
        self.motor2.move_forward()
        self.motor3.move_forward()
        self.motor4.move_forward()

    def move_backward(self):
        print('Moving the car backward ...')
        self.motor1.move_backward()
        self.motor2.move_backward()
        self.motor3.move_backward()
        self.motor4.move_backward()

    def stop(self):
        print('Stopping the car ...')
        self.motor1.stop()
        self.motor2.stop()
        self.motor3.stop()
        self.motor4.stop()

    def read_distance(self, sensor):
        return sensor.read_distance()

    def read_distances(self):
        # s1 = self.front_sensor.read_distance()
        # s2 = self.side_sensor.read_distance()
        # s3 = self.back_sensor.read_distance()

        async_front = self.pool.apply_async(self.read_distance,
                                            (self.front_sensor, ))
        async_side = self.pool.apply_async(self.read_distance,
                                           (self.side_sensor, ))
        async_back = self.pool.apply_async(self.read_distance,
                                           (self.back_sensor, ))

        front_dist = async_front.get()
        side_dist = async_side.get()
        back_dist = async_back.get()

        emergency_stop_front = front_dist < Car.min_stop_sensor_dist
        emergency_stop_side = side_dist < Car.min_stop_sensor_dist
        emergency_stop_back = back_dist < Car.min_stop_sensor_dist

        print("Front = %.2f cm, side = %.2f cm, back %.2f cm" \
              % (front_dist, side_dist, back_dist))

        return front_dist, side_dist, back_dist, emergency_stop_front, emergency_stop_side, emergency_stop_back

    def cleanup(self):
        GPIO.cleanup()
コード例 #43
0
import pwm
from motor import Motor

pwm.enable()

motor = Motor()

#          pwmLeft, pwmRight, gpioLF, gpioLB, gpioRF, gpioRB
motor.attach("P9_16", "P9_31", "48", "49", "117", "115")
print "Attach successful!"

# move forward for 3 seconds
motor.move(3, "forward")
print "Move forward 3 seconds successful!"

# move left for 3 seconds
motor.direction(3, "left")
print "Move left 3 seconds successful!"

# move right for 3 seconds
motor.direction(3, "right")
print "Move right 3 seconds successful!"

# move back for 3 seconds
motor.move(3, "backward")
print "Move backward 3 seconds successful!"

motor.detach()
コード例 #44
0
    def cleanup(self):

        for motor in self.all:
            motor.stop()

        Motor.cleanup_all_motors()
コード例 #45
0
#!/usr/bin/env python

import rospy
from std_msgs.msg import String, Float64
from geometry_msgs.msg import Twist
from tf2_msgs.msg import TFMessage
from geometry_msgs.msg import TransformStamped
from nav_msgs.msg import Odometry
from math import *
import pigpio
from motor import Motor
from omni_Whiles import Omni_Control

pi = pigpio.pi()
ur_motor = Motor(pi, 14, 15)
ul_motor = Motor(pi, 17, 18)
dr_motor = Motor(pi, 22, 23)
dl_motor = Motor(pi, 9, 25)

motors = [ur_motor, ul_motor, dl_motor, dr_motor]
omni_control = Omni_Control(motors, 0.5)
twist_last = Twist()
twist_enable = False
jointstate = TFMessage()


def twist_stamped_callback(twist_msg):
    global twist_last
    global twist_enable
    twist_last = twist_msg
    twist_enable = True
コード例 #46
0
class PiCar:
    '''Builds a PiCar class'''
    def __init__(self):
        #TODO: impliment some behaviour if no client is found
        #client = ClientConnection()

        self.tilt = Servo(pwm_pin=0, center=515, minVal=425, maxVal=650)
        self.pan = Servo(1, 330, 60, 600)
        self.wheel = Servo(2, 370, 180, 520)

        #motor = Motor(4,14,15)
        self.motor = Motor(17, 27, 18)

        self.sonar = UltraSounder()
        self.camera = Camera(sonar=self.sonar, clientAddr=None)

        #TODO: give car access to leds
        # mainly so it can do the police car thing

    def drive_f(self, speed=100):
        '''
        Drive forward
        
        If no speed is specified, go full speed
        '''
        self.motor.drive(speed)
        pass

    def drive_r(self, speed=100):
        '''
        Drive in reverse

        If no speed is specified, go full speed
        '''
        self.motor.drive(-speed)
        pass

    def turn_l(self, angle=None):
        '''
        Turn wheels to the left

        If no angle specified, incriment from current position
        '''
        if angle:
            self.wheel.rotate(angle)
        else:
            self.wheel.goto_max()

        pass

    def turn_r(self, angle=None):
        '''
        Turn wheels to the right

        If no angle specified, incriment from current position
        '''
        if angle:
            self.wheel.rotate(-angle)
        else:
            self.wheel.goto_min()
        pass

    def look_l(self, angle=None):
        '''
        Pan head to the left

        If no angle specified, incriment from current position
        '''
        if angle:
            self.pan.rotate(angle)
        else:
            self.pan.goto_max()

        pass

    def look_r(self, angle=None):
        '''
        Pan head to the right

        If no angle specified, incriment from current position
        '''
        if angle:
            self.pan.rotate(-angle)
        else:
            self.pan.goto_min()

        pass

    def look_u(self, angle=None):
        '''
        Tilt head up

        If no angle specified, incriment from current position
        '''

        if angle:
            self.tilt.rotate(angle)
        else:
            self.pan.goto_max()

        pass

    def look_d(self, angle=None):
        '''
        Tilt head down

        If no angle specified, incriment from current position
        '''
        if angle:
            self.tilt.rotate(-angle)
        else:
            self.pan.goto_min()

        pass

    def all_stop(self):
        '''Stop motor'''

        self.motor.stop()

        pass

    def all_ahead(self):
        '''Move all servos to center position'''

        self.wheel.goto_center()
        self.pan.goto_center()
        self.tilt.goto_center()

        pass

    def sonar_scan(self, ):
        '''Scan sonar from left to right'''

        pass

    def __del__(self):
        '''Close everything nicely so the resources can be used again'''
        self.camera.close()
        pass  #TODO: impliment code that closes everything nicely

    def shakedown(self):
        '''Run through all capabilities'''
        pass
コード例 #47
0
ファイル: controller.py プロジェクト: KeirRice/PolarTable
class Controller(object):
	"""Abstractions to interacts with the motors."""

	def __init__(self):
		"""Init."""
		self.display = Display()

		self.r_motor = Motor(Millimeters(0.16))  # 0.16mm per step
		self.r_motor.set_hard_range(Millimeters(0.0), TABLE_RADIUS_MM)
		self.r = 0.0
		# self.r_motor.set_step(0.5)  # Half step mode

		self.t_motor = Motor(Degrees(0.05).radians())  # 0.05deg per step

		self.grid_size = PolarResolution(self.r_motor.effective_step_distance, self.t_motor.effective_step_distance)

	# def calculate_grid_size(self):
	# 	circumference = math.tau * self.r
	# 	# mm_per_raidian = circumference / Radiansmath.tau
	
	# 	target_step_mm = Millimeters(0.1)
	# 	target_steps = circumference / target_step_mm
	# 	return PolarResolution(self.r_motor.effective_step_distance, Radiansmath.tau / target_steps)

	def __str__(self):
		"""Str."""
		return u'Controller(<{0}>, <{1}>)'.format(self.t_motor, self.r_motor)

	def step_r(self, steps):
		"""Step the radius motor."""
		self.display.hud['step_r'] = 'r stepping {}'.format(steps)
		for movement in self.r_motor.steps(steps):
			self.r += movement
			yield movement
		self.display.hud['step_r'] = 'r idle'
	
	def step_t(self, steps):
		"""Step the radius motor."""
		self.display.hud['step_t'] = 't stepping {}'.format(steps)
		for movement in self.t_motor.steps(steps):
			yield movement
		self.display.hud['step_t'] = 't idle'

	if FAST_SKIP_MOTORS:
		def step(self, steps):
			"""Step the motors in sync."""
			biggest = int(max(abs(steps.r), abs(steps.t)))
			step_size = Steps(float(steps.r) / biggest, float(steps.t) / biggest)
			
			accumulation = Steps(0.0, 0.0)
			for i in range(biggest):
				accumulation += step_size
				while abs(accumulation.r) >= 1.0 or abs(accumulation.t) >= 1.0:
					this_step = Polar(0.0, 0.0)
					if abs(accumulation.r) >= 1.0:
						r_step = self.r_motor.effective_step_distance * (self.r_motor.step_size * sign(steps.r))
						this_step.r = r_step
						accumulation.r = accumulation.r - sign(steps.r)
						
					if abs(accumulation.t) >= 1.0:
						this_step.t = self.t_motor.effective_step_distance * (self.t_motor.step_size * sign(steps.t))
						accumulation.t = accumulation.t - sign(steps.t)
					yield this_step

	else:
		def step(self, steps):
			"""Step the motors in sync."""
			biggest = int(max(abs(steps.r), abs(steps.t)))
			step_size = Steps(float(steps.r) / biggest, float(steps.t) / biggest)
			
			self.r_motor.set_direction(sign(steps.r))
			self.t_motor.set_direction(sign(steps.t))

			accumulation = Steps(0.0, 0.0)
			for i in range(biggest):
				accumulation += step_size
				while abs(accumulation.r) >= 1.0 or abs(accumulation.t) >= 1.0:
					this_step = Polar(0.0, 0.0)
					if abs(accumulation.r) >= 1.0:
						r_step = self.r_motor.step()
						this_step.r = r_step
						if r_step.is_boundry:
							accumulation.r = 0.0
						else:
							accumulation.r = accumulation.r - sign(steps.r)
						
					if abs(accumulation.t) >= 1.0:
						this_step.t = self.t_motor.step()
						accumulation.t = accumulation.t - sign(steps.t)
					yield this_step

	def motor_step_distance(self):
		"""Return the step size of the motors."""
		return Polar(self.r_motor.step_distance, self.t_motor.step_distance)
コード例 #48
0
ファイル: plugins.py プロジェクト: xfangfang/PiCar
 def __init__(self, bus):
     super(MotorPlugin, self).__init__(bus)
     logger.info('Initializing MotorPlugin')
     self.motor = Motor(L1,L2,R1,R2,INIT)
コード例 #49
0
ファイル: swivel.py プロジェクト: clubcapra/Ibex
 def __init__(self, name):
     Motor.__init__(self, name, name)
     self.serial_com = serial.Serial(Config.get_swivel_encoder_port(),Config.get_swivel_encoder_baud())
     self._last_angle = 0
コード例 #50
0
ファイル: motorkit.py プロジェクト: nivaranavat/SmartCar
class MotorKit:

	"""  the 4 Adafruit DC motors kit"""


	def __init__(self, pca):
		self.pca = pca

		self.motor1 = Motor(self.pca, [0,1])
		self.motor2 = Motor(self.pca, [3,2])
		self.motor3 = Motor(self.pca, [4,5])
		self.motor4 = Motor(self.pca,[6,7])
		
		self.speed = 0.5 #default to 0.5 change it when needed, turns need slower

	def changeSpeed(self, speed):
		self.speed = speed

	def forward(self):
		
		""" move all motors forward for point one second """


		self.motor1.moveForwardwithSpeed(self.speed,0)
		self.motor2.moveForwardwithSpeed(self.speed,0)
		self.motor3.moveForwardwithSpeed(self.speed,0)
		self.motor4.moveForwardwithSpeed(self.speed,0)
		time.sleep(0.1)
	
	def backward(self):
		""" move all motors backward for point one second """
		self.motor1.moveBackwardwithSpeed(self.speed,0)
		self.motor2.moveBackwardwithSpeed(self.speed,0)
		self.motor3.moveBackwardwithSpeed(self.speed,0)
		self.motor4.moveBackwardwithSpeed(self.speed,0)
		time.sleep(0.1)

	def stop(self):
		"""  stop all movements"""
		self.motor1.stop()
		self.motor2.stop()
		self.motor3.stop()
		self.motor4.stop()

	def forwardLeft(self):
		self.motor4.moveForwardwithSpeed(self.speed,0)
		self.motor3.moveForwardwithSpeed(self.speed,0)
		time.sleep(0.1)

	def forwardRight(self):
		self.motor1.moveForwardwithSpeed(self.speed,0)
		self.motor2.moveForwardwithSpeed(self.speed,0)
		time.sleep(0.1)
	
	def backwardLeft(self):
		self.motor1.moveBackwardwithSpeed(self.speed,0)
		self.motor2.moveBackwardwithSpeed(self.speed,0)
		time.sleep(0.1)

	def backwardRight(self):
		self.motor3.moveBackwardwithSpeed(self.speed,0)
		self.motor4.moveBackwardwithSpeed(self.speed,0)
		time.sleep(0.1)
コード例 #51
0
    def test_load_drivetrain_from_file(self):
        testbot = SimpleDrivetrain()
        filepath = 'drivetrain_test.xml'
        testbot.load_drivetrain_from_file(filepath)

        observed_motors = testbot.motors

        pwm_bounds_std = (1100, 1500, 1900)
        norm_const = np.sqrt(2) / 2
        expected_motors = [
            Motor('front_right', (1, 1, 0), (-norm_const, norm_const, 0), True,
                  pwm_bounds_std),
            Motor('front_left', (-1, 1, 0), (norm_const, norm_const, 0), False,
                  pwm_bounds_std),
            Motor('back_left', (-1, -1, 0), (norm_const, -norm_const, 0),
                  False, pwm_bounds_std),
            Motor('back_right', (1, -1, 0), (-norm_const, -norm_const, 0),
                  False, pwm_bounds_std),
            Motor('front_ascent', (0, 0.5, 0), (0, 0, 1), False,
                  pwm_bounds_std),
            Motor('back_ascent', (0, -0.5, 0), (0, 0, 1), False,
                  pwm_bounds_std)
        ]

        expected_orientation = (0, 0, 0)
        observed_orientation = testbot.orientation

        for i in range(0, len(expected_orientation)):
            self.assertAlmostEqual(expected_orientation[i],
                                   observed_orientation[i])

        for i in range(0, len(expected_motors)):
            observed_motor = observed_motors[i]
            expected_motor = expected_motors[i]

            observed_name = observed_motor.name
            expected_name = expected_motor.name

            observed_inverted = observed_motor.inverted
            expected_inverted = expected_motor.inverted

            observed_position = observed_motor.position
            expected_position = expected_motor.position

            observed_direction = observed_motor.direction
            expected_direction = expected_motor.direction

            observed_pwm_bounds = observed_motor.pwm_bounds
            expected_pwm_bounds = expected_motor.pwm_bounds

            self.assertEqual(expected_name, observed_name)
            self.assertEqual(expected_inverted, observed_inverted)
            for j in range(0, len(expected_position)):
                self.__assertWithinRange(expected_position[j],
                                         observed_position[j], 0.01)
            for j in range(0, len(expected_direction)):
                self.__assertWithinRange(expected_direction[j],
                                         observed_direction[j], 0.01)
            for j in range(0, len(expected_pwm_bounds)):
                self.assertEqual(expected_pwm_bounds[j],
                                 observed_pwm_bounds[j], 0.01)
コード例 #52
0
ファイル: testMain.py プロジェクト: pcruiher08/rubikCV
                U.turn90CW()
                U.turn90CW()
            else:
                U.turn90(directionFlag)
        if(move[0] == 'D'):
            if(twoMoves):
                D.turn90CW()
                D.turn90CW()
            else:
                D.turn90(directionFlag)
        twoMoves = False
        directionFlag = False

turnDelay = .005 / 8 * 1/2

F = Motor(26,19)
L = Motor(13,6)
R = Motor(21, 20)
B = Motor(16,12)
U = Motor(5, 22)
D = Motor(27, 17)

screen = LCD.lcd()
screen.lcd_clear()
screen.lcd_display_string("hay 6 motores", 1)

F.setDelay(turnDelay)
L.setDelay(turnDelay)
R.setDelay(turnDelay)
B.setDelay(turnDelay)
U.setDelay(turnDelay)
コード例 #53
0
Ridwan Alaoudian
Tyler Bursa
"""

# Import classes
from button import Button
from linear_actuator import LinearActuator
from load_cell_amplifier import LoadCellAmplifier
from load_cell import LoadCell
from motor import Motor
from rotary_encoder import RotaryEncoder

import multiprocessing

# Initialize objects
motor = Motor(20, 21)
linear_actuator = LinearActuator(motor)
rotary_encoder = RotaryEncoder(9, 11, 10)
load_cell_amplifier = LoadCellAmplifier(5, 6)
load_cell = LoadCell(load_cell_amplifier)

# Initialize buttons
up_button = Button(27, linear_actuator.move_up)
down_button = Button(17, linear_actuator.move_down)
stop_button = Button(2, linear_actuator.stop)
inc_speed_button = Button(7, linear_actuator.increase_speed)
dec_speed_button = Button(3, linear_actuator.decrease_speed)

# Initialize limit switches
bottom_limit_switch = Button(19, linear_actuator.stop)
top_limit_switch = Button(26, linear_actuator.stop)
コード例 #54
0
class Predict:
    """
    Class for making a prediction based on real hardware
    """

    def __init__(self, directory, model):

        self.current_position = 0
        self.iteration = 0
        self.directory = directory
        self.predictions = []
        self.model = model
        self.rotation = 0
        self.mic_centre = np.array([1.5, 1.5])
        self.prediction = 0
        self.audio = audio_recorder(self.directory)
        self.motor = Motor()
        self.current_model = self.get_model()  # init
        self.motor_offset = 0
        self.relative_prediction = 0
        self.results = []
        self.initial_adjust = False

    def store_prediction(self, doa_list):
        """
        convert relative prediction to home coordinates
        """

        true_doas = [utility_methods.cylindrical(self.current_position + doa_list[0]),
                     utility_methods.cylindrical(self.current_position + doa_list[1])]

        self.predictions.append(true_doas)

    def get_model(self):

        model = None
        if self.model == "gcc_cnn":
            model = final_models.gcc_cnn()
        elif self.model == "gcc_dsp":
            model = final_models.gcc_dsp()
        else:
            print("Error -> No file found")
        return model

    def record(self):

        self.motor.red_led_on()
        self.audio.record(self.iteration)
        self.motor.red_led_off()

    def load_audio(self):
        """
        Reads wav file based on csv values, resamples audio to 8000hz, fixes length to 1 second
        :return: numpy array of stereo audio, DOA from file
        """
        df = pd.read_csv("{dir}/iteration_{iter}.csv".format(dir=self.directory, iter=self.iteration),
                         usecols=[1, 2])

        wav_name = df.iloc[0][0]
        filename = "{wav_name}".format(wav_name=wav_name)

        y, sr = librosa.load(filename, mono=False)

        y_8k = librosa.resample(y, sr, 8000)

        o_env = librosa.onset.onset_strength(y_8k[0], sr=8000)

        peaks = librosa.util.peak_pick(o_env, 3, 3, 3, 5, 0.25, 5)

        times = librosa.frames_to_time(np.arange(len(o_env)),
                                       sr=8000, hop_length=512)

        peak_times = times[peaks]

        time = 0
        for i in range(1, len(peak_times) + 1):
            if 3 - peak_times[-i] >= 0.75:
                time = peak_times[-i] - 0.25
                break

        sample = librosa.time_to_samples(np.array([time]), sr=8000)

        sliced_y = np.array([y_8k[0][sample[0]:], y_8k[1][sample[0]:]])

        y_out = librosa.util.fix_length(sliced_y, 8000)

        return y_out

    def format_gcc_cnn(self):
        """
            Format the stereo Audio file for input to the gcc_phat CNN
            :return: data formatted for gcc_phat CNN, DOA read from file
            """
        result_x = self.load_audio()

        signal = result_x[0]
        reference_signal = result_x[1]
        _, raw_gcc_vector = gccphat.gcc_phat(signal=signal, reference_signal=reference_signal, fs=8000)

        cross_correlation_vector = cnn.reshape_x_for_cnn(cnn.normalize_x_data(np.array([raw_gcc_vector])))

        return cross_correlation_vector

    def format_gcc_dsp(self):
        """
        Format stereo audio file for gcc_dsp model
        :return: signal, reference signal and doa_from_file
        """
        result_x = self.load_audio()

        return result_x

    def load_and_process_audio(self):
        """
        Wrapping loading and processing of models into a single function
        """
        output_vector = None
        if self.model == "gcc_cnn":
            output_vector = self.format_gcc_cnn()
        elif self.model == "gcc_dsp":
            output_vector = self.format_gcc_dsp()
        else:
            print("Error -> No file found")

        return output_vector

    def compute_rotation(self):
        """
        compute rotation based on current and prior predictions
        :return:
        """

        if self.predictions[self.iteration][0] == 90.0 or self.predictions[self.iteration][0] == 270.0:
            self.rotation = 20
            self.initial_adjust = True
            return

        if self.iteration == 0:
            self.rotation = rotate.get_90_deg_rotation(self.predictions[self.iteration])
        elif self.iteration == 1:
            self.rotation = rotate.get_45_deg_rotation(self.predictions, self.current_position)
        elif self.iteration >= 2:
            self.rotation = rotate.get_fine_rotation(self.iteration)

    def update_position(self):
        """
        update current position of microphone based on rotation
        :param rotation:
        :return:
        """
        self.current_position = utility_methods.cylindrical(self.current_position + self.rotation)
        self.motor.rotate(np.abs(self.rotation), "cw" if self.rotation < 0 else "ccw")

    def compute_rotation_to_prediction(self):

        self.rotation = self.prediction - self.current_position - 90
        # self.rotation = ((self.prediction - self.current_position) + 180) % 360 - 180 -90

    def reset(self):
        """
        This method resets the prediction, iteration, position and rotation values to initial state. Rotates The
        motor back to 0 degrees

        :return:
        """

        self.rotation = -(self.prediction - 90)
        self.update_position()

        self.iteration = 0
        self.predictions = []
        self.prediction = 0
        self.change_motor_offset(-self.motor_offset)

    def change_motor_offset(self, offset):
        self.motor_offset = offset

        self.motor.rotate(np.abs(self.motor_offset), "ccw" if self.motor_offset < 0 else "cw")

    def save_results(self):

        results_row = [self.prediction, self.motor_offset, self.prediction - self.motor_offset, self.iteration]

        self.results.append(results_row)

        #

    def run(self):

        while self.iteration < 6:

            self.record()

            print("Recording Successful")

            vector = self.load_and_process_audio()

            doa_list = self.current_model.predict(vector)

            print("Model Prediction: {list}".format(list=doa_list))

            self.store_prediction(doa_list)

            print("Prediction List: {list}".format(list=self.predictions))

            val = utility_methods.check_if_twice(self.predictions, self.iteration)

            if val is not None:
                self.prediction = val
                self.compute_rotation_to_prediction()
                self.update_position()
                return self.prediction

            self.compute_rotation()

            print("Rotation: {rotation}".format(rotation=self.rotation))

            self.update_position()
            print("Current Position: {position}".format(position=self.current_position))

            self.iteration += 1

        self.prediction = utility_methods.get_mean_prediction(prediction_list=self.predictions)
        self.compute_rotation_to_prediction()
        self.update_position()
        self.motor.green_led()
        return self.prediction

    def evaluate(self):

        for i in np.arange(0, 360, 45):
            self.change_motor_offset(i)
            self.run()

            self.save_results()
            self.reset()

        df = pd.DataFrame(self.results, columns=['prediction', 'offset', 'prediction - offset', 'Number of iterations'])
        df.to_csv("{dir}/iteration_{model}.csv".format(dir="evaluation_results", model=self.model))
コード例 #55
0
robotY = 800

robot = Robot(screen, robotX, robotY)

y_change = 0
x_change = 0

barrierList = []

joysticks = []

pygame.joystick.init()

c = Controller()

fl = Motor(4)
fr = Motor(17)
br = Motor(22)
bl = Motor(27)

auto = Automobile(fr, br, fl, bl)

angle = 0
rotation = ''

running = True
while running:

    screen.fill((0,0,0))
    for event in pygame.event.get():
        if event.type == pygame.QUIT:
コード例 #56
0
ファイル: main.py プロジェクト: vignolet/turbo-happiness
from motor import Motor
import time

m = Motor('moteur 1', 5, 3, 10)
m.start()
time.sleep(3)
m.setPwmRapport(40)
time.sleep(1)
m.setPwmRapport(50)
time.sleep(1)
m.setPwmRapport(60)
time.sleep(1)
m.setPwmRapport(70)
time.sleep(1)
m.setPwmRapport(80)
time.sleep(1)
m.setPwmRapport(90)
time.sleep(1)
m.setPwmRapport(70)
time.sleep(1)
m.setPwmRapport(50)
time.sleep(1)
m.reverse(True)
time.sleep(3)
m.reverse(True)
time.sleep(3)
m.stop()
m.clear()
コード例 #57
0
def panic_button(y, b, w, bl, io):
    m1 = Motor(y, b, w, bl, io)
    moves = [0, 0, 0, 0]
    for move in moves:
        m1.move_deg(move)
コード例 #58
0
from motor_angle_control import MotorAngleControl
from motor import Motor
from encoder import Encoder
from car_config import gpio_dict

# 左侧电机
lmotor = Motor(gpio_dict['LEFT_MOTOR_A'],
               gpio_dict['LEFT_MOTOR_B'])  #,motor_install_dir=False)
# 右侧电机
rmotor = Motor(gpio_dict['RIGHT_MOTOR_A'], gpio_dict['RIGHT_MOTOR_B'])

# 左侧编码器
lencoder = Encoder(gpio_dict['LEFT_ENCODER_A'],
                   gpio_dict['LEFT_ENCODER_A'],
                   3,
                   motor_install_dir=False,
                   name='LeftEncoder',
                   is_debug=False)

mac = MotorAngleControl(lmotor, lencoder, kp=-10, is_debug=True)
mac.set_angle(90, is_reset=True)
コード例 #59
0
import ultrasonic
from time import sleep
#from machine import Pin
from motor import Motor

motor_left = Motor("left", "D6", "D7", "D4")
motor_right = Motor("right", "D8", "D9", "D5")

print("Hello, world!")  # Print a welcome message on reset

# These statements make the code more readable.
# Instead of a pin number "D13" or "D12" we can now write "TRIG" or "ECHO"
TRIG = "D13"
ECHO = "D12"
ultrasonic_sensor = ultrasonic.HCSR04(TRIG, ECHO)

while True:
    dist = ultrasonic_sensor.distance_mm()

    if dist >= 60:
        print('if')
        #motor_left.ctrl_alloc(1, 70)
        #motor_right.ctrl_alloc(1, 70)

        motor_left.set_forwards()
        motor_right.set_forwards()

        motor_left.duty(90)
        motor_right.duty(90)
        sleep(0.1)
コード例 #60
-3
ファイル: square.py プロジェクト: godiard/frank
class Square:

    def __init__(self):
        print "Set mode BCM"
        config = json.load(open("config.json"))
        GPIO.setmode(GPIO.BCM)
        self.motorX = Motor(config["motor_x"]["pins"])
        self.motorX.name = "X"
        self.motorX.delay = config["motor_x"]["delay"]
        self.motorX.steps_by_mm = config["motor_x"]["steps_by_mm"]

        self.motorY = Motor(config["motor_y"]["pins"])
        self.motorY.name = "Y"
        self.motorY.delay = config["motor_y"]["delay"]
        self.motorY.steps_by_mm = config["motor_y"]["steps_by_mm"]

        self.laser = Laser(config["laser"]["pin"])

    def start(self):
        size = 10
        self.laser.on()
        self.move(self.motorY, Motor.RIGHT, size)
        self.move(self.motorX, Motor.RIGHT, size)
        self.move(self.motorY, Motor.LEFT, size)
        self.move(self.motorX, Motor.LEFT, size)
        self.laser.off()
        self.motorX.off()
        self.motorY.off()

    def move(self, motor, direction, size):
        print "motor %s %d %d" % (motor.name, direction,
                                  size * motor.steps_by_mm)
        for x in range(0, int(size * motor.steps_by_mm)):
            motor.moveTo(direction)

    def cleanUp(self):
        GPIO.cleanup()