def move(direction):
# Choose the direction of the request
if direction == 'camleft':
video_dir.move_decrease_x()
elif direction == 'camright':
video_dir.move_increase_x()
elif direction == 'camup':
video_dir.move_increase_y()
elif direction == 'camdown':
video_dir.move_decrease_y()
elif direction == 'camhome':
video_dir.home_x_y()
elif direction == 'record':
subprocess.call(['motion'], shell=True)
elif direction == 'stoprecord':
subprocess.call(['./stop.sh'], shell=True)
elif direction == 'forward':
motor.forward()
elif direction == 'reverse':
motor.backward()
elif direction == 'left':
car_dir.turn_left()
elif direction == 'right':
car_dir.turn_right()
elif direction == 'stop':
motor.ctrl(0)
elif direction == 'home':
car_dir.home()
def forward(self, speed_percent, sec=0):
#ctrl(stasus(1 : on,0 : off), direction (1 : devant de base,-1 : derriere))
m.ctrl(1)
m.setSpeed(speed_percent)
time.sleep(sec)
if sec != 0:
m.ctrl(0)
def move(direction):
# Choose the direction of the request
if direction == 'camleft':
video_dir.move_decrease_x()
elif direction == 'camright':
video_dir.move_increase_x()
elif direction == 'camup':
video_dir.move_increase_y()
elif direction == 'camdown':
video_dir.move_decrease_y()
elif direction == 'camhome':
video_dir.home_x_y()
elif direction == 'record':
subprocess.call(['motion'], shell=True)
elif direction == 'stoprecord':
subprocess.call(['./stop.sh'], shell=True)
elif direction == 'forward':
motor.forward()
elif direction == 'reverse':
motor.backward()
elif direction == 'left':
car_dir.turn_left()
elif direction == 'right':
car_dir.turn_right()
elif direction == 'stop':
motor.ctrl(0)
elif direction == 'home':
car_dir.home()
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 pilonhunt():
print 'Looking for pilons...'
motor.ctrl(0)
car_dir.home()
pilons = []
#video_dir.maxright()
#time.sleep(2.0)
video_dir.tilt_rel(0)
r = 0.2
while r < 1:
# r = video_dir.pan(100)
video_dir.tilt_rel(0)
video_dir.pan_rel(r)
r = r + 0.2
time.sleep(1.0)
pilons.append(len(vision.detect()))
video_dir.tilt_rel(0.05)
time.sleep(0.5)
pilons.append(len(vision.detect()))
video_dir.home_x()
print pilons
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 init():
motor.setup()
motor.ctrl(1)
global pwm
pwm = servo.PWM()
def run_server(key):
#!/usr/bin/env python
import RPi.GPIO as GPIO
import video_dir
import car_dir
import motor
from time import ctime # Import necessary modules
ctrl_cmd = [
'forward', 'backward', 'left', 'right', 'stop', 'read cpu_temp',
'home', 'distance', 'x+', 'x-', 'y+', 'y-', 'xy_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:
try:
data = ''
data = tcpCliSock.recv(
BUFSIZ) # Receive data sent from the client.
print "\nEncrypted command recieved from client = ,", eval(
data)[2]
data = crypto.AES_decrypt(eval(data), key)
except:
print "INCOMPLETE PACKET ERROR - try to input the command again"
# Analyze the command received and control the car accordingly.
if not data:
break
if data == ctrl_cmd[0]:
print 'motor moving forward'
motor.forward()
elif data == ctrl_cmd[1]:
print 'recv backward cmd'
motor.backward()
elif data == ctrl_cmd[2]:
print 'recv left cmd'
car_dir.turn_left()
elif data == ctrl_cmd[3]:
print 'recv right cmd'
car_dir.turn_right()
elif data == ctrl_cmd[6]:
print 'recv home cmd'
car_dir.home()
elif data == ctrl_cmd[4]:
print 'recv stop cmd'
motor.ctrl(0)
elif data == ctrl_cmd[5]:
print 'read cpu temp...'
temp = cpu_temp.read()
tcpCliSock.send('[%s] %0.2f' % (ctime(), temp))
elif data == ctrl_cmd[8]:
print 'recv x+ cmd'
video_dir.move_increase_x()
elif data == ctrl_cmd[9]:
print 'recv x- cmd'
video_dir.move_decrease_x()
elif data == ctrl_cmd[10]:
print 'recv y+ cmd'
video_dir.move_increase_y()
elif data == ctrl_cmd[11]:
print 'recv y- cmd'
video_dir.move_decrease_y()
elif data == ctrl_cmd[12]:
print 'home_x_y'
video_dir.home_x_y()
elif data[0:5] == 'speed': # Change the speed
print data
numLen = len(data) - len('speed')
if numLen == 1 or numLen == 2 or numLen == 3:
tmp = data[-numLen:]
print 'tmp(str) = %s' % tmp
spd = int(tmp)
print 'spd(int) = %d' % spd
if spd < 24:
spd = 24
motor.setSpeed(spd)
elif data[0:5] == 'turn=': #Turning Angle
print 'data =', data
angle = data.split('=')[1]
try:
angle = int(angle)
car_dir.turn(angle)
except:
print 'Error: angle =', angle
elif data[0:8] == 'forward=':
print 'data =', data
spd = data[8:]
try:
spd = int(spd)
motor.forward(spd)
except:
print 'Error speed =', spd
elif data[0:9] == 'backward=':
print 'data =', data
spd = data.split('=')[1]
try:
spd = int(spd)
motor.backward(spd)
except:
print 'ERROR, speed =', spd
else:
#print 'Command Error! Cannot recognize command: ' + data
print "COMMAND ERROR - Unable to interpret recieved command"
tcpSerSock.close()
def backward(self, speed_percent, sec=0):
m.ctrl(1, -1)
m.setSpeed(speed_percent)
time.sleep(sec)
if sec != 0:
m.ctrl(0)
def run(self):
while True:
print 'Waiting for connection...'
tcpCliSock, addr = tcpSerSock.accept()
print '...connected from :', addr
while True:
data = ''
data = tcpCliSock.recv(BUFSIZ)
if not data:
break
if data == ctrl_cmd[0]:
print 'motor moving forward'
motor.forward()
elif data == ctrl_cmd[1]:
print 'recv backward cmd'
motor.backward()
elif data == ctrl_cmd[2]:
print 'recv left cmd'
car_dir.turn_left()
elif data == ctrl_cmd[3]:
print 'recv right cmd'
car_dir.turn_right()
elif data == ctrl_cmd[6]:
print 'recv home cmd'
car_dir.home()
elif data == ctrl_cmd[4]:
print 'recv stop cmd'
motor.ctrl(0)
elif data == ctrl_cmd[5]:
print 'read cpu temp...'
temp = cpu_temp.read()
tcpCliSock.send('[%s] %0.2f' % (ctime(), temp))
elif data == ctrl_cmd[8]:
print 'recv x+ cmd'
video_dir.move_increase_x()
elif data == ctrl_cmd[9]:
print 'recv x- cmd'
video_dir.move_decrease_x()
elif data == ctrl_cmd[10]:
print 'recv y+ cmd'
video_dir.move_increase_y()
elif data == ctrl_cmd[11]:
print 'recv y- cmd'
video_dir.move_decrease_y()
elif data == ctrl_cmd[12]:
print 'home_x_y'
video_dir.home_x_y()
elif data[0:5] == 'speed':
print data
numLen = len(data) - len('speed')
if numLen == 1 or numLen == 2 or numLen == 3:
tmp = data[-numLen:]
print 'tmp(str) = %s' % tmp
spd = int(tmp)
print 'spd(int) = %d' % spd
if spd < 24:
spd = 24
motor.setSpeed(spd)
elif data[0:5] == 'turn=':
print 'data =', data
angle = data.split('=')[1]
try:
angle = int(angle)
car_dir.turn(angle)
except:
print 'Error: angle =', angle
elif data[0:8] == 'forward=':
print 'data =', data
spd = data[8:]
try:
spd = int(spd)
motor.forward(spd)
except:
print 'Error speed =', spd
elif data[0:9] == 'backward=':
print 'data =', data
spd = data.split('=')[1]
try:
spd = int(spd)
motor.backward(spd)
except:
print 'ERROR, speed =', spd
else:
print 'Command Error! Cannot recognize command: ' + data
tcpSerSock.close()
def stop(self):
m.ctrl(0)
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 = tcpCliSock.recv(BUFSIZ) # Receive data sent from the client. # Analyze the command received and control the car accordingly. if not data: break if data == ctrl_cmd[0]: print 'motor moving forward' motor.ctrl(1, 1) elif data == ctrl_cmd[1]: print 'recv backward cmd' motor.ctrl(1, -1) elif data == ctrl_cmd[2]: print 'recv left cmd' car_dir.turn_left() elif data == ctrl_cmd[3]: print 'recv right cmd' car_dir.turn_right() elif data == ctrl_cmd[6]: print 'recv home cmd' car_dir.home() elif data == ctrl_cmd[4]: print 'recv stop cmd' motor.ctrl(0)
def stop(self):
motor.ctrl(0)
return self.ack()
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...'
def motor_stop(request):
motor.ctrl(0)
return HttpResponse("Motor stop")
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 = tcpCliSock.recv(BUFSIZ) # Receive data sent from the client.
# Analyze the command received and control the car accordingly.
if not data:
break
if data == ctrl_cmd[0]:
print 'motor moving forward'
motor.ctrl(1, 1)
elif data == ctrl_cmd[1]:
print 'recv backward cmd'
motor.ctrl(1, -1)
elif data == ctrl_cmd[2]:
print 'recv left cmd'
car_dir.turn_left()
elif data == ctrl_cmd[3]:
print 'recv right cmd'
car_dir.turn_right()
elif data == ctrl_cmd[6]:
print 'recv home cmd'
car_dir.home()
elif data == ctrl_cmd[4]:
print 'recv stop cmd'
motor.ctrl(0)
def process_request(data):
# Analyze the command received and control the car accordingly.
if not data:
return "cmd not understood"
if data == ctrl_cmd[0]:
print('motor moving forward')
motor.forward()
elif data == ctrl_cmd[1]:
print('recv backward cmd')
motor.backward()
elif data == ctrl_cmd[2]:
print('recv left cmd')
car_dir.turn_left()
elif data == ctrl_cmd[3]:
print('recv right cmd')
car_dir.turn_right()
elif data == ctrl_cmd[6]:
print('recv home cmd')
car_dir.home()
elif data == ctrl_cmd[4]:
print('recv stop cmd')
motor.ctrl(0)
elif data == ctrl_cmd[5]:
print('read cpu temp...')
temp = cpu_temp.read()
tcpCliSock.send('[%s] %0.2f' % (ctime(), temp))
elif data == ctrl_cmd[8]:
print('recv x+ cmd')
video_dir.move_increase_x()
elif data == ctrl_cmd[9]:
print('recv x- cmd')
video_dir.move_decrease_x()
elif data == ctrl_cmd[10]:
print('recv y+ cmd')
video_dir.move_increase_y()
elif data == ctrl_cmd[11]:
print('recv y- cmd')
video_dir.move_decrease_y()
elif data == ctrl_cmd[12]:
print('home_x_y')
video_dir.home_x_y()
elif data[0:5] == 'speed': # Change the speed
print(data)
numLen = len(data) - len('speed')
if numLen == 1 or numLen == 2 or numLen == 3:
tmp = data[-numLen:]
print('tmp(str) = %s' % tmp)
spd = int(tmp)
print('spd(int) = %d' % spd)
if spd < 24:
spd = 24
motor.setSpeed(spd)
elif data[0:5] == 'turn=': # Turning Angle
print('data =', data)
angle = data.split('=')[1]
try:
angle = int(angle)
car_dir.turn(angle)
except:
print('Error: angle =', angle)
elif data[0:8] == 'forward=':
print('data =', data)
spd = data[8:]
try:
spd = int(spd)
motor.forward(spd)
except:
print('Error speed =', spd)
elif data[0:9] == 'backward=':
print('data =', data)
spd = data.split('=')[1]
try:
spd = int(spd)
motor.backward(spd)
except:
print('ERROR, speed =', spd)
else:
print('Command Error! Cannot recognize command: ' + data)
def SrvCommand(cmd):
result = 'ERROR'
num = 0
for str in cmd.split(' '):
num += 1
#print "split number {0}".format(num)
if num < 2:
print "format dont match \n"
return result
header = cmd.split(' ')[0]
type = cmd.split(' ')[1]
if header == "motor":
if num < 3:
print "format dont match \n"
return result
para = cmd.split(' ')[2]
if type == 'read':
result = motor.state()
elif type == 'write':
result = motor.ctrl(para)
elif type == 'getspeed':
result = motor.getSpeed()
elif type == 'setspeed':
result = motor.setSpeed(para)
else:
print "motor: ctrl type error"
elif header == "car":
if type == 'go':
result = car.ctrl("right", 1)
result = car.ctrl("left", 1)
elif type == 'right':
result = car.ctrl("right", 0)
result = car.ctrl("left", 1)
elif type == 'left':
result = car.ctrl("right", 1)
result = car.ctrl("left", 0)
elif type == 'back':
result = car.ctrl("right", -1)
result = car.ctrl("left", -1)
elif type == 'bright':
result = car.ctrl("right", 0)
result = car.ctrl("left", -1)
elif type == 'bleft':
result = car.ctrl("right", -1)
result = car.ctrl("left", 0)
elif type == 'stop':
result = car.ctrl("right", 0)
result = car.ctrl("left", 0)
else:
print "car: ctrl type error"
elif header == "zoom":
if type == 'in':
#pos =str(int( lens.getPos("focus")) + 100)
#print "pos",pos
result = lens.ctrl("zoom", 200)
elif type == 'out':
pos = lens.getPos("zoom")
print "pos", pos
if int(pos) > 10:
result = lens.ctrl("zoom", -200)
else:
print "overrange"
else:
print "zoom: ctrl type error"
elif header == "focus":
if type == 'near':
result = lens.ctrl("focus", 10)
elif type == 'far':
result = lens.ctrl("focus", -10)
else:
print "focus: ctrl type error"
else:
print "header error"
return result
def CarController(socket):
while True:
print 'Waiting for connection to car controller service...'
# 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.
clientSocket, addr = socket.accept()
print '...connected to car controller service:', addr # Print the IP address of the client connected with the server.
lastCmd = ''
while True:
msgs = ''
recdata = clientSocket.recv(BUFSIZ)
# Receive data sent from the client.
# Analyze the command received and control the car accordingly.
msgs = recdata.split(';')
#print("Received", len(msgs), "new messages")
for data in msgs:
if not data:
break
if lastCmd == data:
print("Last Command:", lastCmd, "Current Data:", data,
"Ignoring")
break
if data == ctrl_cmd[0]:
print 'motor moving forward'
motor.forward()
elif data == ctrl_cmd[1]:
print 'recv backward cmd'
motor.backward()
elif data == ctrl_cmd[2]:
print 'recv left cmd'
car_dir.turn_left()
elif data == ctrl_cmd[3]:
print 'recv right cmd'
car_dir.turn_right()
elif data == ctrl_cmd[6]:
print 'recv home cmd'
car_dir.home()
elif data == ctrl_cmd[4]:
print 'recv stop cmd'
motor.ctrl(0)
elif data == ctrl_cmd[5]:
print 'read cpu temp...'
temp = cpu_temp.read()
tcpCliSock.send('[%s] %0.2f' % (ctime(), temp))
elif data[0:5] == 'speed':
#print data
numLen = len(data) - len('speed')
if numLen == 1 or numLen == 2 or numLen == 3:
tmp = data[-numLen:]
#print 'tmp(str) = %s' % tmp
spd = int(tmp)
#print 'spd(int) = %d' % spd
if spd < 24:
spd = 24
motor.setSpeed(spd)
elif data[0:8] == 'network=':
print 'network =', data
spd = data.split('=')[1]
try:
spd = int(spd)
os.system('sudo tc qdisc del dev wlan0 root')
os.system(
'sudo tc qdisc add dev wlan0 root netem delay {0}ms'
.format(spd))
except:
print 'ERROR , speed =', spd
elif data[0:7] == 'offset=':
print 'offset called, data = ', data
offset = int(data[7:]) + 28
car_dir.calibrate(offset)
elif data[0:8] == 'forward=':
#print 'data =', data
spd = data.split('=')[1]
try:
spd = int(spd)
motor.setSpeed(spd)
motor.forward()
except:
print 'Error speed =', spd
elif data[0:9] == 'backward=':
#print 'data =', data
spd = data.split('=')[1]
try:
spd = int(spd)
motor.setSpeed(spd)
motor.backward()
except:
print 'ERROR , speed =', spd
else:
print 'Command Error! Cannot recognize command: ' + data
def server(
value, collision
): #get value as Direction and collision as Collision detection from Shared memory
pre_dir = 'home'
x = 0
flag = 0
while True:
sys.stdout.flush()
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.
print data
if not data:
break
if x == 1:
if flag < 5:
flag = flag + 1
continue #if there is any collision, Do not receive data from client.If so, Get stacked!
x = 0 #after refusing data from client, start receiving
flag = 0
if data == ctrl_cmd[0]: #If Client send message "forward"
if collision.value == 1: #if there is obstacle in front of iCar
motor.ctrl(0) #stop
else:
motor.forward() #Run the motors to forward
elif data == ctrl_cmd[1]: #If Client send message "backward"
motor.backward()
elif data == ctrl_cmd[2]: #If Client send message "left"
car_dir.turn_left() #change car direction to Left
elif data == ctrl_cmd[3]: #If Client send message "right"
car_dir.turn_right() #change car direction to Right
elif data == ctrl_cmd[6]: #If Client send message "home"
car_dir.home() #Set car direction to center
elif data == ctrl_cmd[4]: #if Client send message "stop"
motor.ctrl(0) #Stop Motor running
elif data == ctrl_cmd[
5]: #If Client click auto button, send message "auto"
#auto drive
motor.setSpeed(44) #Set motor speed with optimized speed 44
temp = value.value #get Value from jeongwook.py that is information of Car Direction
if collision.value != 0: #If there is Collision
print 'Collision detected'
if collision.value == 1: #Collision in front of iCar
print "Obstacle In Front"
motor.collision_auto(
) #collision_auto function let iCar move backward
car_dir.turn_right() #to avoid collision, turn right
motor.forward_auto(
) #move only for 0.8second (forward_auto let iCar move for 0.2second)
motor.forward_auto()
motor.forward_auto()
motor.forward_auto()
car_dir.home()
motor.forward_auto()
motor.forward_auto()
pre_dir = 'left' #if iCar cannot detect any lane, it SHOULD BE on left side, so makes iCar go left after avoiding
elif collision.value == 2: #Collision on Left side
print "Obstacle is on Left"
motor.collision_auto()
car_dir.turn_right() #to avoid collision, turn right
motor.forward_auto()
motor.forward_auto()
motor.forward_auto()
motor.forward_auto()
car_dir.home()
motor.forward_auto()
motor.forward_auto()
pre_dir = 'left'
elif collision.value == 3: #go left
print "Obstacle is on Right"
motor.collision_auto()
car_dir.turn_left() #to avoid collision, turn left
motor.forward_auto()
motor.forward_auto()
motor.forward_auto()
motor.forward_auto()
car_dir.home()
motor.forward_auto()
motor.forward_auto()
pre_dir = 'right' #if iCar cannot detect any lane, it SHOULD BE on right side, so makes iCar go right after avoiding
collision.value = 0
x = 1 #set x = 1 to Not receiving data from client for a moment
elif temp == 1: #iCar is on Lane, Go with center direction
print 'Lane is on my way'
car_dir.home()
motor.forward_auto() #move iCar for 0.2second with 44speed
#because of camera delay
elif temp == 2: #lane is located on left side
print 'Lane is on left side'
car_dir.turn_left()
motor.forward_auto()
pre_dir = 'left'
elif temp == 3: #lane is located on right side
print 'Lane is on right side'
car_dir.turn_right()
motor.setSpeed(44)
motor.forward_auto()
pre_dir = 'right'
else:
if pre_dir == 'right': #when No detection but predict that lane is on right side
print 'cannot find but go right'
car_dir.turn_right()
motor.setSpeed(44)
motor.forward_auto()
elif pre_dir == 'left': #when No detection but predict that lane is on left side
print 'cannot find but go left'
car_dir.turn_left()
motor.forward_auto()
else:
print 'Cannot find a Lane' #No detection with no prediction
car_dir.home() #set center direction and stop
motor.backward()
time.sleep(0.6)
motor.ctrl(0)
time.sleep(1)
elif data == ctrl_cmd[7]: #move camera right
video_dir.move_increase_x()
elif data == ctrl_cmd[8]: #move camera left
video_dir.move_decrease_x()
elif data == ctrl_cmd[9]: #move camera up
video_dir.move_increase_y()
elif data == ctrl_cmd[10]: #move camera down
video_dir.move_decrease_y()
elif data[0:5] == 'speed': # Change the speed
print data
numLen = len(data) - len('speed')
if numLen == 1 or numLen == 2 or numLen == 3:
tmp = data[-numLen:]
spd = int(tmp)
if spd < 24:
spd = 24
motor.setSpeed(spd)
elif data[
0:
7] == 'leftPWM': #If Client send message like ~PWM, it is for initialization or servo motor that control Car direction.
numLen = len(data) - len('leftPWM')
pwm = data[-numLen:]
leftpwm = int(pwm)
car_dir.change_left(leftpwm)
elif data[0:7] == 'homePWM':
numLen = len(data) - len('homePWM')
pwm = data[-numLen:]
homepwm = int(pwm)
car_dir.change_home(homepwm)
elif data[0:8] == 'rightPWM':
numLen = len(data) - len('rightPWM')
pwm = data[-numLen:]
rightpwm = int(pwm)
car_dir.change_right(rightpwm)
else:
print 'Command Error! Cannot recognize command: ' + data
tcpSerSock.close()
def stop_driving(self):
sfdriving.ctrl(0)
return True
motor.forward()
elif data == ctrl_cmd[1]:
print 'recv backward cmd'
motor.backward()
elif data == ctrl_cmd[2]:
print 'recv left cmd'
car_dir.turn_left()
elif data == ctrl_cmd[3]:
print 'recv right cmd'
car_dir.turn_right()
elif data == ctrl_cmd[6]:
print 'recv home cmd'
car_dir.home()
elif data == ctrl_cmd[4]:
print 'recv stop cmd'
motor.ctrl(0)
elif data == ctrl_cmd[5]:
print 'read cpu temp...'
temp = cpu_temp.read()
tcpCliSock.send('[%s] %0.2f' % (ctime(), temp))
elif data == ctrl_cmd[8]:
print 'recv x+ cmd'
video_dir.move_increase_x()
elif data == ctrl_cmd[9]:
print 'recv x- cmd'
video_dir.move_decrease_x()
elif data == ctrl_cmd[10]:
print 'recv y+ cmd'
video_dir.move_increase_y()
elif data == ctrl_cmd[11]:
print 'recv y- cmd'
if __name__ == '__main__':
# Obstacle Detection
sleep_time = 0.3
GPIO.setmode(GPIO.BOARD)
# Settings initialization
busnum = 1 # Edit busnum to 0, if you uses Raspberry Pi 1 or 0
video_dir.setup(busnum=busnum)
car_dir.setup(busnum=busnum)
motor.setup(busnum=busnum) # Initialize the Raspberry Pi GPIO connected to the DC motor.
motor.setSpeed(50)
video_dir.home_x_y()
car_dir.home()
motor.ctrl(0)
# Load classifier
model_path = os.path.join('..', '..', 'models', 'model_aug_bright.h5')
model = load_model(model_path)
ctrl_cmd = ['forward',
'backward',
'left',
'right',
'stop',
'read cpu_temp',
'home',
'distance',
'x+',
def __init__(self):
threading.Thread.__init__(self)
self.model = load_model(os.path.join('..', '..', 'models', 'sign_classification')
def run(self):
while True:
if output_full is not None:
global stop_detected
global lstop_detected
stop_detected = self.model.predict(output_full)
if stop_detected:
sleep(1)
lstop_detected = True
stop_detected = False
sleep(1)
threadss = SignsThread()
threadss.start()
time.sleep(2)
ultrason = ultrasonic.UltrasonicAsync(sleep_time)
ultrason.start()
obstacleExist = False
motor.forward()
while True:
data = ''
# Check of obstacles
if ultrason.dist < 20:
motor.ctrl(0)
obstacleExist = True
while obstacleExist:
time.sleep(sleep_time)
if ultrason.dist < 20:
obstacleExist = False
motor.forward()
# Take input from camera
output_full2 = np.empty(480 * 640 * 3, dtype=np.uint8)
cam.capture(output_full2, 'bgr', use_video_port=True)
output_full = output_full2.reshape((480, 640, 3))
crop_idx = int(output_full.shape[0] / 2)
output = output_full[crop_idx:, :]
img_detection = imresize(output, (120, 160))
pred = model.predict(img_detection.reshape((1, 120, 160, 3)))
speed_pred = pred[1][0][0]
motor.setSpeed(int(speed_pred * 62 + 40))
pred = pred[0][0][0]
i += 1
data = ctrl_cmd[0]
if stop_detected:
data = ctrl_cmd[4]
def on_message(ws, data):
print data
# Analyze the command received and control the car accordingly.
#if not data:
if data == ctrl_cmd[0]:
print 'motor moving forward'
motor.forward()
elif data == ctrl_cmd[1]:
print 'recv backward cmd'
motor.backward()
elif data == ctrl_cmd[2]:
print 'recv left cmd'
car_dir.turn_left()
elif data == ctrl_cmd[3]:
print 'recv right cmd'
car_dir.turn_right()
elif data == ctrl_cmd[6]:
print 'recv home cmd'
car_dir.home()
elif data == ctrl_cmd[4]:
print 'recv stop cmd'
motor.ctrl(0)
elif data == ctrl_cmd[5]:
print 'read cpu temp...'
temp = cpu_temp.read()
tcpCliSock.send('[%s] %0.2f' % (ctime(), temp))
elif data == ctrl_cmd[8]:
print 'recv x+ cmd'
video_dir.move_increase_x()
elif data == ctrl_cmd[9]:
print 'recv x- cmd'
video_dir.move_decrease_x()
elif data == ctrl_cmd[10]:
print 'recv y+ cmd'
video_dir.move_increase_y()
elif data == ctrl_cmd[11]:
print 'recv y- cmd'
video_dir.move_decrease_y()
elif data == ctrl_cmd[12]:
print 'home_x_y'
video_dir.home_x_y()
elif data[0:12] == ctrl_cmd[13]: # Change the speed
print data
#numLen = len(data) - len('speed')
#if numLen == 1 or numLen == 2 or numLen == 3:
# tmp = data[-numLen:]
# print 'tmp(str) = %s' % tmp
# spd = int(tmp)
# print 'spd(int) = %d' % spd
# if spd < 24:
# spd = 24
motor.setSpeed(30)
elif data[0:5] == 'turn=': #Turning Angle
print 'data =', data
angle = data.split('=')[1]
try:
angle = int(angle)
car_dir.turn(angle)
except:
print 'Error: angle =', angle
elif data[0:8] == 'forward=':
print 'data =', data
spd = 30
try:
spd = int(spd)
motor.forward(spd)
except:
print 'Error speed =', spd
elif data[0:9] == 'backward=':
print 'data =', data
spd = data.split('=')[1]
try:
spd = int(spd)
motor.backward(spd)
except:
print 'ERROR, speed =', spd
else:
print 'Command Error! Cannot recognize command: ' + data
motor.forward()
elif data == ctrl_cmd[1]:
motor.backward()
elif data == ctrl_cmd[2]:
car_dir.turn_left()
elif data == ctrl_cmd[3]:
car_dir.turn_right()
elif data == ctrl_cmd[6]:
car_dir.home()
elif data == ctrl_cmd[4]:
motor.ctrl(0)
elif data == ctrl_cmd[5]:
temp = cpu_temp.read()
tcpCliSock.send('[%s] %0.2f' % (ctime(), temp))
elif data == ctrl_cmd[8]:
video_dir.move_increase_x()
elif data == ctrl_cmd[9]:
video_dir.move_decrease_x()
elif data == ctrl_cmd[10]:
video_dir.move_increase_y()
elif data == ctrl_cmd[11]:
def motor_stop(self):
motor.ctrl(0)
