def go_Backward(speed, running_time):
global backward0, backward1
motor.setSpeed(speed)
motor.motor0(backward0)
motor.motor1(backward1)
time.sleep(running_time)
motor.stop()
def go_Forward(speed, running_time):
global forward0, forward1
motor.setSpeed(speed)
motor.motor0(forward0)
motor.motor1(forward1)
time.sleep(running_time)
motor.stop()
def main():
try:
setup()
except KeyboardInterrupt:
exit()
killer = GracefulShutdown()
client_name = 'car0'
# Begin MQTT connection
print("Creating MQTT client data...")
client = mqtt.Client(client_id=client_name, clean_session=True, userdata=None, protocol=mqtt.MQTTv31,
transport='tcp')
client.on_connect = on_connect
client.on_message = on_message
print('Connecting to host...')
client.connect('192.168.1.169', 1883)
print('Connected...')
client.subscribe(light_topic)
motor.setSpeed(50)
print('Waiting for green light...')
rc = 0
while rc == 0:
if killer.shutdown:
client.disconnect()
client.loop_stop()
break
rc = client.loop()
print('Shutting down...')
sys.exit()
def init(speed):
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)
motor.setSpeed(speed)
def straight():
global distance_r, distance_c, distance_l
home()
setSpeed(STRAIGHT_SPEED, STRAIGHT_SPEED)
move('up')
print("Keep moving forward.")
while distance_r / distance_l > 1.0 and distance_r < THRESHOLD:
move('right65')
distance = setupSensors(TRIG_PIN_L, ECHO_PIN_L, TRIG_PIN_C, ECHO_PIN_C,
TRIG_PIN_R, ECHO_PIN_R)
distance_l = distance[0]
distance_c = distance[1]
distance_r = distance[2]
print(distance)
print('Calibrate right.')
while distance_r / distance_l < 1.0 and distance_l < THRESHOLD:
move('left65')
distance = setupSensors(TRIG_PIN_L, ECHO_PIN_L, TRIG_PIN_C, ECHO_PIN_C,
TRIG_PIN_R, ECHO_PIN_R)
distance_l = distance[0]
distance_c = distance[1]
distance_r = distance[2]
print(distance)
print('Calibrate left.')
return
def calibrateReverse(sec):
global distance_r, distance_c, distance_l, distance_old_r, distance_old_c, distance_old_l
stop()
home()
setSpeed(STRAIGHT_SPEED, STRAIGHT_SPEED)
move('down')
print('reverse')
distance = setupSensors(TRIG_PIN_L, ECHO_PIN_L, TRIG_PIN_C, ECHO_PIN_C,
TRIG_PIN_R, ECHO_PIN_R)
distance_l = distance[0]
distance_c = distance[1]
distance_r = distance[2]
start = time.time()
end = time.time()
while end - start < sec:
if distance_r / distance_l > 1.0:
move('down_right65')
distance = setupSensors(TRIG_PIN_L, ECHO_PIN_L, TRIG_PIN_C,
ECHO_PIN_C, TRIG_PIN_R, ECHO_PIN_R)
distance_l = distance[0]
distance_c = distance[1]
distance_r = distance[2]
print(distance)
print('Calibrate left.')
elif distance_r / distance_l < 1.0:
move('down_left65')
distance = setupSensors(TRIG_PIN_L, ECHO_PIN_L, TRIG_PIN_C,
ECHO_PIN_C, TRIG_PIN_R, ECHO_PIN_R)
distance_l = distance[0]
distance_c = distance[1]
distance_r = distance[2]
print(distance)
print('Calibrate right.')
end = time.time()
return
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 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 on_message(client, userdata, msg):
print('Msg received...')
print(msg.topic + ' ' + str(msg.payload))
print(msg.payload.decode())
color_str = str(msg.payload.decode())
if color_str == 'GREEN':
motor.setSpeed(50)
motor.start()
def motor_set_speed(request, speed): speed = int(speed) if speed < 24: speed = 24 if speed > 100: speed = 100 motor.setSpeed(speed) text = "Speed set to", speed return HttpResponse(text)
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
# 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
#--------Motor calibration----------
if data == 'motor_run':
print 'motor moving forward'
motor.setSpeed(50)
motor.motor0(forward0)
motor.motor1(forward1)
elif data[0:9] == 'leftmotor':
forward0 = data[9:]
motor.motor0(forward0)
elif data[0:10] == 'rightmotor':
forward1 = data[10:]
motor.motor1(forward1)
elif data == 'motor_stop':
print 'motor stop'
motor.stop()
#---------------------------------
#-------Turing calibration------
elif data[0:7] == 'offset=':
offset = int(data[7:])
car_dir.calibrate(offset)
#--------------------------------
#----------Mount calibration---------
elif data[0:8] == 'offsetx=':
offset_x = int(data[8:])
print 'Mount offset x', offset_x
video_dir.calibrate(offset_x, offset_y)
elif data[0:8] == 'offsety=':
offset_y = int(data[8:])
print 'Mount offset y', offset_y
video_dir.calibrate(offset_x, offset_y)
#----------------------------------------
else:
print 'cmd error !'
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 # 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 #--------Motor calibration---------- if data == 'motor_run': print 'motor moving forward' motor.setSpeed(50) motor.motor0(forward0) motor.motor1(forward1) elif data[0:9] == 'leftmotor': forward0 = data[9:] motor.motor0(forward0) elif data[0:10] == 'rightmotor': forward1 = data[10:] motor.motor1(forward1) elif data == 'motor_stop': print 'motor stop' motor.stop() #--------------------------------- #-------Turing calibration------ elif data[0:7] == 'offset=': offset = int(data[7:]) car_dir.calibrate(offset) #-------------------------------- #----------Mount calibration--------- elif data[0:8] == 'offsetx=': offset_x = int(data[8:]) print 'Mount offset x', offset_x video_dir.calibrate(offset_x, offset_y) elif data[0:8] == 'offsety=': offset_y = int(data[8:]) print 'Mount offset y', offset_y video_dir.calibrate(offset_x, offset_y) #---------------------------------------- else: print 'cmd error !'
def set_actuator_from_cmdvel(self, message):
"""
Get a Twist message from cmd_vel, assuming max inmput is 1
"""
#-- Save the time
self._last_time_cmd_rcv = time.time()
#-- Convert vel into servo values
self.actuators['throttle'].get_value_out(message.linear.x)
self.actuators['steering'].get_value_out(message.angular.z)
rospy.loginfo("Got a command v = %2.1f s=%2.1f" %
(message.linear.x, message.angular.z))
self.send_servo_msg()
motor.setSpeed(message.linear.x)
def do_POST(self):
req = urlparse(self.path)
if req.path == re.search('/turn', self.path):
car_dir.turn(req.body.angle)
elif req.path == re.search('/motors', self.path):
if req.body.forward == True:
motor.forward()
elif req.body.backward == True:
motor.forward()
elif req.body.stop == True:
motor.forward()
elif req.path == re.search('/speed', self.path):
motor.setSpeed(req.body.speed)
self.send_response(200)
self.send_header('Content-type', 'application/json')
self.end_headers()
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 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
# 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
#--------Motor calibration----------
if data == 'motor_run':
print 'motor moving forward'
motor.setSpeed(50)
motor.motor0(forward0)
motor.motor1(forward1)
elif data[0:9] == 'leftmotor':
forward0 = data[9:]
motor.motor0(forward0)
def startMovement():
global distance_r, distance_c, distance_l, distance_old_r, distance_old_c, distance_old_l, data, client_socket
setSpeed(STRAIGHT_SPEED, STRAIGHT_SPEED)
move('up')
MAX_DIST = 350
while data != 'BACK':
client_socket.send('0')
# try:
# data = client_socket.recv(1024)
# except socket.timeout:
# print(data)
# data = client_socket.recv(1024)
# print(data)
distance_old_c = distance_c
distance_old_l = distance_l
distance_old_r = distance_r
distance = setupSensors(TRIG_PIN_L, ECHO_PIN_L, TRIG_PIN_C, ECHO_PIN_C,
TRIG_PIN_R, ECHO_PIN_R)
distance_l = distance[0]
distance_c = distance[1]
distance_r = distance[2]
print(distance)
if distance_l < THRESHOLD and distance_c < THRESHOLD_C and distance_r < THRESHOLD and distance_old_l < THRESHOLD and distance_old_c < THRESHOLD_C and distance_old_r < THRESHOLD:
# Dead end/destination arrived
recentre()
setSpeed(STRAIGHT_SPEED, STRAIGHT_SPEED)
move('up')
if distance_l / distance_old_l > LW_LIMIT and distance_l / distance_old_l < UP_LIMIT and distance_c / distance_old_c > LW_LIMIT and distance_c / distance_old_c < UP_LIMIT and distance_r / distance_old_r > LW_LIMIT and distance_r / distance_old_r < UP_LIMIT:
calibrateReverse(0.8)
# print("You have arrived at your destination.")
# break
elif distance_l < THRESHOLD and distance_c >= THRESHOLD_C and distance_r < THRESHOLD and distance_old_l < THRESHOLD and distance_old_c >= THRESHOLD_C and distance_old_r < THRESHOLD:
# Can only go straight
straight()
elif distance_l < THRESHOLD and distance_c < THRESHOLD_C and distance_r >= THRESHOLD and distance_old_l < THRESHOLD and distance_old_c < THRESHOLD_C and distance_old_r >= THRESHOLD:
# Only right turn available
client_socket.send('1')
move('down')
time.sleep(0.3)
setSpeed(TURN_SPEED, TURN_SPEED)
turning('right')
time.sleep(1.0)
while distance_r > THRESHOLD - 10 or distance_old_r > THRESHOLD - 10:
setSpeed(TURN_SPEED - 5, TURN_SPEED - 5)
turning('right')
if distance_l < THRESHOLD and distance_c >= THRESHOLD and distance_r >= THRESHOLD and distance_old_l < THRESHOLD and distance_old_c >= THRESHOLD and distance_old_r >= THRESHOLD:
print('junction')
# move('right')
# time.sleep(0.3)
# home()
setSpeed(STRAIGHT_SPEED, STRAIGHT_SPEED)
move('up')
time.sleep(0.1)
break
# if distance_l < THRESHOLD and distance_c < THRESHOLD_C and distance_r >= THRESHOLD_OUT and distance_old_l < THRESHOLD and distance_old_c < THRESHOLD and distance_old_r >= THRESHOLD_OUT:
# break
if distance_l >= THRESHOLD_OUT and distance_c >= THRESHOLD_OUT and distance_r >= THRESHOLD_OUT and distance_old_l >= THRESHOLD_OUT and distance_old_c >= THRESHOLD_OUT and distance_old_r >= THRESHOLD_OUT:
break
elif distance_l >= THRESHOLD and distance_c < THRESHOLD_C and distance_r < THRESHOLD and distance_old_l >= THRESHOLD and distance_old_c < THRESHOLD_C and distance_old_r < THRESHOLD:
# Only left turn available
client_socket.send('2')
move('down')
time.sleep(0.3)
setSpeed(TURN_SPEED, TURN_SPEED)
turning('left')
time.sleep(0.5)
while distance_l > THRESHOLD - 10 or distance_old_l > THRESHOLD - 10:
setSpeed(TURN_SPEED + 5, TURN_SPEED + 5)
turning('left')
# if distance_l >= THRESHOLD_OUT and distance_c >= THRESHOLD_OUT and distance_r < THRESHOLD_OUT and distance_old_l >= THRESHOLD_OUT and distance_old_c >= THRESHOLD_OUT and distance_old_r < THRESHOLD_OUT:
# move('left')
# time.sleep(0.2)
# home()
# setSpeed(STRAIGHT_SPEED,STRAIGHT_SPEED)
# move('up')
# time.sleep(0.5)
# break
if distance_l >= THRESHOLD_OUT and distance_c >= THRESHOLD_OUT and distance_r >= THRESHOLD_OUT and distance_old_l >= THRESHOLD_OUT and distance_old_c >= THRESHOLD_OUT and distance_old_r >= THRESHOLD_OUT:
break
elif distance_l < THRESHOLD and distance_c >= THRESHOLD_C and distance_r >= THRESHOLD and distance_old_l < THRESHOLD and distance_old_c >= THRESHOLD_C and distance_old_r >= THRESHOLD:
# Junction on front and right
setSpeed(STRAIGHT_SPEED, STRAIGHT_SPEED)
move('up')
time.sleep(0.8)
junction()
elif distance_l >= THRESHOLD and distance_c < THRESHOLD_C and distance_r >= THRESHOLD and distance_old_l >= THRESHOLD and distance_old_c < THRESHOLD_C and distance_old_r >= THRESHOLD:
# Junction on left and right
setSpeed(STRAIGHT_SPEED, STRAIGHT_SPEED)
move('down')
time.sleep(0.5)
junction()
elif distance_l >= THRESHOLD and distance_c >= THRESHOLD_C and distance_r < THRESHOLD and distance_old_l >= THRESHOLD and distance_old_c >= THRESHOLD_C and distance_old_r < THRESHOLD:
# Junction on front and left
setSpeed(STRAIGHT_SPEED, STRAIGHT_SPEED)
move('up')
time.sleep(0.8)
junction()
elif distance_l >= THRESHOLD and distance_c >= THRESHOLD_C and distance_r >= THRESHOLD and distance_old_l >= THRESHOLD and distance_old_c >= THRESHOLD_C and distance_old_r >= THRESHOLD and sign_detect == 1:
print('Open Space')
client_socket.send('3')
recentre()
return
else:
recentre()
setSpeed(STRAIGHT_SPEED - 5, STRAIGHT_SPEED - 5)
move('up')
if distance_l / distance_old_l > LW_LIMIT and distance_l / distance_old_l < UP_LIMIT and distance_c / distance_old_c > LW_LIMIT and distance_c / distance_old_c < UP_LIMIT and distance_r / distance_old_r > LW_LIMIT and distance_r / distance_old_r < UP_LIMIT:
calibrateReverse(0.8)
print('waiting')
def setSpeed(self):
motor.setSpeed(self.slider_setgeschw.value())
def final(speed):
return jsonify({"speed": setSpeed(finalSpeed=speed)})
import motor motor.setSpeed(0)
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:
def calibrate_motor_run(request):
motor.setSpeed(50)
motor.motor0(forward0)
motor.motor1(forward1)
return HttpResponse('Motors Runing')
def init():
print("a")
motor.setup()
servo_test.setup()
motor.setSpeed(20)
servo_test.pwm.write(0, 0, 240)
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
# 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
# --------Motor calibration----------
if data == 'motor_run':
print 'motor moving forward'
motor.setSpeed(50)
motor.motor0(forward0)
motor.motor1(forward1)
elif data[0:9] == 'leftmotor':
forward0 = data[9:]
motor.motor0(forward0)
elif data[0:10] == 'rightmotor':
forward1 = data[10:]
motor.motor1(forward1)
# -------------Added--------------
elif data == 'leftreverse':
if forward0 == "True":
forward0 = "False"
else:
forward0 = "True"
print "left motor reversed to", forward0
motor.motor0(forward0)
elif data == 'rightreverse':
if forward1 == "True":
forward1 = "False"
else:
forward1 = "True"
print "right motor reversed to", forward1
motor.motor1(forward1)
elif data == 'motor_stop':
print 'motor stop'
motor.stop()
# ---------------------------------
# -------Turing calibration------
elif data[0:7] == 'offset=':
offset = int(data[7:])
car_dir.calibrate(offset)
# --------------------------------
# ----------Mount calibration---------
elif data[0:8] == 'offsetx=':
offset_x = int(data[8:])
print 'Mount offset x', offset_x
video_dir.calibrate(offset_x, offset_y)
elif data[0:8] == 'offsety=':
offset_y = int(data[8:])
print 'Mount offset y', offset_y
video_dir.calibrate(offset_x, offset_y)
# ----------------------------------------
# -------Turing calibration 2------
elif data[0:7] == 'offset+':
offset = offset + int(data[7:])
print 'Turning offset', offset
car_dir.calibrate(offset)
elif data[0:7] == 'offset-':
offset = offset - int(data[7:])
print 'Turning offset', offset
car_dir.calibrate(offset)
# --------------------------------
# ----------Mount calibration 2---------
elif data[0:8] == 'offsetx+':
offset_x = offset_x + int(data[8:])
print 'Mount offset x', offset_x
video_dir.calibrate(offset_x, offset_y)
elif data[0:8] == 'offsetx-':
offset_x = offset_x - int(data[8:])
print 'Mount offset x', offset_x
video_dir.calibrate(offset_x, offset_y)
elif data[0:8] == 'offsety+':
offset_y = offset_y + int(data[8:])
print 'Mount offset y', offset_y
video_dir.calibrate(offset_x, offset_y)
elif data[0:8] == 'offsety-':
offset_y = offset_y - int(data[8:])
print 'Mount offset y', offset_y
video_dir.calibrate(offset_x, offset_y)
# ----------------------------------------
# ----------Confirm--------------------
elif data == 'confirm':
config = 'offset_x = %s\noffset_y = %s\noffset = %s\nforward0 = %s\nforward1 = %s\n ' % (
offset_x, offset_y, offset, forward0, forward1)
print ''
print '*********************************'
print ' You are setting config file to:'
print '*********************************'
print config
print '*********************************'
print ''
fd = open('config', 'w')
fd.write(config)
fd.close()
motor.stop()
tcpCliSock.close()
quit()
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 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
# 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
#--------Motor calibration----------
if data == 'motor_run':
print 'motor moving forward'
motor.setSpeed(50)
motor.motor0(forward0)
motor.motor1(forward1)
elif data[0:9] == 'leftmotor':
forward0 = data[9:]
motor.motor0(forward0)
elif data[0:10] == 'rightmotor':
forward1 = data[10:]
motor.motor1(forward1)
# -------------Added--------------
elif data == 'leftreverse':
if forward0 == "True":
forward0 = "False"
else:
forward0 = "True"
print "left motor reversed to", forward0
motor.motor0(forward0)
elif data == 'rightreverse':
if forward1 == "True":
forward1 = "False"
else:
forward1 = "True"
print "right motor reversed to", forward1
motor.motor1(forward1)
elif data == 'motor_stop':
print 'motor stop'
motor.stop()
#---------------------------------
#-------Turing calibration------
elif data[0:7] == 'offset=':
offset = int(data[7:])
car_dir.calibrate(offset)
#--------------------------------
#----------Mount calibration---------
elif data[0:8] == 'offsetx=':
offset_x = int(data[8:])
print 'Mount offset x', offset_x
video_dir.calibrate(offset_x, offset_y)
elif data[0:8] == 'offsety=':
offset_y = int(data[8:])
print 'Mount offset y', offset_y
video_dir.calibrate(offset_x, offset_y)
#----------------------------------------
#-------Turing calibration 2------
elif data[0:7] == 'offset+':
offset = offset + int(data[7:])
print 'Turning offset', offset
car_dir.calibrate(offset)
elif data[0:7] == 'offset-':
offset = offset - int(data[7:])
print 'Turning offset', offset
car_dir.calibrate(offset)
#--------------------------------
#----------Mount calibration 2---------
elif data[0:8] == 'offsetx+':
offset_x = offset_x + int(data[8:])
print 'Mount offset x', offset_x
video_dir.calibrate(offset_x, offset_y)
elif data[0:8] == 'offsetx-':
offset_x = offset_x - int(data[8:])
print 'Mount offset x', offset_x
video_dir.calibrate(offset_x, offset_y)
elif data[0:8] == 'offsety+':
offset_y = offset_y + int(data[8:])
print 'Mount offset y', offset_y
video_dir.calibrate(offset_x, offset_y)
elif data[0:8] == 'offsety-':
offset_y = offset_y - int(data[8:])
print 'Mount offset y', offset_y
video_dir.calibrate(offset_x, offset_y)
#----------------------------------------
#----------Confirm--------------------
elif data == 'confirm':
config = 'offset_x = %s\noffset_y = %s\noffset = %s\nforward0 = %s\nforward1 = %s\n ' % (offset_x, offset_y, offset, forward0, forward1)
print ''
print '*********************************'
print ' You are setting config file to:'
print '*********************************'
print config
print '*********************************'
print ''
fd = open('config', 'w')
fd.write(config)
fd.close()
motor.stop()
tcpCliSock.close()
quit()
else:
print 'Command Error! Cannot recognize command: ' + data
def setMotorSpeed(pwr):
#pwr is 0 to 100
motor.setSpeed(pwr)
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
def stop():
setSpeed(finalSpeed=0)
return jsonify({"speed": 0})
def speed(self, value):
motor.setSpeed(int(value))
return self.ack()
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 set_speed(self, speed):
m.setSpeed(speed)
def init():
motor.setup()
servo_test.setup()
motor.setSpeed(0)
servo_test.pwm.write(0,0,500)
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 accel(self, spd_cmd):
drive_motor.setSpeed(spd_cmd)
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()
video_dir.move_decrease_y()
elif data == ctrl_cmd[12]:
video_dir.home_x_y()
elif data[0:5] == 'speed': # Change the speed
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:5] == 'turn=': #Turning Angle
angle = data.split('=')[1]
try:
angle = int(angle)
car_dir.turn(angle)
except:
print('Error: angle ='+ str(angle))
elif data[0:8] == 'forward=':
spd = data[8:]
try:
spd = int(spd)
motor.forward(spd)
except:
def delta(delta):
return jsonify({"speed": setSpeed(delta=int(delta))})
