-
Notifications
You must be signed in to change notification settings - Fork 1
/
ctrl_cmd.py
46 lines (40 loc) · 1.68 KB
/
ctrl_cmd.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
# -*- coding: utf-8 -*-
try:
import vrep
except:
print ('--------------------------------------------------------------')
print ('"vrep.py" could not be imported. This means very probably that')
print ('either "vrep.py" or the remoteApi library could not be found.')
print ('Make sure both are in the same folder as this file,')
print ('or appropriately adjust the file "vrep.py"')
print ('--------------------------------------------------------------')
print ('')
import sys
from utils.quadpad import quadCTRL
#from quadstick import PS3 as Controller
initialCall=True
print ('VREP Simulation Program')
vrep.simxFinish(-1) # just in case, close all opened connections
clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to V-REP
if clientID!=-1:
print ('Connected to remote API server')
joystick = quadCTRL()
# Object handle
# _,Quadbase=vrep.simxGetObjectHandle(clientID,'Quadricopter_base',vrep.simx_opmode_oneshot_wait)
else:
print('Connection Failure')
sys.exit('Abort Connection')
while True:
# Code for testing...
if initialCall:
mode = vrep.simx_opmode_streaming
initialCall = False
else:
mode = vrep.simx_opmode_buffer
# Poll controller
demands = joystick.DetectAction()
print("R: " + str(demands[0]) + " " +str(demands[1]) + " " +
"L: " + str(demands[2]) + " " +str(demands[3]))
demandString=vrep.simxPackFloats(demands)
vrep.simxSetStringSignal(clientID,'demandstring',demandString,vrep.simx_opmode_oneshot)
print('EOS')