def servo_turn(self, cur_angle=-90): if cur_angle == -90: return if CTRL_DEBUG: print("&Before PID angle =", cur_angle) steering_angle = int(self.pid(cur_angle)) + 90 if CTRL_DEBUG: print("$After PID angle =", steering_angle) car_dir.turn(steering_angle)
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 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 turn_angle(self, angle): cd.turn(angle)
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
cap = cv2.VideoCapture(0) cv2.namedWindow('frame') cur_time = 0 motor.setSpeed(40) while (cur_time < 2500): line = ser.readline() magData = open('magData.txt', 'a') magData.write(str(cur_time)) magData.write(',') magData.write(line) magData.close() motor.forward() cur_time += 1 car_dir.turn(determine_steering_angle(cur_time) + 20) time.sleep(1 / 60) if (cur_time % 200 == 0): motor.setSpeed(0) time.sleep(5) motor.setSpeed(40) ## ret, frame = cap.read() ## ## numRows = 10 ## processed = readyImage(frame) ## imageRows = splitImage(processed, numRows) ## ## centroidArray = [] ## centroidData = open('centroidData.txt', 'a') ## for i in range(7, len(imageRows)):
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()
controlState = 'manual' #start the process in manual control 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. cur_time = 0 while True: motor.forward() cur_time += 1 car_dir.turn(frequency_sweep.determine_steering_angle(cur_time)) data = '' 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[0:3] == 'mag': controlState = 'magnetic' elif data[0:3] == 'vis': controlState = 'visual' elif data[0:6] == 'manual': controlState = 'manual'
cap = cv2.VideoCapture(0) if display == 1: cv2.namedWindow('frame') sampleTime = .0166667 dt = sampleTime motor.setSpeed(0) totalTime = 0 errorSum = 0 prevError = 0 time.sleep(1) car_dir.turn(100) time.sleep(1) car_dir.turn(0) time.sleep(1) car_dir.turn(80) time.sleep(1) car_dir.turn(0) time.sleep(1) car_dir.turn(60) time.sleep(1) car_dir.turn(0) # ret, frame = cap.read() # numRows = 8 # processed = readyImage(frame) # imageRows = splitImage(processed, numRows)
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)
showCentroids(frame, numRows, centroidArray) cv2.imshow('frame', frame) cv2.waitKey(0) # PID controller error = errorCalc(frame, centroidArray, sampleTime) error = error[-1] errorSum += error * dt errordt = (error - prevError) / dt u = kp * error + ki * errorSum + kd * errordt prevError = error motor.forward() car_dir.turn(int(Map(np.pi/2 + u, 0, np.pi, 0, 255))) visData = open('visData.txt', 'a') visData.write(str(totalTime)) visData.write(',') visData.write(str(error)) visData.write(',') visData.write(str(errordt)) visData.write(',') visData.write(str(u)) visData.write(',') visData.write(raw_vals) visData.close() time.sleep(sampleTime)
def saturate(x,ub,lb): if x>ub: return(ub) elif x<lb: return(lb) else: return(x) # what is thiserror supposed to be? Is it the erorr in the heading angle for the car? # I'm supposing that it's the error in the heading angle for the car, and if it's not, # then we can convert this function to be exactly that. # Uncomment delay if you want to run the car on the ground. car_dir.turn(25) time.sleep(5) tot_time = 0 start_time = 0 SampleTime = .01666667 lbase = -75 # numeric values for the turn limits of the car (lbase = left rbase = right) rbase = 125 langle = np.pi/2-np.pi/6-0.25 # Angular limits for turning the car rangle = np.pi/2+np.pi/6+0.25 c_alpha = Map(25,lbase,rbase,langle,rangle) # Initial car-heading, set at straight ahead, 90 degrees interror = 0 # Gain Kp = 0.9 # used to be 1 Kd = 0 # used to be 120 Ki = 0.2
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 turn(angle_in_degree): car_dir.turn(angle_in_degree)
car_dir.home() pass motor.ctrl(0) #while (datetime.datetime.now() - #car_dir.turn_right() #car_dir.turn_right() #motor.forward() print "pausing" start = datetime.datetime.now() #pause for 2 seconds while (datetime.datetime.now() - start).seconds < 2: pass print "turnign left" car_dir.turn(turn_angle) motor.forward() start = datetime.datetime.now() while (datetime.datetime.now() - start).seconds < turn_time: car_dir.turn(turn_angle) pass motor.ctrl(0) print "pausing again" print "go home" car_dir.home() motor.forward() start = datetime.datetime.now() while (datetime.datetime.now() - start).seconds < forward_time: pass
def turning(request, angle): angle = int(angle) car_dir.turn(angle) text = "Turing angle", angle return HttpResponse(text)
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)
def setup(): car_dir.setup(busnum=busnum) motor.setup(busnum=busnum) car_dir.calibrate(0) motor.setSpeed(50) if __name__ == "__main__": #try: #setup() #loop() #except KeyboardInterrupt: # tcpSerSock.close() setup() car_dir.turn(0); time.sleep(2); car_dir.turn(45); time.sleep(2); car_dir.turn(90); time.sleep(2); while (True): time_before = time.time() motor.forward() # To do #motor.ctrl(1) #time.sleep(3) #motor.ctrl(1, -1)
def steer(self, str_cmd): MAX_STEER_DIFF = 1 steer_motor.turn(self.str_cmd)
if __name__ == "__main__": run = True port = serialPort() motor.setup(busnum=busnum) steer.setup(busnum=busnum) print 'motor moving forward' motor.setSpeed(speed) motor.motor0(forward0) motor.motor1(forward1) # motor.setSpeed(0) # motor.motor0(forward0) # motor.motor1(forward1) steer.turn(0) port.openPort("/dev/ttyUSB1") # open lidar with width, depth in cm, refresh rate in hz port.lidarStart(30, 200, 10) while run: try: data = port.lidarRead() print("distance: ", data[0]) print("angle: ", data[1]) distance = data[0] if distance != 0: scale = gain / distance # the further away something is, the less we have to turn else: scale = gain angle = data[1] angle = kalman(
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: print('Error speed ='+str(spd)) elif data[0:9] == 'backward=': spd = data.split('=')[1] try: spd = int(spd)
error = errorCalc(frame, centroidArray) error = error[5] errorSum += error * dt errordt = (error - prevError) / dt u = kp * error + ki * errorSum + kd * errordt # u = saturate(u, rangle, langle) prevError = error motor.forward() # print 'u is: ', u # controlsignal = int(Map(np.pi/2 + u, 0, np.pi, 0, 255)) controlsignal = int(Map(u, ledge, redge, lbase, rbase)) print 'Control siganl is: ', controlsignal car_dir.turn(controlsignal) visData = open('visData3.txt', 'a') visData.write(str(totalTime)) visData.write(',') visData.write(str(error)) visData.write(',') # visData.write(str(errordt)) # visData.write(',') visData.write(str(u)) visData.write('\n') visData.close() time.sleep(sampleTime) motor.setSpeed(0)