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]
