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)