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))})