def scenario1(proxy):
car = tcnp.init()
print('car')
try:
joy = xbox.Joystick()
print('joy')
joy.connected()
print(joy)
except Exception as e:
print(e)
joy = xbox.Joystick()
mapping_flag = True
msgb = ""
while mapping_flag:
try:
#print('try')
tcnx.xbox_velocity_vision(joy, car)
#print('xbox')
#time.sleep(0.1)
pose_resp = proxy.get_pose()
status_resp = proxy.get_status()
msgb = show_info(proxy, msgb)
#msg1 = "status:" + format(status_resp[0]) + ", "
#msg2 = "mapid:" + format(pose_resp[1]) + ", "
#msg3 = "(x,y):(" + format(pose_resp[2]) + "," + format(pose_resp[3]) + "), "
#msg4 = "thida: " + format(pose_resp[4]) + ", "
#msg5 = "conf: " + format(pose_resp[5])
#msg6 = " ############################### "
#if status_resp[0]==5:
# msga = msg1 + msg2 + msg3 + msg4 + msg5 + msg6
#else:
# msga = msg1 + msg2 + msg3 + msg4 + msg5
#if msgb!=msga:
# print(msga)
# msgb=msga
# else:
# print(msga)
if joy.Back():
mapping_flag = False
print(joy.Back())
joy.close()
tcng.relay_off()
except KeyboardInterrupt:
joy.close()
tcng.relay_off()
#interrupt(car,joy,proxy)
break
except Exception as e:
tcng.relay_off
joy.close()
print(e)
def main():
arduino_bt_library.setup_bl(False)
bluetooth = arduino_bt_library.get_bluetooth()
#arduino = arduino_bt_library.get_arduino()
if use_xbox:
controller = xbox.Joystick()
prev_l_x = 0
prev_l_y = 0
prev_r_x = 0
print_cols()
while (1):
try:
string = arduino_bt_library.get_string_bl()
print("\r" + string, end="")
to_send = ""
send = False
if use_xbox:
l_x = round(controller.leftX() * 4 + 4)
l_y = round(controller.leftY() * 4 + 4)
r_x = round(controller.rightX() * -90 + 90)
if l_x != prev_l_x:
send = True
prev_l_x = l_x
if l_y != prev_l_y:
send = True
prev_l_y = l_y
if True:
to_send = "v" + str(l_y) + str(l_x) + ";"
to_send += "c" + str(r_x) + ";"
#print("\r" + to_send, end="")
bluetooth.write(to_send.encode())
# if to_send != "":
if False:
s = "v" + str(l_x) + str(l_y) + ";"
print(s)
bluetooth.write(s.encode())
pass
to_send = ""
# if (string != ''):
# print(string)
# print("-")
except KeyboardInterrupt:
break
print("\nUscita")
bluetooth.close()
if use_xbox:
controller.close()
try:
arduino.close()
except:
pass
return
def steering(self):
joy = xbox.Joystick()
while True:
if joy.leftX():
#an dem Wert fr die Lenkung spielen.. was macht sinn wo laesst sich das auto gut steuern
x_value = joy.leftX()
print("X-Axis: {}".format(x_value))
if x_value > 0:
#Powering steering right
angle_param = int(80 * x_value +
410) # steeringfunction: 90x+410
self.steer_control(angle_param)
if x_value < 0:
#Powering steering
angle_param = int(80 * x_value +
410) # steeringfunction: 90x+410
self.steer_control(angle_param)
if x_value == 0:
self.steer_control(410)
if joy.A():
print("Paused!")
self.steer_control(410)
if joy.B():
print("B pressed! Goodbye!")
joy.close()
break
def main():
atexit.register(killall)
args = parse_args()
setup_logging(args)
# read tool config
config = DFSConfig()
# setup show
show = Show(config, args.show_name)
if not show:
print('no such show %s', args.show_name)
sys.exit(1)
# setup stage
stage = Stage(show, args.stage_name)
if not stage:
print('could not load or create stage %s', args.stage_name)
sys.exit(1)
# setup joystick
joy = xbox.Joystick(debug=args.debug, config=config.joystick)
if args.check_mode:
sys.exit(0)
handler = DmxHandler(config, show, stage, joy)
# setup data handler, this is our callback loop
# as DMX data comes in constantly
wrapper = ClientWrapper()
rx = wrapper.Client()
rx.RegisterUniverse(config.input.universe, rx.REGISTER, handler.handle)
wrapper.Run()
def sample_first_joystick(dt):
xbl = xbox.Joystick()
start = time.time()
#press B to exit
while not xbl.Back():
now = time.time() - start
xboxInput = {
keys[0]: fmtFloat(xbl.leftX()),
keys[1]: fmtFloat(xbl.leftY()),
keys[2]: fmtFloat(xbl.rightX()),
keys[3]: fmtFloat(xbl.rightY()),
keys[4]: xbl.A(),
keys[5]: xbl.B(),
keys[6]: xbl.X(),
keys[7]: xbl.Y(),
keys[8]: xbl.dpadUp(),
keys[9]: xbl.dpadDown(),
keys[10]: xbl.dpadLeft(),
keys[11]: xbl.dpadRight(),
keys[12]: xbl.leftBumper(),
keys[13]: xbl.rightBumper(),
keys[14]: fmtFloat(xbl.leftTrigger()),
keys[15]: fmtFloat(xbl.rightTrigger()),
keys[16]: xbl.Back(),
keys[17]: xbl.Guide(),
keys[18]: xbl.Start(),
keys[19]: fmtFloat(now)
}
xboxq.put(xboxInput)
time.sleep(dt)
def scenario2(proxy):
joy = xbox.Joystick()
car = tcnp.init('/dev/ttyUSB1')
mainflag = True
while mainflag:
try:
runflag = True
runflagx = True
runflagy = True
while runflagx:
try:
tx = int(raw_input('Target X'))
runflagx = False
except KeyboardInterrupt:
runflagy = False
runflag = False
mainflag = False
tcng.relay_off()
print('Proxy.reset(), response: {}'.format(proxy.reset()))
break
except ValueError:
print('You must enter numbers')
except Exception as e:
print(e)
tcng.relay_off()
while runflagy:
try:
ty = int(raw_input('Target Y'))
runflagy = False
except KeyboardInterrupt:
runflag = False
mainflag = False
tcng.relay_off()
print('Proxy.reset(), response: {}'.format(proxy.reset()))
break
except ValueError:
print('You must enter numbers')
except Exception as e:
print(e)
tcng.relay_off()
vision_to_target_position(car, joy, tx, ty)
except KeyboardInterrupt:
#car[0] = 0
#car[1] = 0
#car[2] = 0
#mainflag = False
#tcnp.move_to_coordinate(car)
#tcng.relay_off()
#print( 'Proxy.reset(), response: {}'.format(proxy.reset()) )
interrupt(car, joy, proxy)
break
except Exception as e:
mainflag = False
print(e)
tcng.relay_off()
print('Proxy.reset(), response: {}'.format(proxy.reset()))
def gamepad_thread():
prev = 0
frame_rate = 15
TCP_IP = '192.168.0.20'
TCP_IP_LOCAL = '127.0.0.1'
TCP_PORT_GAMEPAD = 9997
kp = 1.5
max_value = 100
sock_gamepad = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock_gamepad.connect((TCP_IP, TCP_PORT_GAMEPAD))
sock_local = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock_local.bind((TCP_IP_LOCAL, 1924))
sock_local.listen(1)
conn_local, addr = sock_local.accept()
print('Connected: ', addr)
u_last = ''
u_new = ''
joy = xbox.Joystick()
print('Gamepad start')
while 1:
buff = getDataFromGamepad(joy)
u_new = read_until(conn_local)
if (not u_new):
u_new = u_last
print('U: ', u_new)
buff.append(int(u_new))
u_last = u_new
time_elapsed = time.time() - prev
if (time_elapsed > 1./frame_rate):
prev = time.time()
print(buff)
sock_gamepad.send(bytearray(buff))
def run_control():
joy = xbox.Joystick()
toggleon = False
while joy.Guide() != 1:
if joy.Start() == 1:
toggleon = not toggleon
motor.writeBlock([0, 0, 0, 0, 0, 0])
time.sleep(1)
if toggleon:
motor_one_dir, motor_two_dir, motor_three_dir = 1, 1, 1
if joy.leftX() > 0:
motor_one_dir = 0
if joy.leftY() > 0:
motor_two_dir = 0
if joy.rightY() > 0:
motor_three_dir = 0
motor_speeds = [
scale_number(joy.leftX()),
scale_number(joy.leftY()),
scale_number(joy.rightY())
]
motor.writeBlock([
motor_one_dir, motor_speeds[0], motor_two_dir, motor_speeds[1],
motor_three_dir, motor_speeds[2]
])
def publisher():
"""Publishes /left and /right based on joystick input
The left and right triggers are treated as enables for the respective motors"""
rospy.init_node('manual_control', log_level=rospy.INFO)
joystick = None
for _ in range(3):
try:
joystick = xbox.Joystick()
except IOError:
rospy.logerr(
"Failed to connect to xbox controller. Trying agin in 10 seconds..."
)
rospy.sleep(10.)
if not joystick:
rospy.logfatal("Xbox controller not found. Shutting down...")
rospy.signal_shutdown("Couldn't connect to xbox controller")
rospy.loginfo("Joystick found")
#TODO measure speed going straght and constrain to 5mph
max_speed = .5
left = rospy.Publisher('left', Float32, queue_size=10)
right = rospy.Publisher('right', Float32, queue_size=10)
#TODO: test other rates and find a balance between bandwidth usage and fine control
rate = rospy.Rate(10)
while not rospy.is_shutdown():
left_speed, right_speed = joy2speed(joystick, True)
left_speed *= max_speed * joystick.leftTrigger()
right_speed *= max_speed * joystick.rightTrigger()
left.publish(left_speed)
right.publish(right_speed)
rate.sleep()
def __init__(self, server_ip, server_port):
logging.basicConfig(level=logging.DEBUG)
self.server_ip = server_ip
self.server_port = server_port
self._joy = xbox.Joystick()
self._joy_dict = {}
self._sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
def main_xbox_velocity_lidar():
joy = xbox.Joystick()
car = tcn.init('/dev/ttyUSB1')
sock = tcns.init_server_udp()
try:
g.init()
g.relay_on()
# Loop until back button is pressed
while True:
command = tcns.listen_udp(sock)
if command != None:
tcnv.lidar_evade_velocity(car, command)
else:
x = 100 * round(joy.leftX(), 2)
y = 100 * round(joy.leftY(), 2)
z = 100 * round(joy.rightX(), 2)
a = int(joy.A())
b = int(joy.B())
#print('{} {} {}'.format(x,y,z))
car = joy_stick_control_v(car, a, b, x, y, -z)
except Exception as e:
g.relay_off()
joy.close()
print(e)
except KeyboardInterrupt:
g.relay_off()
joy.close()
def main_xbox_velocity():
joy = xbox.Joystick() # joy represent xbox object
car = tcn.init('/dev/ttyUSB1')
try:
g.init()
g.relay_on() # turn on the power of STM32
# Loop until back button is pressed
while not joy.Back():
try:
x = 100 * round(
joy.leftX(), 2
) # The number from joystick is float , since joy_stick_control_v
y = 100 * round(
joy.leftY(), 2
) # require int input , we convert it into integer through round it to 2 decimals
z = 100 * round(joy.rightX(), 2) # and multiply by 100
a = int(joy.A())
b = int(joy.B())
#print('{} {} {}'.format(x,y,z))
car = joy_stick_control_v(car, a, b, x, y, -z)
except:
g.relay_off()
except:
g.relay_off()
joy.close()
def __init__(self):
self.snap = False
if len(sys.argv) == 2:
self.delay = float(sys.argv[1])
self.snap = True
else:
print("No Snaps, specify a delay value (float) to activate")
self.label = [-1, 2]
#set controls
self.pwm = Adafruit_PCA9685.PCA9685()
self.pwm.set_pwm_freq(50)
# Init speed
self.speed = SPEED_NORMAL
# Init direction
self.direction = DIR_C
# Setup Camera
self.camera = PiCamera()
self.camera.resolution = IM_SIZE
self.camera.framerate = 60
self.rawCapture = PiRGBArray(self.camera, size = IM_SIZE)
time.sleep(0.5)
self.joy = xbox.Joystick()
def main():
import argparse
parser = argparse.ArgumentParser(description="Water Linked Modem example")
parser.add_argument('-D',
'--device',
action="store",
required=True,
type=str,
help="Serial port.")
parser.add_argument('-r',
'--role',
action="store",
type=str,
default="a",
help="Role: a or b.")
parser.add_argument('-c',
'--channel',
action="store",
type=int,
default=4,
help="Channel: 1-7.")
args = parser.parse_args()
ch = args.channel
if ch < 1 or ch > 7:
print("Error: invalid channel: {}".format(ch))
sys.exit(1)
role = args.role
if role not in ["a", "b"]:
print("Error: invalid role: {}".format(role))
sys.exit(1)
joy = xbox.Joystick()
modem = WlModem(args.device)
if not modem.connect():
print("Error connecting to modem")
sys.exit(1)
print("Set modem role ({}) and channel ({}): ".format(role, ch), end="")
success = modem.cmd_configure(role, ch)
if success:
print("success")
else:
print("failed")
sys.exit(1)
success = modem.cmd_flush_queue()
print("Modem connected")
try:
run(joy, modem)
except KeyboardInterrupt:
pass
finally:
joy.close()
def __init__(self):
try:
self.step = 0
self.joy = xbox.Joystick()
except IOError:
print('Xbox connect error, auto retry again.')
self.__init__()
except:
traceback.print_exc()
def main():
changeSpeed()
joy = xbox.Joystick()
last_trig = False
last_x = 0
data = ''
start_time = time.time()
angle = 125
last_is_home = False
last_start = 0
cam_state = 0
last_t = -1
while True:
t = joy.rightTrigger()
if (last_t != t):
last_t = t
val = int(normalize_label(t, 0.0, 1.0, 40.0, 102.0))
msg = "speed" + str(val)
send_data(tcpCliSock, msg)
x = joy.rightX()
cur_trig = t > 0
s = joy.Start()
if (s != last_start):
last_start = s
if (s == 1):
if (cam_state == 0):
cam_state = 1
print 'Start'
send_data(tcpCliSock, 'Start')
else:
cam_state = 0
print 'Stop'
send_data(tcpCliSock, 'Stop')
if (not (last_trig == cur_trig)):
last_trig = cur_trig
if (cur_trig == 0.0):
send_data(tcpCliSock, 'stop ')
if (cur_trig > 0):
send_data_angle(tcpCliSock, 'forward ', str(angle), x,
start_time)
elif (not (x == last_x)):
last_x = x
if (x > -0.03 and x < 0.03):
if not (last_is_home):
start_time = send_data_angle(tcpCliSock, 'home', '180', x,
start_time)
last_is_home = True
else:
last_is_home = False
angle = ((x / 2) + 0.5) * 170 + 35
angle = int(angle)
str_angle = str(angle)
data = 'turn=' + str_angle
start_time = send_data_angle(tcpCliSock, data, str_angle, x,
start_time)
def _xBoxConnect(self):
'''
Initializes a listener for the Xbox controller
'''
self.joy = xbox.Joystick()
print("Waiting on Xbox connect")
while not self.joy.connected():
time.sleep(1)
print("Accepted connection from Xbox controller", self.joy.connected())
def connectJoystick(self):
if self.joy == None:
try:
self.joy = xbox.Joystick()
except Exception as ex:
self.joy = None
self.putstring(ex)
self.putstring('\n')
return
def getController(i=0):
try:
return xbox.Joystick()
except:
i = i + 1
sys.stdout.write(
"\nError setting up to controller. Retrying... (Retry #" + str(i) +
")\n")
sleep(0.2)
return getController(i + 1)
def calibrate_controller(self):
# Remove the xpad module that interferes with xboxdrv.
#os.system('sudo rmmod xpad 2> /dev/null')
# Construct joystick and check that the driver/controller are working.
self.joy = xbox.Joystick()
# Check that the xbox controller is connected.
print("Press the back button to calibrate the controller...")
while not self.joy.Back():
pass
print("Controller calibrated.\n")
def init_controller():
lamp = Lamp(pin=25)
while True:
try:
joy = xbox.Joystick()
lamp.on()
return joy
except Exception as e:
print(e)
lamp.on()
time.sleep(0.5)
lamp.off()
time.sleep(0.5)
class controller():
def getVals():
v = 100 * joy.leftY()
turn = 10 * joy.rightX()
if turn > 1: r = (turn * (-23)) + 250
if -1 < turn < 1: r = 250
if turn < -1: r = (turn * (-23)) - 250
return v, r
joy.close()
if __name__ == "__main__":
joy = xbox.Joystick()
def xboxControl(self):
joy = xbox.Joystick()
while not joy.Back():
# button A to stop
if joy.A():
self.motor.stop()
else:
# left joystick to move
x, y = joy.leftStick()
self.moveByAxis(x, y)
joy.close()
def steering(self):
joy = xbox.Joystick()
while True:
if joy.leftStick():
# Transforming input x,y in acc and angle parameter - steeringfunction : angle_parm = 90*x+410
# steering min 320 steering max = 510
x, y = joy.leftStick()
acceleration = 100 * y * abs(y)
angle_param = int(80 * x + 410)
self.control_order = (acceleration, angle_param)
print("Y-Axis: {}".format(acceleration))
# Forward - Right
if acceleration > 0 and angle_param > 410:
self.RearMotors(1, 0, acceleration, 1, 0, acceleration)
self.steer_control(angle_param)
# Forward - Left
if acceleration > 0 and angle_param < 410:
self.RearMotors(1, 0, acceleration, 1, 0, acceleration)
self.steer_control(angle_param)
# Backward - Right
if acceleration < 0 and angle_param > 410:
#Powering backwards attention power hast to be positiv _ output is negative-> *-1
self.RearMotors(0, 1, -1 * acceleration, 0, 1,
-1 * acceleration)
self.steer_control(angle_param)
# Backward - Right
if acceleration < 0 and angle_param < 410:
#Powering backwards attention power hast to be positiv _ output is negative-> *-1
self.RearMotors(0, 1, -1 * acceleration, 0, 1,
-1 * acceleration)
self.steer_control(angle_param)
if acceleration == 0:
self.RearMotors(0, 0, 0, 0, 0, 0)
if joy.A():
print("Paused!")
self.RearMotors(0, 0, 0, 0, 0, 0)
self.steer_control(410)
if joy.B():
print("B pressed! Goodbye!")
#set steering to netral and closing everything
self.steer_control(410)
self.control_sequence = False
joy.close()
GPIO.cleanup()
return False
def run(ch, distance):
# Instantiate the controller
joy = xbox.Joystick()
while not joy.Back():
global voice_data
global lidar_data
time.sleep(0.1)
#print (voice_data)
degree = 0
x = 0
y = 0
print("in run")
if joy.rightTrigger() > 0:
degree = 1
if joy.leftTrigger() > 0:
degree = -1
x = joy.leftX()
y = joy.leftY()
if ch == 'x':
x = 0
y = 0
if ch == 'w':
x = -1
y = -1
if ch == 's':
x = 1
y = 1
if ch == 'a':
x = 1
y = -1
if ch == 'd':
x = -1
y = 1
if ch == 'h':
head_command(1)
time.sleep(1)
head_command(-1)
time.sleep(1)
head_command(0)
print("lidar in run")
print(lidar_data)
if int(lidar_data) == 1:
print("stop")
x = 0
y = 0
motor_command(x, y)
head_command(degree)
def main_xbox_position():
joy = xbox.Joystick()
car = tcn.init('/dev/ttyUSB1')
# Loop until back button is pressed
while not joy.Back():
#print(start)
x = 100 * round(joy.leftX(), 2)
y = 100 * round(joy.leftY(), 2)
z = 100 * round(joy.rightX(), 2)
a = int(joy.A())
b = int(joy.B())
car = joy_stick_control_2(car, a, b, x, y, -z)
joy.close()
def start():
# print('sdfghjsdfghj\n\n\n\nasdgfhjk')
rospy.init_node('joy_fucking_stick')
rate = rospy.Rate(30)
command_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=30)
joy = xbox.Joystick()
# x,y = 0,1
drive_command = Twist()
while not rospy.is_shutdown():
x = joy.leftX()
y = joy.leftY()
# print 'x: ', x, 'y: ', y
drive_command.linear.x = y
drive_command.angular.z = -x * sign(y)
command_publisher.publish(drive_command)
rate.sleep()
def __init__(self):
# Attempt To Connect To I2C Adafruit 9685 Board
try:
import board
print('I2C Board Connection Established.')
self.i2c = busio.I2C(board.SCL, board.SDA)
print('I2C Board Connection Persist Complete.')
self.ok_to_run = False
self.DetectI2CModule(
) # Read I2C Pins To Validate Active Connection Before Continuing
if self.ok_to_run is True:
print('I2C Board Valid Controller Detected.')
# Create Instance Of PCA Controller
self.pwm = Adafruit_PCA9685.PCA9685()
print('PCA Object Created.')
self.pwm.set_pwm_freq(60)
print('PWM Frequency Set.')
except NotImplementedError:
print(
'Board Not Detected. Unable To Create Controller I2C Connection.'
)
print('Check I2C Is Enabled Using The Terminal.')
print('sudo raspi-config.')
# Detect XBox Controller Plugin
try:
self.joy = xbox.Joystick()
self.enable_joy = True
print('XBox Controller Detected & Enabled')
except OSError:
self.joy = None
self.enable_joy = False
print(
'No XBox Controller Detected. Controller Fucntion Not Enabled.'
)
print('Reboot Controller To Retry Connection')
print('Local Clock Update Frequency Set')
self.clock = 0.05 # Set Internal Clock For Localized Update Speeds
print('Robot Controller Driver Connected')
self.robot = SainSmartRobot.SixAxisRobot(self)
print('Running Robot...')
self.RunRobot()
def __init__(self):
self.NebSpeed = 1
self.ultraBackRightSide = Ultra(18, 27)
self.ultraBackRight = Ultra(4, 17)
self.ultraFrontRight = Ultra(22, 23)
self.ultraFrontRightSide = Ultra(25, 5)
self.ultraBackLeftSide = Ultra(26, 21)
self.ultraBackLeft = Ultra(20, 19)
self.ultraFrontLeft = Ultra(6, 16)
self.ultraFrontLeftSide = Ultra(13, 12)
self.ZB = ThunderBorg3.ThunderBorg()
self.ZB.Init()
self.reverse = 0
print(self.ZB.GetBatteryReading())
self.leftSpeed = 0
self.rightSpeed = 0
print("waiting for remote")
while True:
try:
self.controller = xbox.Joystick()
break
#self.controller.startstart()
except:
time.sleep(0.5)
# remove this just to not have to have remote
self.camera = Camera()
print("waiting for compass")
self.compass = Compass()
self.compass.Start() #remove if no compass
print("blasting sensors")
for i in range(1000):
print(i)
self.simRead([
self.ultraFrontRight,
self.ultraFrontLeft,
self.ultraBackRight,
self.ultraBackLeft,
self.ultraFrontRightSide,
self.ultraFrontLeftSide,
self.ultraBackRightSide,
self.ultraBackLeftSide,
])
self.compass.Heading()
def run(distance):
# Instantiate the controller
joy = xbox.Joystick()
while not joy.Back():
time.sleep(0.1)
degree = 0
x = 0
y = 0
if joy.rightTrigger() > 0:
degree = 1
if joy.leftTrigger() > 0:
degree = -1
x = joy.leftX()
y = joy.leftY()
print(x)
print(y)
print("x y values")
motor_command(x, y)
head_command(degree)
