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)
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)
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()
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
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")
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)
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
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()
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()
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
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()
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)
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)
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()
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()
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)
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")
def setup(): #initialization
motor.setup()
led.setup()
def init():
motor.setup()
servo_test.setup()
motor.setSpeed(0)
servo_test.pwm.write(0,0,500)
def setup():
GPIO.setwarnings(True)
try:
motor.setup()
except:
pass
def setup():
motor.setup()
led.setup()
turn.turn_middle()
led.green()
def setup():
motor.setup()
def setup(): #initialization
motor.setup()
turn.ahead()
findline.setup()
def setup():
'''initialization'''
#??? seperate function seems unecessary
motor.setup()
turn.ahead()
findline.setup()
def __init__(self):
m.setup() #Have to be done
cd.setup()
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
def init():
motor.setup()
motor.ctrl(1)
global pwm
pwm = servo.PWM()
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()
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)
def setup():
motor.setup()
led.setup()
breathingLed.setup()
activeBuzzer.setup()
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.
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.
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...'
