def __init__(self): threading.Thread.__init__(self) self.shutdown_flag = threading.Event() self.motor = Motorcontroller() self.buzzer = Buzzer() self.xbee = Xbee() self.decoder = Decoder() self.servo1 = 96 self.servo2 = 75 self.joycalc = Joystick() self.motor.setServo1(self.servo1) self.motor.setServo2(self.servo2) self.lastSavedTime = 0
class Decoder:
def __init__(self):
print "Create decoder class"
self.joycalc = Joystick()
self.joystick_VRX1 = 512
self.joystick_VRY1 = 512
self.joystick_PB1 = 0
self.joystick_VRX2 = 512
self.joystick_VRY2 = 512
self.joystick_PB2 = 0
self.calcCRC = 0
self.rcvCRC = 0
self.status = 0
self.lastidd = 0
self.iddcounter = 0
self.rfrating = 0
self.temp = 0
def decode(self, rcvdata):
self.status = 0
try:
if rcvdata != "":
rcvdata_split = (rcvdata.split(':'))
self.joystick_VRX1 = int(rcvdata_split[0])
self.joystick_VRY1 = int(rcvdata_split[1])
self.joystick_PB1 = int(rcvdata_split[2])
self.joystick_VRX2 = int(rcvdata_split[3])
self.joystick_VRY2 = int(rcvdata_split[4])
self.joystick_PB2 = int(rcvdata_split[5])
self.temp = int(rcvdata_split[6])
idd = int(rcvdata_split[6])
self.rcvCRC = int(rcvdata_split[7])
self.joycalc.calculate(self.getJoystickVRX1(),
self.getJoystickVRY1())
self.calcCRC = self.joystick_VRX1 + self.joystick_VRY1 + self.joystick_PB1 + self.joystick_VRX2 + self.joystick_VRY2 + self.joystick_PB2
self.status = 1
if idd < self.lastidd:
self.rfrating = (self.iddcounter * 100) / 255
self.iddcounter = 0
self.lastidd = 0
else:
self.lastidd = idd
self.iddcounter = self.iddcounter + 1
except ValueError, e:
print "JSON type error"
except IndexError, e:
print "Ontvangen data was het verkeerde formaat"
def __init__(self):
print "Create decoder class"
self.joycalc = Joystick()
self.joystick_VRX1 = 512
self.joystick_VRY1 = 512
self.joystick_PB1 = 0
self.joystick_VRX2 = 512
self.joystick_VRY2 = 512
self.joystick_PB2 = 0
self.calcCRC = 0
self.rcvCRC = 0
self.status = 0
self.lastidd = 0
self.iddcounter = 0
self.rfrating = 0
self.temp = 0
def setUp(self):
'''
Verify environment is setup properly.
'''
self.joystick = Joystick()
j_list = self.joystick.get_joystick_list()
self.joystick.connect(j_list[0])
def __init__(self):
'''
Initialize class attributes.
'''
self.joystick = Joystick()
self.communication = Communication()
self.driving = Driving()
self.gear = 0
def __init__(self, master=None):
self.lastPressed = 'man' # string to remember which button has been pressed
# socket connection to send data from remote
self.s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
self.s.connect(("8.8.8.8", 80))
self.HOST = self.s.getsockname()[0]
self.PORT = 5002
print self.HOST
self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.s.bind((self.HOST, self.PORT))
self.s.listen(1)
self.conn, self.addr = self.s.accept()
self.s.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
print 'Connected by', self.addr
GPIO.setmode(GPIO.BCM)
# creates instance of the joystick class to control the physical joysticks
self.joystickBus = Joystick.getInstance()
Frame.__init__(self, master)
self.master = master
self.init_window()
def hardware_setup():
dm = DeviceManager()
js = Joystick()
cam = Cam(CAM_DEVICE_INDEX, LOG_PATH + "/images")
dm.add("dc_motor", DCMotorDevice(DC_MOTOR_HUB, DC_MOTOR_PORT))
dm.link("dc_motor", "setTargetVelocity", "throttle")
dm.add("servo", ServoMotorDevice(SERVO_HUB, SERVO_PORT, SERVO_CHANNEL))
dm.link("servo", "setTargetPosition", "direction",
lambda val: interp(val, [0, 180], [45, 135]))
dm.add("gps", GPSDevice(GPS_SERIAL_NUM))
dm.add("wind_speed", WindSpeedDevice(WIND_HUB, WIND_SPEED_PORT))
dm.add("wind_direction", WindDirectionDevice(WIND_HUB,
WIND_DIRECTION_PORT))
dm.add("spatial", SpatialDevice())
dm.add(
"water_speed_forward",
WaterSpeedDevice("forward", WATER_SPEED_HUB, WATER_SPEED_FORWARD_PORT))
dm.add(
"water_speed_backward",
WaterSpeedDevice("backward", WATER_SPEED_HUB,
WATER_SPEED_BACKWARD_PORT))
dm.add("water_speed_left",
WaterSpeedDevice("left", WATER_SPEED_HUB, WATER_SPEED_LEFT_PORT))
dm.add("water_speed_right",
WaterSpeedDevice("right", WATER_SPEED_HUB, WATER_SPEED_RIGHT_PORT))
dm.add("lidar", LidarLiteDevice())
dm.waitUntilAllReady()
return dm, js, cam
from Joystick import Joystick import time joystick = Joystick() while True: print joystick.get_x(), joystick.get_y(), joystick.get_r(), joystick.get_throttle() time.sleep(0.05)
mqtt.on_connect = on_connect mqtt.on_disconnect = on_disconnect mqtt.on_message = on_message mqtt.will_set(SERVICE_NAME + "/state/status", "OFF", 1, True) mqtt.reconnect_delay_set(min_delay=1, max_delay=60) mqtt.connect(BROKER_ADDRESS, BROKER_PORT, 60) # connect to broker mqtt.publish(SERVICE_NAME + "/state/status", "ON", 1, True) # Modules pixels = Pixels(mqtt, SERVICE_NAME, DEBUG) buzzer = Buzzer(mqtt, SERVICE_NAME, DEBUG) cameraServo = CameraServo(mqtt, SERVICE_NAME, DEBUG) infrared_receiver = InfraredReceiver(mqtt, SERVICE_NAME, DEBUG) infrared_sensor = InfraredSensor(mqtt, SERVICE_NAME, DEBUG) joystick = Joystick(mqtt, SERVICE_NAME, DEBUG) obstacle_avoidance = ObstacleAvoidance(mqtt, SERVICE_NAME, DEBUG) trSensor = TRSensor(mqtt, SERVICE_NAME, 5, DEBUG) ultrasonic = Ultrasonic(mqtt, SERVICE_NAME, DEBUG) wheels = Wheels(mqtt, SERVICE_NAME, DEBUG) # Setup IR Sensor infrared_receiver.buzzer = buzzer infrared_receiver.camera_servo = cameraServo infrared_receiver.pixels = pixels infrared_receiver.wheels = wheels # Setup Joystick joystick.buzzer = buzzer joystick.camera_servo = cameraServo joystick.wheels = wheels
import time
from Joystick import Joystick
js = Joystick()
def test_joystick():
while True:
print(js.get_event())
time.sleep(1)
test_joystick()
class SolomonsKey:
level = None
keys = {}
cam_pos = XYZ(0, 0, 0)
cam_pos_target = XYZ(0, 0, 0)
cam_focus = XYZ(0, 0, 0)
cam_focus_target = XYZ(0, 0, 0)
lastFrameTime = 0
topFPS = 0
camera_sweep = 20
joystick = Joystick()
def animate(self, FPS=30):
currentTime = time()
#key movement for omnicontrol
try:
if self.keys["x"]: self.cam_focus.x += 1
if self.keys["z"]: self.cam_focus.x -= 1
if self.keys["d"]: self.cam_focus.y += 1
if self.keys["c"]: self.cam_focus.y -= 1
if self.keys["f"]: self.cam_focus.z += 1
if self.keys["v"]: self.cam_focus.z -= 1
except:
pass
# run calculations for level inc collision etc
if not self.level == None:
self.level.evaluate(self.joystick, self.keys)
glutPostRedisplay()
glutTimerFunc(int(1000 / FPS), self.animate, FPS)
drawTime = currentTime - self.lastFrameTime
self.topFPS = int(1000 / drawTime)
# set camera target focus points
self.cam_focus_target = XYZ(
self.level.solomon.x + 0.2 * self.level.solomon.facing,
self.level.solomon.y - 0.5, 3.0)
# set camera target position
self.cam_pos_target = XYZ(
self.level.solomon.x + 1 * self.level.solomon.facing,
self.level.solomon.y - 0.2, float(self.level.target_z))
# calculate current focal point and camera position
# self.camera_sweep is the "speed" at which transitions are being made
self.cam_pos = self.cam_pos.add(
self.cam_pos_target.sub(self.cam_pos).mult(1 / self.camera_sweep))
self.cam_focus = self.cam_focus.add(
self.cam_focus_target.sub(self.cam_focus).mult(1 /
self.camera_sweep))
self.lastFrameTime = time()
# if windowed ensure aspect ratio correct
def reshape(self, width, height):
r = float(width) / float(height)
glViewport(0, 0, width, height)
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
gluPerspective(60.0, r, 1., 50.)
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
#glPushMatrix()
def __init__(self):
print(str(bool(glutInit)))
print("hello and weolcome")
print(
"if you see an error next try the unofficial binaries of pyopengl")
print("initializing glut etc")
glutInit(sys.argv)
glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH)
glutInitWindowSize(640, 480)
glutCreateWindow(name)
print("set blend function")
glBlendFunc(GL_SRC_ALPHA, GL_ONE)
print("set colours and lights")
glClearColor(0., 0., 0., 1.)
glShadeModel(GL_SMOOTH)
glEnable(GL_CULL_FACE)
glEnable(GL_DEPTH_TEST)
glEnable(GL_LIGHTING)
print("set light 1")
lightZeroPosition = [10., 4., 10., 1.]
lightZeroColor = [0.9, 1.0, 0.9, 1.0] #green tinged
glLightfv(GL_LIGHT0, GL_POSITION, lightZeroPosition)
glLightfv(GL_LIGHT0, GL_DIFFUSE, lightZeroColor)
glLightf(GL_LIGHT0, GL_CONSTANT_ATTENUATION, 0.2)
glLightf(GL_LIGHT0, GL_LINEAR_ATTENUATION, 0.05)
glEnable(GL_LIGHT0)
print("set light 2")
lightZeroPosition2 = [-10., -4., 10., 1.]
lightZeroColor2 = [1.0, 0.9, 0.9, 1.0] #green tinged
glLightfv(GL_LIGHT1, GL_POSITION, lightZeroPosition2)
glLightfv(GL_LIGHT1, GL_DIFFUSE, lightZeroColor2)
glLightf(GL_LIGHT1, GL_CONSTANT_ATTENUATION, 0.2)
glLightf(GL_LIGHT1, GL_LINEAR_ATTENUATION, 0.05)
glEnable(GL_LIGHT1)
#initialization of letters
print("initialzing letters")
self.letters = Letters.Letters()
#for game models
print("making model lists")
MakeLists()
print("ignore key repeat")
glutIgnoreKeyRepeat(1)
print("attach glut events to functions")
glutSpecialFunc(self.keydownevent)
glutSpecialUpFunc(self.keyupevent)
glutReshapeFunc(self.reshape)
glutKeyboardFunc(self.keydownevent)
glutKeyboardUpFunc(self.keyupevent)
glutDisplayFunc(self.display)
#glutIdleFunc(self.display)
print("initial projection")
glMatrixMode(GL_PROJECTION)
gluPerspective(60.0, 640.0 / 480., 1., 50.)
glMatrixMode(GL_MODELVIEW)
glPushMatrix()
print("generating level")
self.level = generateLevel(0)
print("keys set up")
self.initkey("zxdcfvqaopm")
self.animate()
print("about to loop...")
glutMainLoop()
return
def initkey(self, cl):
for c in cl:
self.keydownevent(str(c.lower()).encode(), 0, 0)
self.keyupevent(str(c.lower()).encode(), 0, 0)
print("end initkey")
def display(self):
glLoadIdentity()
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
gluLookAt(self.cam_pos.x, self.cam_pos.y, self.cam_pos.z,
self.cam_focus.x, self.cam_focus.y, self.cam_focus.z, 0, 1,
0)
self.level.draw()
glLoadIdentity()
gluLookAt(0, -0.5, 2.5, 0, 0, 0, 0, 1, 0)
wdth = 0.3
glTranslate(
0.0 -
(len(self.level.solomon.current_state.keys()) - 1) * wdth / 2.0,
-1.3, 0)
if debug == True:
for k in self.level.solomon.current_state.keys():
col = "red"
if self.level.solomon.current_state[k]: col = "green"
glMaterialfv(GL_FRONT, GL_DIFFUSE, colours[col])
glutSolidCube(wdth - 0.02)
glTranslate(wdth, 0, 0)
glPushMatrix()
#glLoadIdentity()
glScale(0.005, 0.01, -0.01)
glTranslate(-90, 4, -20)
#glTranslate(-180,-70,0)
glTranslate(-wdth, 0, 0)
self.letters.drawString(k[:4])
glPopMatrix()
glLoadIdentity()
gluLookAt(0, -0.5, 2.5, 0, 0, 0, 0, 1, 0)
wdth = 0.2
joystick_actions = [x for x in dir(self.joystick) if x[0:2] == "is"]
glTranslate(0.0 - (len(joystick_actions) - 1) * wdth / 2.0, -1.0, 0)
if debug == True:
for k in joystick_actions:
#print(k)
col = "red"
if getattr(self.joystick, k)(self.keys): col = "green"
glMaterialfv(GL_FRONT, GL_DIFFUSE, colours[col])
glutSolidCube(wdth - 0.02)
glTranslate(wdth, 0, 0)
glPushMatrix()
#glLoadIdentity()
glScale(0.005, 0.01, -0.01)
glTranslate(-45, 0, -20)
#glTranslate(-180,-70,0)
glTranslate(-wdth, 0, 0)
self.letters.drawString(k[2:4])
glPopMatrix()
glLoadIdentity()
gluLookAt(0, 0, 2.5, 0, 0, 0, 0, 1, 0)
glScale(0.01, 0.01, -0.01)
glTranslate(-180, -70, 0)
if debug == True:
self.letters.drawString('X:' + str(self.level.solomon.x) + ' Y:' +
str(self.level.solomon.y))
glTranslate(0, 0 - 15, 0)
gx, gy = int(self.level.solomon.x), int(self.level.solomon.y)
self.letters.drawString('G {0} on: {1} below: {2}'.format(
str((gx, gy)), self.level.grid[gy][gx],
self.level.grid[gy - 1][gx]))
glTranslate(0, 0 - 15, 0)
self.letters.drawString('')
glutSwapBuffers()
def keydownevent(self, c, x, y):
print(
"*******************************************************************************"
)
try:
self.keys[c] = True
print(self.keys)
except Exception as e:
print(e)
pass
glutPostRedisplay()
def keyupevent(self, c, x, y):
try:
if c in self.keys: self.keys[c] = False
print(self.keys)
except Exception as e:
print(e)
pass
glutPostRedisplay()
class Controller(object):
__metaclass__ = SingletonController
def __init__(self):
'''
Initialize class attributes.
'''
self.joystick = Joystick()
self.communication = Communication()
self.driving = Driving()
self.gear = 0
def get_joystick_list(self):
'''
Return the control joystick list.
'''
return self.joystick.get_joystick_list()
def get_bluetooth_list(self):
'''
Return the bluetooth communication list.
'''
return self.communication.get_bluetooth_list()
def connect(self, joystick_name, bluetooth_name):
'''
Initialize the joystick and bluetooth connection.
'''
if self.joystick.connect(joystick_name) and \
self.communication.connect(bluetooth_name):
return True
return False
def disconnect(self):
'''
Perform the joystick and bluetooth disconnection.
'''
if self.joystick.disconnect() and \
self.communication.disconnect():
return True
return False
def quit_commands(self):
'''
Send quit signal to the wait queue.
'''
self.joystick.quit_commands()
def send_commands(self):
'''
Read the joystick commands and send it to the rover using a
bluetooth device.
'''
try:
command = ""
j_command = self.joystick.get_commands()
if j_command["UP"] == True and \
j_command["DOWN"] == False and \
j_command["LEFT"] == False and \
j_command["RIGHT"] == False:
command = "D1"
self.communication.send(command)
self.driving.set_angle(0.0)
elif j_command["UP"] == False and \
j_command["DOWN"] == False and \
j_command["LEFT"] == False and \
j_command["RIGHT"] == True:
command = "D3"
self.communication.send(command)
self.driving.set_angle(90.0)
elif j_command["UP"] == False and \
j_command["DOWN"] == True and \
j_command["LEFT"] == False and \
j_command["RIGHT"] == False:
command = "D5"
self.communication.send(command)
self.driving.set_angle(0.0)
elif j_command["UP"] == False and \
j_command["DOWN"] == False and \
j_command["LEFT"] == True and \
j_command["RIGHT"] == False:
command = "D7"
self.communication.send(command)
self.driving.set_angle(-90.0)
if j_command["SPEED"] == True and self.gear < 6:
self.gear = self.gear + 1
self.driving.set_power(self.gear)
command = "V%d" % self.gear
self.communication.send(command)
elif j_command["BREAK"] == True and self.gear > 0:
self.gear = self.gear - 1
self.driving.set_power(self.gear)
command = "V%d" % self.gear
self.communication.send(command)
if j_command["UP"] == False and \
j_command["DOWN"] == False and \
j_command["LEFT"] == False and \
j_command["RIGHT"] == False and \
j_command["SPEED"] == False and \
j_command["BREAK"] == False:
command = "B0"
self.communication.send(command)
self.driving.set_angle(0.0)
except:
print str(traceback.format_exc())
def event(_keys,key,solomon):
keys = Joystick()
if (keys.isFire(_keys)):
solomon.sol_wand = solomon.sol_wand_limit
print("ding")
def gogogo(level, solomon, _keys):
## print(solomon,level,_keys)
keys = Joystick()
if keys.isDown(_keys): solomon.sol_crouch=1
else: solomon.sol_crouch=0
left_headwall = solomon.sol_tile_at_left_head(level.grid)
left_footwall = solomon.sol_tile_at_left_foot(level.grid)
right_headwall = solomon.sol_tile_at_right_head(level.grid)
right_footwall = solomon.sol_tile_at_right_foot(level.grid)
## print('walls lh {} lf {} rh {} rf {}'.format(left_headwall, left_footwall, right_headwall, right_footwall))
solomon.sol_walking=0
if keys.isLeft(_keys):
solomon.sol_dir = -1
if (solomon.sol_crouch==0 and left_footwall==0 and left_headwall==0):
solomon.solx-=solomon.sol_step
solomon.sol_walking=1
elif keys.isRight(_keys):
solomon.sol_dir = 1
if (solomon.sol_crouch==0 and right_footwall==0 and right_headwall==0):
solomon.solx+=solomon.sol_step
solomon.sol_walking=1
floating=False
leftfootfloor = solomon.sol_tile_at_left_foot_floor(level.grid)
rightfootfloor = solomon.sol_tile_at_right_foot_floor(level.grid)
nofloor=(leftfootfloor==0 and rightfootfloor==0 )
## print('floor lf {} rf {} nf {}'.format(leftfootfloor, rightfootfloor, nofloor))
## print(f"y {solomon.soly} ")
if ((solomon.soly%solomon.tile)!=0 or nofloor ): floating=True
#if (nofloor ): floating=True
## print (f"floating {floating}")
if (solomon.sol_crouch==0 and keys.isUp(_keys) and not floating ):
if (solomon.sol_jump==0):
solomon.sol_jump=solomon.sol_jump_limit
solomon.sol_jump_rest = solomon.sol_jump_rest_limit
if (solomon.sol_jump>0 and solomon.sol_jump_rest==0):
solomon.sol_jump -= 1
leftheadceiling = solomon.sol_tile_at_left_head_ceiling(level.grid)
rightheadceiling = solomon.sol_tile_at_right_head_ceiling(level.grid)
if (leftheadceiling==0 and rightheadceiling==0): solomon.soly+=solomon.sol_jump_inc
else:
solomon.sol_jump=0
if (solomon.sol_dir==-1 and leftheadceiling>0 and leftheadceiling!=3 and solomon.sol_tile_dist_left_head(level.grid)<solomon.sol_size):
ll = floor((solomon.soly+solomon.sol_size)/solomon.tile)
cc = (floor(solomon.solx/solomon.tile))
level.grid[ll][cc] = leftheadceiling-1
#var b = document.getElementById(`block_cc${cc}ll${ll}`)
if (level.grid[ll][cc]==0):
#document.body.removeChild(b)
pass
elif (level.grid[ll][cc]==1):
pass
#b.style.backgroundColor = "grey"
elif (solomon.sol_dir==1 and rightheadceiling>0 and rightheadceiling!=3 and solomon.sol_tile_dist_right_head(level.grid)<(solomon.tile-solomon.sol_size)):
ll = floor((solomon.soly+solomon.sol_size)/solomon.tile)
cc = floor((solomon.solx+solomon.sol_size-solomon.sol_step)/solomon.tile)
level.grid[ll][cc] = rightheadceiling-1
#var b = document.getElementById(`block_cc${cc}ll${ll}`)
if (level.grid[ll][cc]==0):
#document.body.removeChild(b)
pass
elif (level.grid[ll][cc]==1):
pass
#b.style.backgroundColor = "grey"
solomon.sol_crouch = 0 #///decativate crouch
else:
solomon.sol_jump_rest -= 1
## print(f"jmp {solomon.sol_jump} fl {floating} wnd {solomon.sol_wand}")
if (solomon.sol_jump==0 and floating and solomon.sol_wand==0):
## print("falling")
solomon.soly-=solomon.sol_fall_inc
solomon.sol_crouch = 0 #///decativate crouch
if solomon.sol_wand!=0:
solomon.sol_wand -= 1
if (solomon.sol_wand==0):
level.bursts.append(Burst(x=solomon.solx+solomon.sol_dir*6,y=solomon.soly-solomon.sol_crouch*5,z=10))
#print(level.bursts)
#print('wand!', solomon.sol_tile_dist_left_head(level.grid), solomon.sol_tile_dist_right_head(level.grid))
block = solomon.sol_block(level.grid)
## print(block)
block_x = block[1]+solomon.sol_dir
block_y = block[0]-solomon.sol_crouch
if (solomon.sol_dir==1):
if (solomon.sol_tile_dist_right_head(level.grid)>0):
block_x +=1
elif (solomon.sol_dir==-1):
if (solomon.sol_tile_dist_left_head(level.grid)>0):
pass
#//block_x--
level_block = int(level.grid[block_y][block_x])
print(level_block)
if (level_block==0):
level.grid[block_y][block_x] = 2
print("zing {} {} {} ".format(level_block, level.grid[block_y][block_x], block_y,block_x))
#var b = document.createElement('div')
#b.id = `block_cc${block_x}ll${block_y}`
#b.style=`position:absolute; background-color:black; left:${block_x*tile}px; top:${(level.length-block_y-1)*tile}px; width:${tile}px; height:${tile}px;`
#document.body.appendChild(b)
elif (level_block<3):
level.grid[block_y][block_x] = 0
class Serialcom(threading.Thread):
def __init__(self):
threading.Thread.__init__(self)
self.shutdown_flag = threading.Event()
self.motor = Motorcontroller()
self.buzzer = Buzzer()
self.xbee = Xbee()
self.decoder = Decoder()
self.servo1 = 96
self.servo2 = 75
self.joycalc = Joystick()
self.motor.setServo1(self.servo1)
self.motor.setServo2(self.servo2)
self.lastSavedTime = 0
def run(self):
print('Thread #%s started' % self.ident)
self.motor.timeout(1)
while not self.shutdown_flag.is_set():
rcvdata = self.xbee.read()
self.decoder.decode(rcvdata)
self.motor.recalCommand()
currenttime = time.time()
if currenttime - self.lastSavedTime > 1.0:
self.lastSavedTime = time.time()
self.xbee.sendBat(self.decoder.getRfrating())
if self.decoder.getStatus() and self.decoder.checkCRC():
if self.decoder.getJoyStickPB1() == 0:
self.motor.EmergyStop()
self.buzzer.beep(300)
elif self.decoder.getJoystickM1() > 248 and self.decoder.getJoystickM2() > 248:
self.joycalc.calculateReg(255)
self.motor.Motor1MC2(255 - self.joycalc.cor1)
self.motor.Motor2MC2(255 - self.joycalc.cor2)
elif (abs(self.decoder.getJoystickM1() - self.decoder.getJoystickM2()) <= 3) and (self.decoder.getJoystickM1() > 50):
self.joycalc.calculateReg(self.decoder.getJoystickM1())
self.motor.Motor1MC2(self.decoder.getJoystickM1() - self.joycalc.cor1)
self.motor.Motor2MC2(self.decoder.getJoystickM1() - self.joycalc.cor2)
#print "drive forward without full speed"
else:
self.motor.Motor1MC2(self.decoder.getJoystickM1())
self.motor.Motor2MC2(self.decoder.getJoystickM2())
#print "other speeds"
if self.decoder.getJoystickPB2() == 0:
self.servo1 = 96
self.motor.setServo1(self.servo1)
self.buzzer.beep(300)
elif self.decoder.getJoystickVRX2() > 1000:
if(self.servo1 > 0):
self.servo1 = self.servo1 - 1
self.motor.setServo1(self.servo1)
elif self.decoder.getJoystickVRX2() < 24:
if(self.servo1 < 180):
self.servo1 = self.servo1 + 1
self.motor.setServo1(self.servo1)
if self.decoder.getJoystickPB2() == 0:
self.servo2 = 75
self.motor.setServo2(self.servo2)
elif self.decoder.joystick_VRY2 > 1000:
if(self.servo2 > 0):
self.servo2 = self.servo2 - 1
self.motor.setServo2(self.servo2)
elif self.decoder.getJoystickVRY2() < 24:
if(self.servo2 < 180):
self.servo2 = self.servo2 + 1
self.motor.setServo2(self.servo2)
time.sleep(0.001)
# ... Clean shutdown code here ...
self.xbee.close()
self.motor.close()
print('Thread #%s stopped' % self.ident)
def initJoystick(self):
self.j = Joystick(app.sliderPan, app.sliderTilt, app.sliderPower)
self.j.start()
class App(threading.Thread):
def __init__(self, connection: socket):
threading.Thread.__init__(self)
self.start()
def run(self):
self.canvas = "500x500"
self.labelFont = ('times', 20, 'bold')
self.buttonFont = ('times', 10, 'bold')
self.root = Tk()
self.root.geometry(self.canvas)
self.connection = connection
# Create a frame
self.app = Frame(self.root, bg="white")
self.app.grid()
self.app.focus_set()
self.initGuiElements()
self.initVideoStream(STREAM_HOST)
self.initJoystick()
self.root.mainloop()
def initVideoStream(self, STREAM_HOST):
# Create a label in the frame
self.lmain = Label(self.app)
self.lmain.grid(row=0, column=1)
self.videoStream = VideoStreamThread(self.lmain, STREAM_HOST)
self.videoStream.start()
def initGuiElements(self):
self.sliderTilt = PositionSlider('t',
self.root,
from_=-90,
to=90,
tickinterval=20,
orient=HORIZONTAL,
troughcolor='grey',
length=200)
self.sliderTilt.grid(row=1, column=0)
self.sliderTilt.set(0)
self.sliderPan = PositionSlider('p',
self.root,
from_=90,
to=-30,
tickinterval=5,
orient=VERTICAL,
troughcolor='grey',
length=200,
showvalue=0)
self.sliderPan.grid(row=1, column=1)
self.sliderPan.set(0)
self.sliderPower = PositionSlider('w',
self.root,
from_=100,
to=-100,
tickinterval=20,
orient=VERTICAL,
troughcolor='green',
length=200,
showvalue=0)
self.sliderPower.grid(row=1, column=2)
self.sliderPower.set(0)
self.lblPotent = Label(self.root, text="PotentValue: ")
self.lblPotent.grid(row=2, column=0)
"""buttonReset = Button(root, text='Reset To Origin',
command=self.resetSlider, bg='red', fg='#fff')
buttonReset.config(font=buttonFont)
buttonReset.grid(row=3, column=0)"""
self.root.bind("<Left>", self.sliderTilt.decrement)
self.root.bind("<Right>", self.sliderTilt.increment)
self.root.bind("<Up>", self.sliderPan.increment)
self.root.bind("<Down>", self.sliderPan.decrement)
self.root.bind("<W>", self.sliderPower.increment)
self.root.bind("<S>", self.sliderPower.decrement)
self.root.bind('<KeyRelease-Left>', self.sliderTilt.keyReleased)
self.root.bind('<KeyRelease-Right>', self.sliderTilt.keyReleased)
self.root.bind('<KeyRelease-Up>', self.sliderPan.keyReleased)
self.root.bind('<KeyRelease-Down>', self.sliderPan.keyReleased)
self.root.bind("<KeyRelease-W>", self.sliderPower.keyReleased)
self.root.bind("<KeyRelease-S>", self.sliderPower.keyReleased)
def initJoystick(self):
self.j = Joystick(app.sliderPan, app.sliderTilt, app.sliderPower)
self.j.start()
class SolomonsKey(threading.Thread):
thing = 0
level = None
keys = {}
cam_pos = XYZ(0, 0, 0)
cam_pos_target = XYZ(0, 0, 0)
cam_focus = XYZ(0, 0, 0)
cam_focus_target = XYZ(0, 0, 0)
key_light = XYZ(0, 0, 0)
lastFrameTime = 0
topFPS = 0
camera_sweep = 20
joystick = Joystick()
def animate(self, FPS=30):
currentTime = time()
#key movement for omnicontrol
try:
if self.keys[b"x"]: self.cam_focus.x += 1
if self.keys[b"z"]: self.cam_focus.x -= 1
if self.keys[b"d"]: self.cam_focus.y += 1
if self.keys[b"c"]: self.cam_focus.y -= 1
if self.keys[b"f"]: self.cam_focus.z += 1
if self.keys[b"v"]: self.cam_focus.z -= 1
except:
pass
# run calculations for level inc collision etc
#if not self.level==None: self.level.evaluate(self.joystick,self.keys)
glutPostRedisplay()
glutTimerFunc(int(1000 / FPS), self.animate, FPS)
drawTime = currentTime - self.lastFrameTime
self.topFPS = int(1000 / drawTime)
# set camera target focus points
self.cam_focus_target = XYZ(
self.solomon.solx + 0.2 * self.solomon.sol_dir,
self.solomon.soly - 0.5, 3.5)
# set camera target position
self.cam_pos_target = XYZ(
self.solomon.solx, self.solomon.soly, float(self.level.target_z)
) #XYZ(self.solomon.solx+1*self.solomon.sol_dir, self.solomon.soly-0.2,float(self.level.target_z))
# calculate current focal point and camera position
# self.camera_sweep is the "speed" at which transitions are being made
self.cam_pos = self.cam_pos.add(
self.cam_pos_target.sub(self.cam_pos).mult(1 / self.camera_sweep))
self.cam_focus = self.cam_focus.add(
self.cam_focus_target.sub(self.cam_focus).mult(1 /
self.camera_sweep))
self.lastFrameTime = time()
# if windowed ensure aspect ratio correct
def reshape(self, width, height):
r = float(width) / float(height)
glViewport(0, 0, width, height)
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
gluPerspective(60.0, r, 1., 50.)
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
#glPushMatrix()
def __init__(self):
super().__init__()
def run(self):
print(str(bool(glutInit)))
print("hello and weolcome")
print(
"if you see an error next try the unofficial binaries of pyopengl")
print("initializing glut etc")
glutInit(sys.argv)
glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH)
glutInitWindowSize(640, 480)
glutCreateWindow(name)
print("set blend function")
glBlendFunc(GL_SRC_ALPHA, GL_ONE)
print("set colours and lights")
glClearColor(0., 0., 0., 1.)
glShadeModel(GL_SMOOTH)
glEnable(GL_CULL_FACE)
glEnable(GL_DEPTH_TEST)
glEnable(GL_LIGHTING)
glEnable(GL_NORMALIZE)
print("set light 1")
lightZeroPosition = [0, 0, 00, 1]
glLightfv(GL_LIGHT0, GL_POSITION, lightZeroPosition)
lightZeroColor = [0.2, 0.2, 0.2, 0.5] #green tinged
glEnable(GL_LIGHT0)
glLightfv(GL_LIGHT0, GL_DIFFUSE, lightZeroColor)
glLightf(GL_LIGHT0, GL_CONSTANT_ATTENUATION, 0.025)
glLightf(GL_LIGHT0, GL_LINEAR_ATTENUATION, 0.0025)
#print ("set light 2")
#lightZeroPosition2 = [0,1,0]
#glLightfv(GL_LIGHT1, GL_POSITION, lightZeroPosition2)
#lightZeroColor2 = [1,1,1,1.0] #green tinged
#glEnable(GL_LIGHT1)
#glLightfv(GL_LIGHT1, GL_DIFFUSE, lightZeroColor2)
#glLightf(GL_LIGHT1, GL_CONSTANT_ATTENUATION, 0.2)
#glLightf(GL_LIGHT1, GL_LINEAR_ATTENUATION, 0.05)
#print ("set light 3")
##self.key_light.z=5
#self.key_light = XYZ(10,0,15)
#glLightfv(GL_LIGHT2, GL_POSITION, [self.key_light.x,self.key_light.y,self.key_light.z,0.01])
#glEnable(GL_LIGHT2)
#glLightfv(GL_LIGHT2, GL_DIFFUSE, [0.8,0.8,0.8,1.0])
#glLightfv(GL_LIGHT2, GL_AMBIENT, [0.5,0.5,0.5,1.0])
#glLightfv(GL_LIGHT2, GL_SPECULAR, [1.0,1.0,1.0,1.0])
#glLightf(GL_LIGHT2, GL_CONSTANT_ATTENUATION, 0.01)
#glLightf(GL_LIGHT2, GL_LINEAR_ATTENUATION, 0.01)
#
#initialization of letters
print("initialzing letters")
self.letters = Letters.Letters()
#for game models
print("making model lists")
MakeLists()
print("ignore key repeat")
glutIgnoreKeyRepeat(1)
print("attach glut events to functions")
glutSpecialFunc(self.keydownevent)
glutSpecialUpFunc(self.keyupevent)
glutReshapeFunc(self.reshape)
glutKeyboardFunc(self.keydownevent)
glutKeyboardUpFunc(self.keyupevent)
glutDisplayFunc(self.display)
#glutIdleFunc(self.display)
print("initial projection")
glMatrixMode(GL_PROJECTION)
gluPerspective(60.0, 640.0 / 480., 1., 50.)
glMatrixMode(GL_MODELVIEW)
glPushMatrix()
print("generating level")
self.level = Level.generateLevel(0)
self.solomon = Solomon(self.level.solomon_start)
print("keys set up")
self.initkey("zxdcfvqaopm")
self.animate()
print("about to loop...")
glutMainLoop()
return
def initkey(self, cl):
for c in cl:
self.keydownevent(str(c.lower()).encode(), 0, 0)
self.keyupevent(str(c.lower()).encode(), 0, 0)
print("end initkey")
def display(self):
glLoadIdentity()
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
gluLookAt(self.cam_pos.x, self.cam_pos.y, self.cam_pos.z,
self.cam_focus.x, self.cam_focus.y, self.cam_focus.z, 0, 1,
0)
glScale(0.9, 0.9, 0.9)
if False:
xx = (self.thing % 100) / 100
print("set light 2 {}".format(xx))
#glDisable(GL_LIGHT1)
lightZeroPosition2 = [0, -1, 0]
glLightfv(GL_LIGHT1, GL_POSITION, lightZeroPosition2)
lightZeroColor2 = [1, 1, 1, 1.0] #green tinged
#glLightf(GL_LIGHT1, GL_CONSTANT_ATTENUATION, xx*xx)
#glLightf(GL_LIGHT1, GL_LINEAR_ATTENUATION, xx*xx)
glLightfv(GL_LIGHT1, GL_DIFFUSE, lightZeroColor2)
#glEnable(GL_LIGHT1)
if False:
if self.thing % 100 > 50:
glEnable(GL_LIGHT1)
pass
else:
glDisable(GL_LIGHT1)
if self.thing % 50 > 25:
glEnable(GL_LIGHT0)
pass
else:
glDisable(GL_LIGHT0)
self.level.draw()
for b in self.level.bursts:
print("doing burst {}".format(b))
if b.draw():
print("burst complete {}".format(b))
self.level.bursts.remove(b)
gogogo(self.level, self.solomon, self.keys)
#glLightfv(GL_LIGHT2, GL_POSITION, [self.solomon.solx,self.solomon.soly,10,0.01])
#glLightfv(GL_LIGHT2, GL_POSITION, [0,0,0.7,0.01])
self.solomon.draw()
if self.solomon.sol_walking == 1: self.solomon.AG_walk.do()
self.thing += 1
###glDisable(GL_LIGHT0)
#if debug==True:
if False:
glLoadIdentity()
gluLookAt(0, -0.5, 2.5, 0, 0, 0, 0, 1, 0)
wdth = 0.2
joystick_actions = [
x for x in dir(self.joystick) if x[0:2] == "is"
]
glTranslate(0.0 - (len(joystick_actions) - 1) * 2 * wdth / 2.0,
-1.0, 0)
for k in joystick_actions:
#print(k)
col = "red"
if getattr(self.joystick, k)(self.keys): col = "green"
glMaterialfv(GL_FRONT, GL_DIFFUSE, colours[col])
glutSolidCube(2 * wdth - 0.02)
glTranslate(2 * wdth, 0, 0)
glPushMatrix()
#glLoadIdentity()
glScale(0.007, 0.01, -0.01)
glTranslate(-75, 0, -20)
#glTranslate(-180,-70,0)
glTranslate(-2 * wdth, 0, 0)
self.letters.drawString(k[2:5])
glPopMatrix()
##glLoadIdentity()
##
##gluLookAt(0, 0, 2.5,
## 0, 0, 0 ,
## 0, 1, 0 )
##
##glScale(0.01,0.01,-0.01)
##glTranslate(-180,-70,0)
#if debug==True:
##if True:
## self.letters.drawString('X:'+str(self.solomon.solx)+' Y:'+str(self.solomon.soly))
## glTranslate(0,0-15,0)
## gx,gy=int(self.solomon.solx),int(self.solomon.soly)
## self.letters.drawString('G {0} on: {1} below: {2}'.format(str((gx,gy)),self.level.grid[gy][gx],self.level.grid[gy-1][gx]))
## glTranslate(0,0-15,0)
## self.letters.drawString('')
glutSwapBuffers()
def keydownevent(self, c, x, y):
#print ("*******************************************************************************")
try:
self.keys[c] = True
if self.joystick.callback_down is not None:
self.joystick.callback_down(c)
#print(self.keys)
if self.joystick.isFire(self.keys):
Logic.event(self.keys, c, self.solomon)
except Exception as e:
print(e)
pass
glutPostRedisplay()
def keyupevent(self, c, x, y):
try:
if c in self.keys: self.keys[c] = False
if self.joystick.callback_up is not None:
self.joystick.callback_up(c)
#print(self.keys)
except Exception as e:
print(e)
pass
glutPostRedisplay()
class JoystickTest(unittest.TestCase):
def setUp(self):
'''
Verify environment is setup properly.
'''
self.joystick = Joystick()
j_list = self.joystick.get_joystick_list()
self.joystick.connect(j_list[0])
def tearDown(self):
'''
Verify environment is tore down properly.
'''
self.joystick.disconnect()
pass
def test_up(self):
'''
Verify that the joystick up movement is working without
problems.
'''
self.joystick.clear_commmands()
value = False
while not value:
print "Move up using the joystick."
j_command = self.joystick.get_commands()
value = j_command["UP"]
self.assertTrue(value)
def test_down(self):
'''
Verify that the joystick down movement is working without
problems.
'''
self.joystick.clear_commmands()
value = False
while not value:
print "Move down using the joystick."
j_command = self.joystick.get_commands()
value = j_command["DOWN"]
self.assertTrue(value)
def test_left(self):
'''
Verify that the joystick left movement is working without
problems.
'''
self.joystick.clear_commmands()
value = False
while not value:
print "Move left using the joystick."
j_command = self.joystick.get_commands()
value = j_command["LEFT"]
self.assertTrue(value)
def test_right(self):
'''
Verify that the joystick right movement is working without
problems.
'''
self.joystick.clear_commmands()
value = False
while not value:
print "Move right using the joystick."
j_command = self.joystick.get_commands()
value = j_command["RIGHT"]
self.assertTrue(value)
def test_speed(self):
'''
Verify that the joystick speed command is working without
problems.
'''
self.joystick.clear_commmands()
value = False
while not value:
print "Speed up using the joystick."
j_command = self.joystick.get_commands()
value = j_command["SPEED"]
self.assertTrue(value)
def test_break(self):
'''
Verify that the joystick break command is working without
problems.
'''
self.joystick.clear_commmands()
value = False
while not value:
print "break using the joystick."
j_command = self.joystick.get_commands()
value = j_command["BREAK"]
self.assertTrue(value)
print('start')
pioneer_mini.arm() # запуск моторов
pioneer_mini.takeoff() # предстартовые проверки
pygame.init() # Иницилизация пугейм
pygame.key.set_repeat(1, 20) # Включение обработки зажатой клавиши
sc = pygame.display.set_mode((W, H))
clock = pygame.time.Clock()
##############################
"""Инициализация джойстиков"""
##############################
joy = Joystick()
if __name__ == '__main__':
running = True
while running:
# обработка остановочных нажатий
for event in pygame.event.get():
if event.type == pygame.QUIT:
running = False
elif event.type == pygame.KEYDOWN:
if event.key == pygame.K_ESCAPE:
running = False
print "%s pressed" % (jdev.event_number)
else:
print "%s released" % (jdev.event_number)
elif jdev.event_type & 0x02:
# joystick moved #define JS_EVENT_AXIS 0x02
print("axis moved")
print"number: " + str(jdev.event_number) + ", value: " + str(jdev.event_value)
else:
print("unknown event: %s" % jdev.event_type)
# list available devices
print('available devices: ')
for fn in os.listdir('/dev/input'):
if fn.startswith('js'):
path = '/dev/input/' + fn
jdev = Joystick(path)
jdev.initialize()
if jdev.deviceName == 'Generic X-Box pad':
print('correct device: %s' % jdev.deviceName)
break
else:
print('incorrect device: %s' %jdev.deviceName)
jdev.close()
count = 0
while count < 30:
jdev.read_event()
if jdev.newData:
processEvent(jdev);
count = count + 1
