def setup():
	global offset_x,  offset_y, offset, forward0, forward1
	offset_x = 0
	offset_y = 0
	offset = 0
	forward0 = 'True'
	forward1 = 'False'
	try:
		for line in open('config'):
			if line[0:8] == 'offset_x':
				offset_x = int(line[11:-1])
				print 'offset_x =', offset_x
			if line[0:8] == 'offset_y':
				offset_y = int(line[11:-1])
				print 'offset_y =', offset_y
			if line[0:8] == 'offset =':
				offset = int(line[9:-1])
				print 'offset =', offset
			if line[0:8] == "forward0":
				forward0 = line[11:-1]
				print 'turning0 =', forward0
			if line[0:8] == "forward1":
				forward1 = line[11:-1]
				print 'turning1 =', forward1
	except:
		print 'no config file, set config to original'
	video_dir.setup(busnum=busnum)
	car_dir.setup(busnum=busnum)
	motor.setup(busnum=busnum) 
	video_dir.calibrate(offset_x, offset_y)
	car_dir.calibrate(offset)
예제 #2
0
def setup():
    global offset_x, offset_y, offset, forward0, forward1
    offset_x = 0
    offset_y = 0
    offset = 0
    forward0 = 'True'
    forward1 = 'False'
    try:
        for line in open('config'):
            if line[0:8] == 'offset_x':
                offset_x = int(line[11:-1])
                print 'offset_x =', offset_x
            if line[0:8] == 'offset_y':
                offset_y = int(line[11:-1])
                print 'offset_y =', offset_y
            if line[0:8] == 'offset =':
                offset = int(line[9:-1])
                print 'offset =', offset
            if line[0:8] == "forward0":
                forward0 = line[11:-1]
                print 'turning0 =', forward0
            if line[0:8] == "forward1":
                forward1 = line[11:-1]
                print 'turning1 =', forward1
    except:
        print 'no config file, set config to original'
    video_dir.setup(busnum=busnum)
    car_dir.setup(busnum=busnum)
    motor.setup(busnum=busnum)
    video_dir.calibrate(offset_x, offset_y)
    car_dir.calibrate(offset)
예제 #3
0
def setup():
    GPIO.setwarnings(False)
    GPIO.setmode(GPIO.BOARD)
    GPIO.setup(line_pin_right, GPIO.IN)
    GPIO.setup(line_pin_middle, GPIO.IN)
    GPIO.setup(line_pin_left, GPIO.IN)
    motor.setup()
예제 #4
0
    def __init__(self):
        self.readerInput = [0, 0, 0]
        self.observations = [
            [0, 0, 0],  #TODO: add as default options
            [0, 0, 0],
            [0, 0, 0],
            [0, 0, 0],
            [0, 0, 0],
            [0, 0, 0],
            [0, 0, 0],
            [0, 0, 0],
            [0, 0, 0],
            [0, 0, 0],
            [0, 0, 0],
            [0, 0, 0]
        ]

        # Set up pins for linereader
        GPIO.setwarnings(False)
        GPIO.setmode(GPIO.BCM)
        GPIO.setup(line_pin_right, GPIO.IN)
        GPIO.setup(line_pin_middle, GPIO.IN)
        GPIO.setup(line_pin_left, GPIO.IN)

        # Define PID controller
        self.pCorrection = 5
        self.iCorrection = 1
        self.dCorrection = 1

        try:
            motor.setup()
        except:  #??? what exception are
            pass
예제 #5
0
def run_mode(request):
	video_dir.setup()
	car_dir.setup()
	motor.setup()
	video_dir.home_x_y()
	car_dir.home()
	motor.setSpeed(50)
	return HttpResponse("Run mode start")
예제 #6
0
def run_robot(t=0.1, t1=0.01, t2=0.01, direction=1, speed=100):
    motor.setup()    
    # Run robot forward in t seconds then backword in t seconds
    ## Forward
    motor.run_robot_t(t=t, t1=t1, t2=t2, direction=direction, speed=speed)
    time.sleep(1)
    ## Backward
    motor.run_robot_t(t=t, t1=t1, t2=t2, direction=1-direction, speed=speed)
예제 #7
0
def setup():
    GPIO.setwarnings(False)
    GPIO.setmode(GPIO.BCM)
    GPIO.setup(line_pin_right, GPIO.IN)
    GPIO.setup(line_pin_middle, GPIO.IN)
    GPIO.setup(line_pin_left, GPIO.IN)
    try:
        motor.setup()
    except:
        pass
예제 #8
0
def setup():
	# Setup and calibrate direction and motor
	car_dir.setup(busnum=busnum)
	motor.setup(busnum=busnum)
	car_dir.home()

	# Starting streamer thread for image processing
	mjpg_st_th = MJPGStreamerThread()
	mjpg_st_th.start()

	em_th = EmergencyModule()
	em_th.start()
예제 #9
0
 def __init__(self):
     print('debug')
     busnum = 0
     logging.info('Creating an SDC object')
     steer_motor.setup()
     print('debug2')
     drive_motor.setup()
     drive_motor.ctrl(1)
     self.spd_cmd = 0
     self.spd_cur = self.spd_cmd
     self.str_cmd = 450
     self.str_cur = self.str_cmd
     steer_motor.home()
예제 #10
0
    def advance_robot(self):
  	if self.robot_state == START:
            motor.setup()
            self.robot_state = STOPPED

        elif self.robot_state == STOPPED:
            motor.cw()
            self.robot_state = CW

        elif self.robot_state == CW:
            motor.ccw()
            self.robot_state = CCW

        elif self.robot_state == CCW:
            motor.stop()
            self.robot_state = STOPPED
예제 #11
0
def track_object(distance_stay, distance_range, speed_adj=0.4):
    # distance_stay,distance_range in inches
    #Tracking with Ultrasonic
    motor.setup()
    led.setup()
    turn.ahead()
    turn.middle()
    dis = checkdist_inches()
    if dis < distance_range:  #Check if the target is in distance range
        if dis > (
                distance_stay + 4
        ):  #If the target is in distance range and out of distance stay, then move forward to track
            turn.ahead()
            moving_time = (dis - distance_stay) / 0.38
            if moving_time > 1:
                moving_time = 1
            print('mf')
            led.both_off()
            led.cyan()
            motor.motor_left(status, backward,
                             int(left_spd * spd_ad_u * speed_adj))
            motor.motor_right(status, forward,
                              int(right_spd * spd_ad_u * speed_adj))
            time.sleep(moving_time)
            motor.motorStop()
        elif dis < (
                distance_stay - 4
        ):  #Check if the target is too close, if so, the car move back to keep distance at distance_stay
            moving_time = (distance_stay - dis) / 0.38
            print('mb')
            led.both_off()
            led.pink()
            motor.motor_left(status, forward,
                             int(left_spd * spd_ad_u * spd_adj))
            motor.motor_right(status, backward,
                              int(right_spd * spd_ad_u * spd_adj))
            time.sleep(moving_time)
            motor.motorStop()
        else:  #If the target is at distance, then the car stay still
            motor.motorStop()
            led.both_off()
            led.yellow()
    else:
        motor.motorStop()
예제 #12
0
def setup():
    global offset_x, offset_y, offset, forward0, forward1, backward0, backward1
    offset_x = 0
    offset_y = 0
    offset = 0
    forward0 = 'True'
    forward1 = 'False'

    # Read calibration value from config file
    try:
        for line in open('config'):
            if line[0:8] == 'offset_x':
                offset_x = int(line[11:-1])
                print
                'offset_x =', offset_x
            if line[0:8] == 'offset_y':
                offset_y = int(line[11:-1])
                print
                'offset_y =', offset_y
            if line[0:8] == 'offset =':
                offset = int(line[9:-1])
                print
                'offset =', offset
            if line[0:8] == "forward0":
                forward0 = line[11:-1]
                print
                'turning0 =', forward0
            if line[0:8] == "forward1":
                forward1 = line[11:-1]
                print
                'turning1 =', forward1
    except:
        print
        'no config file, set config to original'
    video_dir.setup(busnum=busnum)
    car_dir.setup(busnum=busnum)
    motor.setup(busnum=busnum)
    video_dir.calibrate(offset_x, offset_y)
    car_dir.calibrate(offset)

    # Set the motor's true / false value to the opposite.
    backward0 = REVERSE(forward0)
    backward1 = REVERSE(forward1)
예제 #13
0
    def __init__(self):
        self._lin_vel = 0
        self._ang_vel = 0
        self._cmd_lin_vel = 0
        self._cmd_ang_vel = 0
        self._wheel_vel = (0, 0)
        self._axle_length = 0.11
        self._wheel_radius = 0.025
        self._left_motor_dir = 1
        self._right_motor_dir = 0
        GPIO.setwarnings(False)
        GPIO.setmode(GPIO.BOARD)
        motor.setup()
        motor.motorStop()

        self._twist_sub = rospy.Subscriber('cmd_vel',
                                           Twist,
                                           self._twist_callback,
                                           queue_size=1)
        self._sonar_pub = rospy.Publisher('sonar', Range, queue_size=1)
예제 #14
0
def mover():

    sRobotDirection = Direction()

    rospy.init_node('mover', anonymous=True)

    rospy.Subscriber("motioncommand", String,
                     sRobotDirection.direction_callback)

    video_dir.setup(busnum=busnum)
    car_dir.setup(busnum=busnum)
    motor.setup(
        busnum=busnum
    )  # Initialize the Raspberry Pi GPIO connected to the DC motor.
    video_dir.home_x_y()
    car_dir.home()

    #data = sRobotDirection.directionMsg

    rospy.spin()
예제 #15
0
def loop(distance_stay,distance_range):
    '''Tracking with Ultrasonic'''
    motor.setup()
    led.setup()
    turn.ahead()
    turn.middle()
    dis = checkdist()
    if dis < distance_range:
        #Check if the target is in diatance range
        if dis > (distance_stay+0.1) :
            #If the target is in distance range and out of distance stay,
            #then move forward to track
            turn.ahead()
            moving_time = (dis-distance_stay)/0.38 #??? magic number
            if moving_time > 1:
                moving_time = 1
            print('mf')
            led.both_off()
            led.cyan()
            motor.motor_left(status, backward,left_spd*spd_ad_u)
            motor.motor_right(status,forward,right_spd*spd_ad_u)
            time.sleep(moving_time)
            motor.motorStop()
        elif dis < (distance_stay-0.1) :
            #Check if the target is too close,
            #if so, the car move back to keep distance at distance_stay
            moving_time = (distance_stay-dis)/0.38
            print('mb')
            led.both_off()
            led.pink()
            motor.motor_left(status, forward,left_spd*spd_ad_u)
            motor.motor_right(status,backward,right_spd*spd_ad_u)
            time.sleep(moving_time)
            motor.motorStop()
        else:
            #If the target is at distance, then the car stay still
            motor.motorStop()
            led.both_off()
            led.yellow()
    else:
        motor.motorStop()
예제 #16
0
def setup():
    global offset, offset_x, offset_y, forward0, forward1, backward0, backward1
    offset_x = 0
    offset_y = 0
    offset = 0
    forward0 = 'True'
    forward1 = 'False'

    # Read calibration value from config file
    try:
        for line in open('config'):
            find_line(offset_x, "offset_x", 'offset_x')

            find_line(offset_y, "offset_y", 'offset_y')

            find_line(forward0, "forward0", 'turning0')

            find_line(forward1, "forward1", 'turning1')

            if line[0:8] == 'offset =':
                offset = int(line[9:-1])
                print
                'offset =', offset

    except:
        print
        'no config file, set config to original'

    video_dir.setup(busnum=busnum)
    car_dir.setup(busnum=busnum)
    motor.setup(busnum=busnum)
    video_dir.calibrate(offset_x, offset_y)
    car_dir.calibrate(offset)

    # Set the motor's true / false value to the opposite.
    backward0 = REVERSE(forward0)
    backward1 = REVERSE(forward1)
예제 #17
0
    def __init__(self):
        rospy.loginfo("Setting up the node...")

        rospy.init_node("ts_llc")
        motor.setup()
        motor.ctrl(1)

        #--- Create the actuator dictionary
        self.actuators = {}
        self.actuators['throttle'] = ServoConvert(id=1, centre_value=0)
        motor.setSpeed(0)
        self.actuators['steering'] = ServoConvert(id=2,
                                                  direction=1)  #-positive left

        #--- Create the servo array publisher
        #-- Create the message
        self._servo_msg = ServoArray()
        for i in range(2):
            self._servo_msg.servos.append(Servo())

        self.ros_pub_servo_array = rospy.Publisher("/servos_absolute",
                                                   ServoArray,
                                                   queue_size=1)
        rospy.loginfo("> Publisher correctly initialized")

        #--- Create the subscriber to the /cmd-vel topic
        self.ros_sub_twist = rospy.Subscriber("/cmd_vel", Twist,
                                              self.set_actuator_from_cmdvel)
        #self.motor_sub = rospy.Subscriber("/cmd_vel", Twist, self.move_dc_from_cmdvel)
        rospy.loginfo("> subscriber correctly initialized")

        #--- save the last time we got a reference. Stop the vehicle if no commands given
        self._last_time_cmd_rcv = time.time()
        self._timeout_s = 5  #-- stop after 5 seconds

        rospy.loginfo("Initialization complete")
예제 #18
0
def setup():  #initialization
    motor.setup()
    led.setup()
예제 #19
0
def init():
    motor.setup()
    servo_test.setup()
    motor.setSpeed(0)
    servo_test.pwm.write(0,0,500)
예제 #20
0
파일: speech.py 프로젝트: bswe/robot_car
def setup():
    GPIO.setwarnings(True)
    try:
        motor.setup()
    except:
        pass
def setup():
    motor.setup()
    led.setup()
    turn.turn_middle()
    led.green()
예제 #22
0
 def setup():
     motor.setup()
예제 #23
0
def setup():  #initialization
    motor.setup()
    turn.ahead()
    findline.setup()
예제 #24
0
def setup():
    '''initialization'''
    #??? seperate function seems unecessary
    motor.setup()
    turn.ahead()
    findline.setup()
예제 #25
0
 def __init__(self):
     m.setup()  #Have to be done
     cd.setup()
예제 #26
0
                            offset_y = int(line[11:-1])
                                print 'offset_y =', offset_y
                        if line[0:8] == 'offset =':
                            offset = int(line[9:-1])
                                print 'offset =', offset
                        if line[0:8] == "forward0":
                            forward0 = line[11:-1]
                                print 'turning0 =', forward0
                        if line[0:8] == "forward1":
                            forward1 = line[11:-1]
                                print 'turning1 =', forward1
        except:
            print 'no config file, set config to original'
        video_dir.setup(busnum=busnum)
        car_dir.setup(busnum=busnum)
        motor.setup(busnum=busnum)
        video_dir.calibrate(offset_x, offset_y)
        car_dir.calibrate(offset)

def REVERSE(x):
    if x == 'True':
        return 'False'
    elif x == 'False':
        return 'True'

def loop():
    global offset_x, offset_y, offset, forward0, forward1
        while True:
            print 'Waiting for connection...'
                # Waiting for connection. Once receiving a connection, the function accept() returns a separate
                # client socket for the subsequent communication. By default, the function accept() is a blocking
예제 #27
0
def init():
    motor.setup()
    motor.ctrl(1)
    global pwm
    pwm = servo.PWM()
예제 #28
0
        if msg == 'forward':
            motor.forward()

        if msg == 'stop':
            motor.stop()

        if msg == 'right':
            motor.right()

        if msg == 'left':
            motor.left()

        if msg == 'backward':
            motor.backward()

    #greeting = "Hello {}!".format(name)
    #await websocket.send(greeting)
    #print("> {}".format(greeting))


if __name__ == "__main__":
    # Setup motors
    GPIO.setmode(GPIO.BOARD)
    motor.setup()

    # Setup websocket server
    start_server = websockets.serve(hello, '192.168.1.202', 5678)
    asyncio.get_event_loop().run_until_complete(start_server)
    asyncio.get_event_loop().run_forever()
예제 #29
0
import car_dir

import motor
import time
import numpy as np
import cv2
from visionAlg.myFunctions import readyImage, splitImage, findCentroid, showRows, showCentroids, errorCalc

car_dir.setup()
motor.setup()
motor.setSpeed(0)


def Map(x, in_min, in_max, out_min, out_max):
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min


def saturate(x, ub, lb):
    if x > ub:
        return (ub)
    elif x < lb:
        return (lb)
    else:
        return (x)


kp = 1
ki = 80
kd = 0

lbase = -70  # numeric values for the turn limits of the car (lbase = left rbase = right)
예제 #30
0
def setup():
    motor.setup()
    led.setup()
    breathingLed.setup()
    activeBuzzer.setup()
예제 #31
0
def drive_request_setup():
    car_dir_deep_drive.setup()
    motor.setup(
    )  # Initialize the Raspberry Pi GPIO connected to the DC motor.
    car_dir_deep_drive.home()
busnum = 1          # Edit busnum to 0, if you uses Raspberry Pi 1 or 0

HOST = ''           # The variable of HOST is null, so the function bind( ) can be bound to all valid addresses.
PORT = 21567
BUFSIZ = 1024       # Size of the buffer
ADDR = (HOST, PORT)

tcpSerSock = socket(AF_INET, SOCK_STREAM)    # Create a socket.
tcpSerSock.bind(ADDR)    # Bind the IP address and port number of the server. 
tcpSerSock.listen(5)     # The parameter of listen() defines the number of connections permitted at one time. Once the 
                         # connections are full, others will be rejected. 

video_dir.setup(busnum=busnum)
car_dir.setup(busnum=busnum)
motor.setup(busnum=busnum)     # Initialize the Raspberry Pi GPIO connected to the DC motor. 
video_dir.home_x_y()
car_dir.home()

while True:
	print 'Waiting for connection...'
	# Waiting for connection. Once receiving a connection, the function accept() returns a separate 
	# client socket for the subsequent communication. By default, the function accept() is a blocking 
	# one, which means it is suspended before the connection comes.
	tcpCliSock, addr = tcpSerSock.accept() 
	print '...connected from :', addr     # Print the IP address of the client connected with the server.

	while True:
		data = ''
		data = tcpCliSock.recv(BUFSIZ)    # Receive data sent from the client. 
		# Analyze the command received and control the car accordingly.
예제 #33
0
busnum = 1          # Edit busnum to 0, if you uses Raspberry Pi 1 or 0

HOST = ''           # The variable of HOST is null, so the function bind( ) can be bound to all valid addresses.
PORT = 21565
BUFSIZ = 1024       # Size of the buffer
ADDR = (HOST, PORT)

tcpSerSock = socket(AF_INET, SOCK_STREAM)    # Create a socket.
tcpSerSock.bind(ADDR)    # Bind the IP address and port number of the server. 
tcpSerSock.listen(5)     # The parameter of listen() defines the number of connections permitted at one time. Once the 
                         # connections are full, others will be rejected. 

video_dir.setup(busnum=busnum)
car_dir.setup(busnum=busnum)
motor.setup(busnum=busnum)     # Initialize the Raspberry Pi GPIO connected to the DC motor. 
video_dir.home_x_y()
car_dir.home()

while True:
	print ('Waiting for connection...')
	# Waiting for connection. Once receiving a connection, the function accept() returns a separate 
	# client socket for the subsequent communication. By default, the function accept() is a blocking 
	# one, which means it is suspended before the connection comes.
	tcpCliSock, addr = tcpSerSock.accept() 
	print ('...connected from :'+ str(addr))     # Print the IP address of the client connected with the server.

	while True:
		data = ''
		data = tcpCliSock.recv(BUFSIZ)    # Receive data sent from the client. 
		# Analyze the command received and control the car accordingly.
예제 #34
0
def mover():
    #Init variables for the control loop
    counter = 1
    bDriving = False
    bEndLoops = False

    sRobotDirection = Direction()

    rospy.init_node('mover', anonymous=True)

    rospy.Subscriber("motioncommand", String,
                     sRobotDirection.direction_callback)

    scooper_dir.setup(busnum=busnum)
    car_dir.setup(busnum=busnum)

    # Initialize the Raspberry Pi GPIO connected to the DC motor.
    motor.setup(busnum=busnum)

    #video_dir.home_x_y()
    #scooper_dir.servo_test ()
    car_dir.home()

    # Loop to wait for received commands.
    while (bEndLoops == False):

        # Loop to perform movement controls while input received.
        while (bEndLoops == False):

            data = sRobotDirection.directionMsg

            # Analyze the command received and control the car accordingly.
            #doAction (data, counter)
            #counter += 1
            #print counter

            if not data:
                break

            if data == ctrl_cmd[0]:
                print 'ELFF WILL DRIVE'
                counter += 1
                print counter

                try:
                    spd = 20
                    #print "Moving forward with speed!"
                    motor.forwardWithSpeed(spd)
                except:
                    print 'Error speed =' + str(spd)

            elif data == ctrl_cmd[1]:
                print 'ELFF WILL REVERSE'
                counter += 1
                print counter

                try:
                    spd = 20
                    #print "Moving backward with speed!"
                    motor.backwardWithSpeed(spd)
                except:
                    print 'Error speed =' + str(spd)

            elif data == ctrl_cmd[2]:
                print 'ELFF WILL GO LEFT'
                counter += 1
                car_dir.turn_left()

            elif data == ctrl_cmd[3]:
                print 'ELFF WILL GO RIGHT'
                counter += 1
                car_dir.turn_right()

            elif data == ctrl_cmd[4]:
                print 'ELFF WILL SCOOP'

                scooper_dir.home_x_y()
                scooper_dir.doScoop()
                scooper_dir.home_x_y()

            # Used with publisher.py only as a debug method.
            elif data == ctrl_cmd[5]:
                print 'ELFF WILL RESET POSITION'
                scooper_dir.home_x_y()
                car_dir.home()
                motor.ctrl(0)

                bEndLoops = True

            elif data == ctrl_cmd[6]:
                print 'ELFF WILL STOP'
                counter += 1
                print counter
                try:
                    spd = 0
                    motor.forwardWithSpeed(spd)
                except:
                    print 'Error speed =' + str(spd)

            else:
                print 'Waiting to receive a command...'