def __init__(self, caption, w, h, available_objects, precision): super().__init__() self.caption = caption self.w = w self.h = h self.keyboard = { 'A': False, 'D': False, 'W': False, 'S': False, 'SHIFT': False, 'SPACE': False } self.avaliable = available_objects self.precision = precision self.setFixedSize(w, h) self.setWindowTitle(caption) self.cam = Cam((0, 50, 0)) self.engine = Engine(self.cam) self.pointer = Pointer() self.canvas = Canvas(self, w * 0.75, h, 0, 0, 'rgba(100,100,100,100%)', self.engine) self.selector = ObjectSelector(w / 4, h / 3, 'Select figure to Create', self, 'rgba(50,50,50,100%)', self.avaliable) self.move_window = MoveWindow(self, w / 3, h / 3, 'Move object', -10000, 10000, 2, 0.0) self.scale_window = ScaleWindow(self, w / 3, h / 3, 'Scale object', 0.01, 10, 2, 1.0) self.rotate_window = RotateWindow(self, w / 3, h / 3, 'Rotate object', 0, 360, 2, 0.0) self.objects = SideBar(self, w * 0.25, h, w * 0.75, 0, 'rgba(50,50,50,100%)') main_menu = self.menuBar() camera_menu = main_menu.addMenu('Camera') reset_camera = QAction('Reset', camera_menu) reset_camera.setStatusTip('Reset camera position and mode') reset_camera.triggered.connect(self.reset_camera) self.switch_camera = QAction('Mode: Free flight', camera_menu) self.switch_camera.setStatusTip('Switching camera mode...') self.switch_camera.triggered.connect(self.switch_camera_mode) camera_menu.addAction(self.switch_camera) camera_menu.addAction(reset_camera) self.timer = QBasicTimer() self.frames = 60 self.dt = int(100 / self.frames) self.timer.start(self.dt, self) self.show()
def INIT(self): print("CURRENT STATE: INIT") # use Raspberry Pi board pin numbers GPIO.setmode(GPIO.BCM) GPIO.setup(mbl_bots.TRIG, GPIO.OUT) GPIO.setup(mbl_bots.ECHO, GPIO.IN) self.lastIMU = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] self.currentIMU = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] self.camera = Cam() self.imu = IMU(self.bus) self.distance = -1 signal.signal(signal.SIGINT, self.close) self.state = mbl_bots.REST time.sleep(1)
def test_name_generator(self): prefix = 'test' eng = Engine(Cam()) name = eng.generate_name(prefix) self.assertEqual(prefix + ' 1', name) eng.figures[prefix + ' 1'] = None name = eng.generate_name(prefix) self.assertEqual(prefix + ' 2', name)
def __init__(self, file): super(Robot, self).__init__() (self.actuators, self.sensors) = utils.file2bot(file, mbl_bots.BOTH) self.bus = smbus.SMBus(1) self.pwm = PCA9685(self.bus, 0x40, debug=False) self.pwm.setPWMFreq(50) #print("My name is: ", self.actuators[0].name) self.names_acc = [ self.actuators[i].name for i in range(len(self.actuators)) ] self.names_sen = [ self.sensors[i].name for i in range(len(self.sensors)) ] self.idx_acc = [i for i in range(len(self.actuators))] self.idx_sen = [i for i in range(len(self.sensors))] self.acc_dic = utils.genDictionary(self.names_acc, self.idx_acc) self.sen_dic = utils.genDictionary(self.names_sen, self.idx_sen) self.state = mbl_bots.INIT self.exploreState = mbl_bots.GETDATA self.movesCode = mbl_bots.NONE self.camera = Cam() self.imu = IMU(self.bus) self.vision = Vision()
calibration_params = bme280.load_calibration_params(self.bus, self.address) data = bme280.sample(self.bus, self.address, calibration_params) k=0 self.sensor_data.write(str(date.today())+"\t") self.sensor_data.write(str(datetime.now().strftime('%H:%M:%S'))+"\t") self.sensor_data.write(str(data.temperature)+"\t") self.sensor_data.write(str(data.humidity)+"\t\n") sleep(1) k+=1''' #microphone audio = audio(runtime) #run thread terminates after runtime audioThread = Thread(target=audio.run) audioThread.start() Cam = Cam() #Sensor = Sensor() CamThread = Thread(target=Cam.run) #SensorThread = Thread(target=Sensor.run) CamThread.start() #SensorThread.start() #Temp Hum sensor sensor = tempHumSensor() sensorThread = Thread(target=sensor.run) sensorThread.start() sleep(runtime) Cam.terminate() sensor.terminate() #Sensor.terminate()
from camera import Cam, rotate2d # ### PYGAME INIT pygame.init() w,h = 400,400 cx,cy = w//2,h//2 screen = pygame.display.set_mode((w,h)) clock = pygame.time.Clock() # ### DEFINING VERTECIES & EDGES verts = (-1,-1,-1),(1,-1,-1),(1,1,-1),(-1,1,-1),(-1,-1,1),(1,-1,1),(1,1,1),(-1,1,1) edges = (0, 1),(1, 2),(2, 3),(3, 0),(4, 5),(5, 6),(6, 7),(7, 4),(0, 4),(1, 5),(2, 6),(3, 7) # faces = (0,1,2,3),(4,5,6,7), cam = Cam((0,0,-5)) pygame.event.get() pygame.mouse.get_rel() pygame.mouse.set_visible(0) pygame.event.set_grab(1) # ### WINDOW WHILE LOOP while True: # clock tick time delta dt = clock.tick()/1000 # event handler for event in pygame.event.get():
def startMission(): vrep.simxFinish(-1) clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5) while clientID <= -1: clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5) if clientID != -1: print('Successful !!!') else: print('connection not successful') #sys.exit('could not connect') print('connected to remote api server') #vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot) sens = ProxSensor(clientID) lineSensors = IRSensors(clientID) imu = IMU(clientID) regulator = pidControl(0.006, 0.001, 0.001) cameraAreaControl = relle(10000, 10500, 0.03) cameraPosControl = relle(124, 126, 0.005) car = Robot(clientID, 0.04) camera = Cam(clientID) imuFilter = compliment() gripper = Gripper(clientID) step = 1 velocity = 0.1 crater1 = np.array([3, 4]) crater2 = np.array([3, 7]) def moveDir(vel, pos): dt = 0 edgeS = False #машинка на склоне edgeS2 = False #машинка проехала склон position = 1 while (position < pos): ts = vrep.simxGetLastCmdTime(clientID) camera.getCameraImage() sign, x, y, area = camera.trackObject('red') cameraPosControl.control(y) if area > 120 and area < 140: position = 3 elif area > 180 and area < 200: position = 4 elif area > 260 and area < 300: position = 5 elif area > 450 and area < 550: position = 6 elif area > 1300 and area < 1800: position = 7 elif area > 2400 and area < 3000: position = 8 if (sign): U1 = cameraPosControl.U else: U1 = 0.02 car.move(-U1 - vel, +U1 - vel, 0, dt) print(area, position) #if(position>=pos and y<128 and y>122): # break #sens.updateAllSensors() #regulator.pid(sens.lsVal-0.4,dt) #imu.upgrateIMU() #imuFilter.filter(imu.accelData,imu.gyroData,dt) #if(imuFilter.roll>0.2 and not lineSensors.getGreenColorSignal()): # U=0 # if not edgeS2 and not edgeS: # edgeS=True #else: # #U=regulator.U # U=0 #if(imuFilter.roll<0.2 and edgeS): # edgeS2=True # car.position=0 # edgeS=False #if edgeS: # car.position=0 #car.move(-vel-U,-vel+U,0,dt) dt = (vrep.simxGetLastCmdTime(clientID) - ts) / 1000 car.stop() car.position = 0 def moveWhile(color): dt = 0.001 lineSensors.updateLineSensors() lineSensors.getBlackColorSignal() lineSensors.getGreenColorSignal() signal = False while (not signal): ts = vrep.simxGetLastCmdTime(clientID) lineSensors.getBlackColorSignal() lineSensors.getGreenColorSignal() if color == 'green': signal = lineSensors.greenSignal elif color == 'black': signal = lineSensors.blackSignal #print(lineSensors.blackSignal ) car.move(-velocity, -velocity, 0, dt) dt = (vrep.simxGetLastCmdTime(clientID) - ts) / 1000 time.sleep(2) car.stop() def getBall(): dt = 0.001 camera.setCameraPosition(0, 0) while True: ts = vrep.simxGetLastCmdTime(clientID) camera.getCameraImage() sign, x, y, area = camera.trackObject('blue') cameraAreaControl.control(area) cameraPosControl.control(y) if (sign): U1 = cameraPosControl.U U2 = cameraAreaControl.U #print(y) else: U1 = 0.02 U2 = 0 car.move(-U2 - U1, -U2 + U1, 0, dt) #print(area) if (area > 6700 and y < 126 and y > 122): car.stop() gripper.giveBall() break camera.showStream() dt = (vrep.simxGetLastCmdTime(clientID) - ts) / 1000 def searchDir(color): camera.setCameraPosition(30, 0.8) dt = 0.001 while True: ts = vrep.simxGetLastCmdTime(clientID) camera.getCameraImage() sign, x, y, area = camera.trackObject(color) cameraAreaControl.control(area) cameraPosControl.control(y) if (sign): U1 = cameraPosControl.U U2 = cameraAreaControl.U else: U1 = 0.02 U2 = 0 car.move(-U2 - U1, -U2 + U1, 0, dt) #print(area) if (area > 1000 and y < 128 and y > 122): car.move(-velocity, -velocity, 0, dt) break camera.showStream() dt = (vrep.simxGetLastCmdTime(clientID) - ts) / 1000 def colibrate(): camera.setCameraPosition(30, 0.8) dt = 0.001 while True: ts = vrep.simxGetLastCmdTime(clientID) camera.getCameraImage() sign, x, y, area = camera.trackObject('red') cameraAreaControl.control(area) cameraPosControl.control(y) if (sign): U1 = cameraPosControl.U U2 = cameraAreaControl.U else: U1 = 0.02 U2 = 0 car.move(-2 * U2 - U1, -2 * U2 + U1, 0, dt) #print(area) if (y < 128 and y > 122): car.stop() camera.destroyAllStream() break camera.showStream() dt = (vrep.simxGetLastCmdTime(clientID) - ts) / 1000 def makeOperation(pos): colibrate() moveDir(velocity, pos) car.rotate(80) moveWhile('black') getBall() if pos > 5: searchDir('blue') searchDir('green') moveWhile('green') gripper.brokeBall() makeOperation(crater1[1]) makeOperation(crater2[1]) car.stop() camera.destroyAllStream() time.sleep(1) vrep.simxFinish(clientID)
cam_sources = json.loads(config["Camera"]["camera_sources"]) cam_amount = config['Camera'].getint('number_of_cameras') cam_width = config['Camera'].getint('width') cam_height = config['Camera'].getint('height') #os.system('cls||clear') clear() cam_list = [] for index, i in enumerate(cam_sources): clear() printProgressBar(index, len(cam_sources), prefix='Progress:', suffix='Complete', length=50) cam_list.append(Cam(i, cam_id_list, width=cam_width, height=cam_height)) cv.waitKey(250) clear() printProgressBar(len(cam_sources), len(cam_sources), prefix='Progress:', suffix='Complete', length=50) cv.waitKey(250) cam_list = sort_list(cam_list) clear() #Insert Functions Here # A List of Items #--------------------------------------------------------------------------------------------------------------------- while True:
import operator import cv2 import numpy as np from camera import Cam from flask import Flask, render_template, Response from platform import system if False: from .camera import Cam ifwin = str(system()).lower() == 'windows' app = Flask(__name__) stream = Cam("192.168.43.54:8080" if ifwin else "127.0.0.1:8080") stream.start() hsv_min_white = np.array((51, 0, 180), np.uint8) hsv_max_white = np.array((171, 134, 255), np.uint8) hsv_min_red = np.array((0, 152, 73), np.uint8) hsv_max_red = np.array((206, 232, 162), np.uint8) hsv_min_green = np.array((54, 77, 56), np.uint8) hsv_max_green = np.array((86, 255, 255), np.uint8) hsv_min_yellow = np.array((0, 178, 154), np.uint8) hsv_max_yellow = np.array((37, 255, 255), np.uint8) hsv_min_blue = np.array((86, 38, 35), np.uint8) hsv_max_blue = np.array((190, 204, 164), np.uint8)
import pygame, pygame.gfxdraw import sys, math, platform from camera import Cam cam = Cam() if platform.system == "Windows": gfx = False else: gfx = True # use gfxdraw or not class Axis(object): # color: a tuple of colors of x, y, z axis def __init__(self, color): self.vertices = [(0, 0, 0), (1, 0, 0), (0, 1, 0), (0, 0, 1)] self.edges = [(0, 1), (0, 2), (0, 3)] self.color = list(color) def display(self, surface, perspective): for i in range(len(self.edges)): edge = self.edges[i] points = [] for x, y, z in [self.vertices[edge[0]], self.vertices[edge[1]]]: x, y = x * math.cos(cam.rotation[0]) - y * math.sin(cam.rotation[0]), y * math.cos(cam.rotation[0]) + x * math.sin(cam.rotation[0]) x, z = x * math.cos(cam.rotation[1]) - z * math.sin(cam.rotation[1]), z * math.cos(cam.rotation[1]) + x * math.sin(cam.rotation[1]) amp = pygame.Surface.get_width(surface) // 30 if perspective: px, py = y * amp, - z * amp else: px, py = y * amp, - (z - x) * amp points += [[(pygame.Surface.get_width(surface) - 60) + int(px), 60 + int(py)]]
def test_button_reaction(self): cam = Cam() keys = { 'W': True, 'D': True, 'A': False, 'S': False, 'SPACE': True, 'SHIFT': False } cam.update_position(100, keys) self.assertEqual(cam.pos, [50, -50, 50]) keys = { 'W': False, 'D': False, 'A': True, 'S': True, 'SPACE': False, 'SHIFT': True } cam.update_position(50, keys) self.assertEqual(cam.pos, [25, -25, 25]) cam = Cam() cam.mode = 1 keys = { 'W': True, 'D': True, 'A': False, 'S': False, 'SPACE': True, 'SHIFT': False } cam.update_position(100, keys) self.assertEqual(cam.pos, [50, -50, 50]) keys = { 'W': False, 'D': False, 'A': True, 'S': True, 'SPACE': False, 'SHIFT': True } cam.update_position(50, keys) self.assertEqual(cam.pos, [25, -25, 25])
class SPR4(Robot): def __init__(self): super(SPR4, self).__init__("/home/pi/SPR4/SPR4_code/SPR4.botfile.txt") #Brain method --- conscience!!!! if (len(sys.argv) == 2): print("EXECUTING TEST OF MOVEMENT", str(sys.argv[1])) while True: super(SPR4, self).executeMove(str(sys.argv[1]), 1) else: while True: self.generalFSM(self.state) #============================================================================= # GENERAL FSM #============================================================================= def generalFSM(self, state): states_list = { 0: self.INIT, 1: self.REST, 2: self.EXPLORE, 3: self.SHOWOFF, 4: self.PHOTO, } func = states_list.get(state, lambda: None) return func() #============================================================================= # INIT STATE #============================================================================= def INIT(self): print("CURRENT STATE: INIT") # use Raspberry Pi board pin numbers GPIO.setmode(GPIO.BCM) GPIO.setup(mbl_bots.TRIG, GPIO.OUT) GPIO.setup(mbl_bots.ECHO, GPIO.IN) self.lastIMU = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] self.currentIMU = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] self.camera = Cam() self.imu = IMU(self.bus) self.distance = -1 signal.signal(signal.SIGINT, self.close) self.state = mbl_bots.REST time.sleep(1) #============================================================================= # REST STATE #============================================================================= def REST(self): print("CURRENT STATE: REST") start_time = time.time() while ((time.time() - start_time) < 20): if (super(SPR4, self).detectCatch(self.imu)): for i in range(5): super(SPR4, self).shake() break else: time.sleep(0.5) time.sleep(1) self.state = mbl_bots.EXPLORE #============================================================================= # EXPLORE STATE & FSM #============================================================================= def exploreFSM(self, state): substates_explorelist = { 0: self.exploreGetData, 1: self.exploreProcessData, 2: self.exploreMove, 3: self.exploreReconTurn, } func = substates_explorelist.get(state, lambda: None) return func() def EXPLORE(self): print("CURRENT STATE: EXPLORE") super(SPR4, self).stand() #EXPLORING FiniteStateMachine while (self.state == mbl_bots.EXPLORE): self.exploreFSM(self.exploreState) def exploreGetData(self): print("CURRENT STATE: EXPLORE") print("CURRENT SUBSTATE: DATA ACQUISITION") self.distance = utils.getDistance() self.lastIMU = self.currentIMU self.currentIMU = self.imu.getImuRawData() self.exploreState = mbl_bots.PROCESSDATA def exploreProcessData(self): print("CURRENT STATE: EXPLORE") print("CURRENT SUBSTATE: DATA PROCESSING") if (self.distance < 5): self.movesCode = mbl_bots.WB elif (self.distance > 15): self.movesCode = mbl_bots.WF else: if (randint(0, 1) == 1): self.movesCode = mbl_bots.TR else: self.movesCode = mbl_bots.TL self.exploreState = mbl_bots.MOVE # if(randint(0,5)>1): # self.exploreState = mbl_bots.MOVE # else: # self.exploreState = mbl_bots.GETDATA # self.state = mbl_bots.SHOWOFF def exploreMove(self): print("CURRENT STATE: EXPLORE") print("CURRENT SUBSTATE: MOVING") super(SPR4, self).move(self.movesCode) self.exploreState = mbl_bots.GETDATA def exploreReconTurn(self): print("Not implemented...") #============================================================================= # SHOW OFF STATE #============================================================================= def SHOWOFF(self): print("CURRENT STATE: SHOWOFF") #SHOWOFF METHODS super(SPR4, self).swing() super(SPR4, self).sayHello() self.state = mbl_bots.PHOTO #============================================================================= # PHOTO STATE #============================================================================= def PHOTO(self): print("CURRENT STATE: PHOTO") super(SPR4, self).cameraPose() self.camera.takePic() super(SPR4, self).stand() self.state = mbl_bots.EXPLORE def close(self, signal, frame): print("\nTurning off SPR4 Activity...\n") GPIO.cleanup() sys.exit(0)
class Application(QMainWindow): def __init__(self, caption, w, h, available_objects, precision): super().__init__() self.caption = caption self.w = w self.h = h self.keyboard = { 'A': False, 'D': False, 'W': False, 'S': False, 'SHIFT': False, 'SPACE': False } self.avaliable = available_objects self.precision = precision self.setFixedSize(w, h) self.setWindowTitle(caption) self.cam = Cam((0, 50, 0)) self.engine = Engine(self.cam) self.pointer = Pointer() self.canvas = Canvas(self, w * 0.75, h, 0, 0, 'rgba(100,100,100,100%)', self.engine) self.selector = ObjectSelector(w / 4, h / 3, 'Select figure to Create', self, 'rgba(50,50,50,100%)', self.avaliable) self.move_window = MoveWindow(self, w / 3, h / 3, 'Move object', -10000, 10000, 2, 0.0) self.scale_window = ScaleWindow(self, w / 3, h / 3, 'Scale object', 0.01, 10, 2, 1.0) self.rotate_window = RotateWindow(self, w / 3, h / 3, 'Rotate object', 0, 360, 2, 0.0) self.objects = SideBar(self, w * 0.25, h, w * 0.75, 0, 'rgba(50,50,50,100%)') main_menu = self.menuBar() camera_menu = main_menu.addMenu('Camera') reset_camera = QAction('Reset', camera_menu) reset_camera.setStatusTip('Reset camera position and mode') reset_camera.triggered.connect(self.reset_camera) self.switch_camera = QAction('Mode: Free flight', camera_menu) self.switch_camera.setStatusTip('Switching camera mode...') self.switch_camera.triggered.connect(self.switch_camera_mode) camera_menu.addAction(self.switch_camera) camera_menu.addAction(reset_camera) self.timer = QBasicTimer() self.frames = 60 self.dt = int(100 / self.frames) self.timer.start(self.dt, self) self.show() def keyPressEvent(self, event): key = event.key() if self.canvas.hasFocus(): if key == Qt.Key_A: self.keyboard['A'] = True elif key == Qt.Key_D: self.keyboard['D'] = True elif key == Qt.Key_W: self.keyboard['W'] = True elif key == Qt.Key_S: self.keyboard['S'] = True elif key == Qt.Key_Space: self.keyboard['SPACE'] = True elif key == Qt.Key_Shift: self.keyboard['SHIFT'] = True def keyReleaseEvent(self, event): key = event.key() if key == Qt.Key_A: self.keyboard['A'] = False elif key == Qt.Key_D: self.keyboard['D'] = False elif key == Qt.Key_W: self.keyboard['W'] = False elif key == Qt.Key_S: self.keyboard['S'] = False elif key == Qt.Key_Space: self.keyboard['SPACE'] = False elif key == Qt.Key_Shift: self.keyboard['SHIFT'] = False def timerEvent(self, event): dx, dy = self.pointer.get_movement() self.cam.update_rotation(dx, dy) self.cam.update_position(self.dt, self.keyboard) self.canvas.repaint() def add_object(self, obj): self.engine.add_object(obj) list_object = ListObject(obj.name, self.objects.list) self.objects.list.addItem(list_object) self.objects.list.clearSelection() self.objects.list.setCurrentItem(list_object) def delete_object(self, name): items = self.objects.list.findItems(name, Qt.MatchExactly) if len(items) > 0: row = self.objects.list.row(items[0]) self.objects.list.takeItem(row) self.engine.delete_item(name) def switch_camera_mode(self): if self.cam.mode == 0: self.cam.mode = 1 self.switch_camera.setText('Mode: Centered') else: self.cam.mode = 0 self.switch_camera.setText('Mode: Free Flight') self.reset_camera() def reset_camera(self): self.cam.pos = [0, 50, 0] self.cam.rot = [0, 0, 0]