def _connect_controller(self):
try:
self.xbox_controller = Xbox360Controller(index=1,
axis_threshold=0.05)
except Exception as e: # this library emits a stupid general exception that makes it difficult to retry
try:
self.xbox_controller = Xbox360Controller(index=2,
axis_threshold=0.05)
except Exception as e:
self.xbox_controller = Xbox360Controller(index=0,
axis_threshold=0.05)
def _connect_controller(self):
try:
xbox_controller = Xbox360Controller(index=1, axis_threshold=0.05)
except Exception as e: # this library emits a stupid general exception that makes it difficult to retry
try:
xbox_controller = Xbox360Controller(index=2,
axis_threshold=0.05)
except Exception as e:
xbox_controller = Xbox360Controller(index=0,
axis_threshold=0.05)
xbox_controller.hat.when_moved = self._d_pad
xbox_controller.button_start.when_pressed = self._start
return xbox_controller
def establish_controller(index, logger, light_sender, gui):
try:
controller = Xbox360Controller(index=index)
logger.info(controller.name)
except Exception:
logger.error(
f'Encountered exception initiating controller at index {index}',
exc_info=True)
return
if 'Drum' in controller.name:
light_sender.add_controller(
'drums',
Drums(controller, logger, 'drums', (0, 0, 0), light_sender, gui),
1)
elif 'WingMan' in controller.name:
controller.axis_threshold = 0.05
light_sender.add_controller(
'car', Car(controller, logger, 'car', (0, 0, 0), light_sender,
gui), 2)
elif 'Guitar' in controller.name:
light_sender.add_controller(
'guitar1',
Guitar(controller, logger, 'guitar1', (0, 0, 0), light_sender,
gui), 3)
else:
logger.error(
f'Unrecognized controller found with name {controller.name}. Skipping it.'
)
controller.close()
def HandleInput(self):
try:
self.isActive = True
with Xbox360Controller(0, axis_threshold=0.0) as controller:
# Button A events
controller.button_a.when_pressed = self.on_button_pressed
controller.button_a.when_released = self.on_button_released
# Button B events
controller.button_b.when_pressed = self.on_button_pressed
controller.button_b.when_released = self.on_button_released
# Button Y events
controller.button_y.when_pressed = self.on_button_pressed
controller.button_y.when_released = self.on_button_released
# Button X events
controller.button_x.when_pressed = self.on_button_pressed
controller.button_x.when_released = self.on_button_released
# Button Trigger L events
controller.button_trigger_l.when_pressed = self.on_button_pressed
# Button Trigger R events
controller.button_trigger_r.when_pressed = self.on_button_pressed
# Button Thumb L events
controller.button_thumb_l.when_pressed = self.on_button_pressed
# Button Thumb R events
controller.button_thumb_r.when_pressed = self.on_button_pressed
# Start Button events
controller.button_start.when_pressed = self.on_button_pressed
# Left and right axis move event
controller.axis_l.when_moved = self.on_axis_moved
controller.axis_r.when_moved = self.on_axis_moved
signal.pause()
except KeyboardInterrupt:
self.isActive = False
pass
def main():
try:
configure_gpIO()
except Exception as e:
print(e)
try:
with Xbox360Controller(0, axis_threshold=0.2) as controller:
# Button A events
controller.button_a.when_pressed = walk_forward
controller.button_a.when_released = stop
controller.button_b.when_pressed = walk_backward
controller.button_b.when_released = stop
# Left and right axis move event
#controller.axis_l.when_moved = on_axis_moved
controller.axis_l.when_moved = on_axis_moved
controller.trigger_r.when_moved = on_trigger_pressed
signal.pause()
except KeyboardInterrupt:
pass
def _connect_controller(self):
try:
xbox_controller = Xbox360Controller(index=1, axis_threshold=0.05)
except Exception as e: # this library emits a stupid general exception that makes it difficult to retry
try:
xbox_controller = Xbox360Controller(index=2,
axis_threshold=0.05)
except Exception as e:
xbox_controller = Xbox360Controller(index=0,
axis_threshold=0.05)
xbox_controller.button_trigger_r.when_pressed = lambda axis: self.cycle_right.emit(
axis)
xbox_controller.button_trigger_l.when_pressed = lambda axis: self.cycle_left.emit(
axis)
return xbox_controller
def Start(self):
with Xbox360Controller(0, axis_threshold=self.deadzone) as controller:
# BUTTONS
controller.button_a.when_pressed = self.on_pressed # A
controller.button_a.when_released = self.on_released
controller.button_b.when_pressed = self.on_pressed # B
controller.button_b.when_released = self.on_released
controller.button_x.when_pressed = self.on_pressed # X
controller.button_x.when_released = self.on_released
controller.button_y.when_pressed = self.on_pressed # Y
controller.button_y.when_released = self.on_released
controller.button_trigger_l.when_pressed = self.on_pressed # LB
controller.button_trigger_l.when_released = self.on_released
controller.button_trigger_r.when_pressed = self.on_pressed # RB
controller.button_trigger_r.when_released = self.on_released
controller.button_thumb_l.when_pressed = self.on_pressed # LS
controller.button_thumb_l.when_released = self.on_released
controller.button_thumb_r.when_pressed = self.on_pressed # RS
controller.button_thumb_r.when_released = self.on_released
controller.button_select.when_pressed = self.on_pressed # back
controller.button_select.when_released = self.on_released
controller.button_start.when_pressed = self.on_pressed # start
controller.button_start.when_released = self.on_released
controller.button_mode.when_pressed = self.on_pressed # mode
controller.button_mode.when_released = self.on_released
# AXES
controller.axis_l.when_moved = self.on_moved # left
controller.axis_r.when_moved = self.on_moved # right
controller.hat.when_moved = self.on_moved # hat
controller.trigger_l.when_moved = self.on_moved # LT
controller.trigger_r.when_moved = self.on_moved # RT
# The Loop
counter = 0
while self.run:
#self.steering()
time.sleep(0.005)
try:
#print("przed metoda")
self._run_in_thread2(self.silnik_1.Jedz,
self.right_stick[1])
self._run_in_thread2(self.silnik_2.Jedz,
self.left_stick[1])
# if(self.stara_pozycja_1 != (self.silnik_1.pozycja.voltage-0.780)*360/(3.946 - 0.780) or self.stara_pozycja_2 != (self.silnik_2.pozycja.voltage-0.780)*360/(3.946 - 0.780)):
# os.system("clear")
# print("Silnik 1: \t{:>5.3f}".format((self.silnik_1.pozycja.voltage-0.780)*360/(3.946 - 0.780)))
# print("Silnik 2: \t{:>5.3f}".format((self.silnik_2.pozycja.voltage-0.780)*360/(3.946 - 0.780)))
# self.stara_pozycja_1 = (self.silnik_1.pozycja.voltage-0.780)*360/(3.946 - 0.780)
# self.stara_pozycja_2 = (self.silnik_2.pozycja.voltage-0.780)*360/(3.946 - 0.780)
#print("za metoda")
# Zostawione jako przyklad self.RPI.set_engine_driver_values(self.engines[0]/100, self.engines[1]/100, self.engines[2]/100,self.engines[3]/100, 0, 0)
except Exception as e:
print(e)
print(self.right_stick[1])
pass
def Start(self):
with Xbox360Controller(0, axis_threshold=self.deadzone) as controller:
# BUTTONS
controller.button_a.when_pressed = self.on_pressed # A
controller.button_a.when_released = self.on_released
controller.button_b.when_pressed = self.on_pressed # B
controller.button_b.when_released = self.on_released
controller.button_x.when_pressed = self.on_pressed # X
controller.button_x.when_released = self.on_released
controller.button_y.when_pressed = self.on_pressed # Y
controller.button_y.when_released = self.on_released
controller.button_trigger_l.when_pressed = self.on_pressed # LB
controller.button_trigger_l.when_released = self.on_released
controller.button_trigger_r.when_pressed = self.on_pressed # RB
controller.button_trigger_r.when_released = self.on_released
controller.button_thumb_l.when_pressed = self.on_pressed # LS
controller.button_thumb_l.when_released = self.on_released
controller.button_thumb_r.when_pressed = self.on_pressed # RS
controller.button_thumb_r.when_released = self.on_released
controller.button_select.when_pressed = self.on_pressed # back
controller.button_select.when_released = self.on_released
controller.button_start.when_pressed = self.on_pressed # start
controller.button_start.when_released = self.on_released
controller.button_mode.when_pressed = self.on_pressed # mode
controller.button_mode.when_released = self.on_released
# AXES
controller.axis_l.when_moved = self.on_moved # left
controller.axis_r.when_moved = self.on_moved # right
controller.hat.when_moved = self.on_moved # hat
controller.trigger_l.when_moved = self.on_moved # LT
controller.trigger_r.when_moved = self.on_moved # RT
# The Loop
counter = 0
while self.run:
self.steering()
time.sleep(0.005)
counter += 1
if counter == 20:
counter = 0
#print(self.engines)
for e in self.engines:
if e > 50:
e = -50
if e < 50:
e = -50
try:
self.RPI.set_engine_driver_values(self.engines[0] / 100,
self.engines[1] / 100,
self.engines[2] / 100, 0,
0, self.engines[3] / 100)
except Exception:
pass
def __init__(self, player, usb, comms_config, verbose):
self.controller_state = ControllerState()
self.joystick_deadband = 25 # Out of 512
# Launch comms thread
self.comms = CommsThread(comms_config, self.controller_state, verbose)
self.comms.daemon = True
self.comms.start()
print(f"Controller: Launched player {player}")
try:
with Xbox360Controller(usb, axis_threshold=0.0) as self.controller:
# Left and right joysticks
self.controller.axis_l.when_moved = self.on_left_joystick_moved
self.controller.axis_r.when_moved = self.on_right_joystick_moved
# Dpad
self.controller.hat.when_moved = self.on_dpad_pressed
# Triggers
self.controller.trigger_l.when_moved = self.on_left_trigger
self.controller.trigger_r.when_moved = self.on_right_trigger
# Bumpers
self.controller.button_trigger_l.when_pressed = self.on_left_bumper_pressed
self.controller.button_trigger_l.when_released = self.on_left_bumper_released
self.controller.button_trigger_r.when_pressed = self.on_right_bumper_pressed
self.controller.button_trigger_r.when_released = self.on_right_bumper_released
# Button events
self.controller.button_a.when_pressed = self.on_a_button_pressed
self.controller.button_a.when_released = self.on_a_button_released
self.controller.button_b.when_pressed = self.on_b_button_pressed
self.controller.button_b.when_released = self.on_b_button_released
self.controller.button_x.when_pressed = self.on_x_button_pressed
self.controller.button_x.when_released = self.on_x_button_released
self.controller.button_y.when_pressed = self.on_y_button_pressed
self.controller.button_y.when_released = self.on_y_button_released
self.controller.button_select.when_pressed = self.on_back_button_pressed
self.controller.button_select.when_released = self.on_back_button_released
self.controller.button_mode.when_pressed = self.on_guide_button_pressed
self.controller.button_mode.when_released = self.on_guide_button_released
self.controller.button_start.when_pressed = self.on_start_button_pressed
self.controller.button_start.when_released = self.on_start_button_released
# while True:
# self.controller.set_rumble(self.controller_state.collision*0.2, self.controller_state.collision*0.2,
# duration=100)
# time.sleep(0.1)
signal.pause()
except KeyboardInterrupt:
pass
def Start(self):
with Xbox360Controller(0, axis_threshold=self.deadzone) as controller:
# BUTTONS
controller.button_a.when_pressed = self.on_pressed # A
controller.button_a.when_released = self.on_released
controller.button_b.when_pressed = self.on_pressed # B
controller.button_b.when_released = self.on_released
controller.button_x.when_pressed = self.on_pressed # X
controller.button_x.when_released = self.on_released
controller.button_y.when_pressed = self.on_pressed # Y
controller.button_y.when_released = self.on_released
controller.button_trigger_l.when_pressed = self.on_pressed # LB
controller.button_trigger_l.when_released = self.on_released
controller.button_trigger_r.when_pressed = self.on_pressed # RB
controller.button_trigger_r.when_released = self.on_released
controller.button_thumb_l.when_pressed = self.on_pressed # LS
controller.button_thumb_l.when_released = self.on_released
controller.button_thumb_r.when_pressed = self.on_pressed # RS
controller.button_thumb_r.when_released = self.on_released
controller.button_select.when_pressed = self.on_pressed # back
controller.button_select.when_released = self.on_released
controller.button_start.when_pressed = self.on_pressed # start
controller.button_start.when_released = self.on_released
controller.button_mode.when_pressed = self.on_pressed # mode
controller.button_mode.when_released = self.on_released
# AXES
controller.axis_l.when_moved = self.on_moved # left
controller.axis_r.when_moved = self.on_moved # right
controller.hat.when_moved = self.on_moved # hat
controller.trigger_l.when_moved = self.on_moved # LT
controller.trigger_r.when_moved = self.on_moved # RT
# The Loop
counter =0
while self.run:
#self.steering() # Kompletnie niepotrzebne dla manipulatora, mozna wywalic
time.sleep(0.005)
try:
#print("przed metoda")
self._run_in_thread2(self.silnik_1.Jedz, self.right_stick[1]) # Dzialania silnika sa uruchamiane w watku zeby mozna bylo sterowac np. dwoma silnikami jednoczenie bez
self._run_in_thread2(self.silnik_2.Jedz, self.left_stick[1]) # przerywania pracy programu i monitorowania pada
# if(self.stara_pozycja_1 != (self.silnik_1.pozycja.voltage-0.780)*360/(3.946 - 0.780) or self.stara_pozycja_2 != (self.silnik_2.pozycja.voltage-0.780)*360/(3.946 - 0.780)):
# os.system("clear")
# print("Silnik 1: \t{:>5.3f}".format((self.silnik_1.pozycja.voltage-0.780)*360/(3.946 - 0.780))) =========================================================
# print("Silnik 2: \t{:>5.3f}".format((self.silnik_2.pozycja.voltage-0.780)*360/(3.946 - 0.780))) =MA RAZIE NIE DZIALA TRZEBA USTAWIC ODPOWIEDNIO ENKODERY=
# self.stara_pozycja_1 = (self.silnik_1.pozycja.voltage-0.780)*360/(3.946 - 0.780) =WYSKALOWAC ENKODERY WZGLEDEM MOZLIWYCH OBROTOW MANIPU. =
# self.stara_pozycja_2 = (self.silnik_2.pozycja.voltage-0.780)*360/(3.946 - 0.780) =ZEBY NIE BYLO W POLOWIE OBROTU RAMIENIA ZMIANY ODCZYTU =
#print("za metoda") =Z 0 NA 2V. =
#=========================================================
except Exception as e:
print(e)
print(self.right_stick[1])
pass
def ControllerMovement():
try:
with Xbox360Controller(0, axis_threshold=0.2) as controller:
controller.axis_r.when_moved = axis_move
controller.axis_l.when_moved = speed
controller.button_a.when_pressed = thrower
signal.pause()
except KeyboardInterrupt:
pass
def __init__(self):
#Extra Variables
self.gripper_load = 0.0
#----------------------------------------------------------------------------------------------------
#Pygame/Joystick Initialization
#----------------------------------------------------------------------------------------------------
#Provide Pygame with a Dummy Video Driver to enable functionality
os.environ["SDL_VIDEODRIVER"] = "dummy"
#Initialize Pygame Module
pygame.init()
# Initialize the joysticks
pygame.joystick.init()
#Initialize only one joystick connected
self.joystick = pygame.joystick.Joystick(0)
self.joystick.init()
#Initiate Xbox Controller
self.xbox = Xbox360Controller()
#----------------------------------------------------------------------------------------------------
#ROS Initialization
#----------------------------------------------------------------------------------------------------
rospy.init_node("joystick_controller")
#Initialize arm stuff
self.arm_pub = rospy.Publisher('control_signal', Point, queue_size=10)
self.arm_msg = Point()
rospy.Subscriber("xbox_vibrate", Int64, self.vibrateXbox)
#Initialize gripper stuff
self.gripper_pub = rospy.Publisher("/dual_gripper_controller/command",
Float64,
queue_size=10)
rospy.Subscriber("/dual_gripper_controller/state", JointState,
self.updateGripperLoad)
self.gripper_msg = Float64()
self.gripper_msg.data = 0.0
self.gripper_pub.publish(self.gripper_msg)
#Initialize rover stuff
self.rover_pub = rospy.Publisher("arlo_wheels", String, queue_size=10)
self.rover_msg = String()
self.rover_speed = 35.0 #Out of 200 max , Was 20
self.rover_msg.data = "rst\r"
self.rover_pub.publish(self.rover_msg)
#Rate for loop
self.rate = rospy.Rate(10) #10 Hertz
def __init__(self):
self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
try:
self.controller = Xbox360Controller(0, axis_threshold=0.1)
# Left and right axis move event
self.controller.axis_l.when_moved = self.movement
self.controller.axis_r.when_moved = self.movement
# Button stop and kill
self.controller.button_a.when_released = self.kill
self.controller.button_b.when_released = self.stop
except Exception:
rospy.logwarn("x360 controller disconnected")
rospy.signal_shutdown("controller disconnected")
def Start(self):
with Xbox360Controller(0, axis_threshold=self.deadzone) as controller:
# BUTTONS
controller.button_a.when_pressed = self.on_pressed # A
controller.button_a.when_released = self.on_released
controller.button_b.when_pressed = self.on_pressed # B
controller.button_b.when_released = self.on_released
controller.button_x.when_pressed = self.on_pressed # X
controller.button_x.when_released = self.on_released
controller.button_y.when_pressed = self.on_pressed # Y
controller.button_y.when_released = self.on_released
controller.button_trigger_l.when_pressed = self.on_pressed # LB
controller.button_trigger_l.when_released = self.on_released
controller.button_trigger_r.when_pressed = self.on_pressed # RB
controller.button_trigger_r.when_released = self.on_released
controller.button_thumb_l.when_pressed = self.on_pressed # LS
controller.button_thumb_l.when_released = self.on_released
controller.button_thumb_r.when_pressed = self.on_pressed # RS
controller.button_thumb_r.when_released = self.on_released
controller.button_select.when_pressed = self.on_pressed # back
controller.button_select.when_released = self.on_released
controller.button_start.when_pressed = self.on_pressed # start
controller.button_start.when_released = self.on_released
controller.button_mode.when_pressed = self.on_pressed # mode
controller.button_mode.when_released = self.on_released
# AXES
controller.axis_l.when_moved = self.on_moved # left
controller.axis_r.when_moved = self.on_moved # right
controller.hat.when_moved = self.on_moved # hat
controller.trigger_l.when_moved = self.on_moved # LT
controller.trigger_r.when_moved = self.on_moved # RT
# The Loop
counter = 0
minA = 250 #kat do warunkow, z zapasem w góre zamiast 180
maxA = 350 #kat do warunkow, z zapasem w dół zamiast 360
minB = 100 #kat do warunkow, z zapasem w dół zamiast 450
maxB = 260 #kat do warunkow, z zapasem w dół zamiast 450
dodatnia = 0.5 #rotation speed
ujemna = -0.5 #predkosc obrotow przeciwnie z wskazówkami zegara
while self.run:
time.sleep(0.005)
try:
#=========================================================
pass
except Exception as e:
print(e)
pass
def __init__(self, active_scene, cursor, ms):
global launchOut
self.init_joystick()
self.active_scene = active_scene
self.buttons = [
'A', 'B', 'X', 'Y', 'LB', 'RB', 'Menu', 'Start', 'LS', 'RS', 'R'
]
self.hats = ['Up', 'Down', 'Left', 'Right']
# If you would like there to be a keyboard fallback configuration, fill those out
# here in this mapping.
self.key_map = {
pygame.K_UP: 'up',
pygame.K_DOWN: 'down',
pygame.K_LEFT: 'left',
pygame.K_RIGHT: 'right',
pygame.K_RETURN: 'start',
pygame.K_a: 'A',
pygame.K_b: 'B',
pygame.K_x: 'X',
pygame.K_y: 'Y',
pygame.K_l: 'L',
pygame.K_r: 'R'
}
self.cursor = cursor
self.speed = 75
self.lastPacketSendTime = datetime.datetime.now()
# This dictionary will tell you which logical buttons are pressed, whether it's
# via the keyboard or joystick
self.keys_pressed = {}
self.intakeTimer = pygame.time.get_ticks()
for button in self.buttons:
self.keys_pressed[button] = False
self.quit_attempt = False
self.launchSpeed = 0
self.launchTime = pygame.time.get_ticks()
launchOut = 0
self.decay = False
self.count = 0
self.ms = ms
self.controller = Xbox360Controller(0)
self.ballpos = deque()
self.robpos = deque()
self.vectors = deque()
def laptopNode():
global c_angle, c_speed, img, lscan
# controller = PurePursuit() #PurePursuitPlusPID()
print("h1")
rospy.init_node('ros_node_laptop')
print("h2")
pub = rospy.Publisher('/NN_angle_speed', String, queue_size=1)
print("h2.1")
img_sub = rospy.Subscriber('/raspicam_node/image/compressed',
CompressedImage, img_callback)
print("h2.2")
car_sub = rospy.Subscriber('/car_angle_speed', String, car_callback)
# lidar_sub = rospy.Subscriber('/rplidarNode/scan', LaserScan, lidar_callback)
print("h3")
rate = rospy.Rate(60) # 30 hz
if TRAINING:
xbox = Xbox360Controller()
print("h4")
while not rospy.is_shutdown():
if not TRAINING:
# img = process_img(img)
# # get into correct shape for network
# img = img.reshape(1,200,400,3).reshape(1,1,200,400,3)
# prediction = model.predict(img)[0]
# # construct line from predicted points, each being one meter apart
# trajectory_prediction = np.array([[float(x),prediction[x]] for x in range(len(prediction))])
# speed, angle = controller.get_control(trajectory_prediction, speed, desired_speed = 25, dt=1./FPS)
speed, angle = 0, 0
else:
angle = xbox.axis_l.x if abs(xbox.axis_l.x) > 0.15 else 0
speed = xbox.trigger_r.value - xbox.trigger_l.value if abs(
xbox.trigger_r.value - xbox.trigger_l.value) > 0.075 else 0
speed = -1 * speed / 2
# with open(f"training_{fname}.p",'a') as ff:
# pngname = {datetime.now().strftime('%m_%d_%H_%M_%S')}
# data = {"time" : rospy.get_time(), "c_angle" : c_angle, "c_speed" : c_speed, \
# "lscan" : lscan, "img" : f"raspicam_{pngname}.png"}
# cv2.imwrite(f"raspicam_{pngname}.png",img)
# pickle.dump(data,ff)
pub.publish(f"{angle},{speed}")
print(
f"NN Ang: {round(angle,2)}\tNN Vel: {round(speed,2)}\tCar Ang: {round(c_angle,2)}\tCar Vel: {round(c_speed,2)}\tAng Err: {round(c_angle-angle,2)}\tVel Err: {round(c_speed-speed,2)}"
)
rate.sleep()
if TRAINING:
xbox.close()
def configure_gamepad(self, index):
from xbox360controller import Xbox360Controller
self.gamepads.append([])
try:
with Xbox360Controller(index, raw_mode=True) as controller:
self.log("configuring gamepad", index)
for b in controller.buttons:
b.when_pressed = self.on_button_pressed
self.gamepads[index].append(b)
for a in controller.axes:
a.when_moved = self.on_axis_moved_raw
self.gamepads[index].append(a)
pause()
except Exception as error:
print(
f"{self.term_fail}No USB Controller connected. Skipped. \nError : {error}{self.term_endc}"
)
def trainNetwork():
global Recording
client = airsim.CarClient()
client.confirmConnection()
print('Connect succcefully！')
# client.enableApiControl(True)
# client.reset()
print('Environment initialized!')
controller = Xbox360Controller()
# Initialize the buffer
replay_experiences = deque()
replay_experiences = store_transition(replay_experiences, 'read')
# get the first state
kinematics = client.getCarState().kinematics_estimated
img = get_image(client)
velocity_x = kinematics.linear_velocity.x_val + 1.6
velocity_y = kinematics.linear_velocity.y_val - 15
velocity = np.sqrt(velocity_x**2 + velocity_y**2)
acc_x = kinematics.linear_acceleration.x_val
acc_y = kinematics.linear_acceleration.y_val
acc = np.sqrt(acc_x**2 + acc_y**2)
kinematics_state = np.array([velocity, acc])
state_previous = {}
state_previous['image'] = img
state_previous['kinematics_state'] = kinematics_state
reward_previous = 10
pbar = tqdm(total=MEMORY_SIZE - len(replay_experiences))
controller.button_mode.when_pressed = mode_button_pressed
client.simSetTimeOfDay(False)
start_size = int(len(replay_experiences))
difference = int(MEMORY_SIZE - len(replay_experiences))
# for i in tqdm(range(MEMORY_SIZE+10)):
while len(replay_experiences) < MEMORY_SIZE:
if len(replay_experiences) + 1 == start_size + difference // 2:
client.simSetTimeOfDay(True, "2019-6-17 18:00:00", move_sun=True)
if (len(replay_experiences) + 1 - start_size) % (difference // 2) == 0:
client.simSetWeatherParameter(airsim.WeatherParameter.Rain, 0)
client.simSetWeatherParameter(airsim.WeatherParameter.Snow, 0)
client.simSetWeatherParameter(airsim.WeatherParameter.MapleLeaf, 0)
if (len(replay_experiences) + 1 -
start_size) % (difference // 2) == (difference // 2) * (1 / 4):
client.simSetWeatherParameter(airsim.WeatherParameter.Rain, 0.99)
if (len(replay_experiences) + 1 -
start_size) % (difference // 2) == (difference // 2) * (1 / 4):
client.simSetWeatherParameter(airsim.WeatherParameter.Snow, 0.99)
if (len(replay_experiences) + 1 -
start_size) % (difference // 2) == (difference // 2) * (1 / 4):
client.simSetWeatherParameter(airsim.WeatherParameter.Rain, 0)
client.simSetWeatherParameter(airsim.WeatherParameter.MapleLeaf,
0.99)
if (len(replay_experiences) + 1) % 200 == 0:
signal_back = store_transition(replay_experiences, 'store')
if signal_back:
print('store pkl file succcefully')
car_state = client.getCarState()
_, reward_step = compute_reward(car_state)
kinematics = car_state.kinematics_estimated
img = get_image(client)
velocity_x = kinematics.linear_velocity.x_val + 1.6
velocity_y = kinematics.linear_velocity.y_val - 15
velocity = np.sqrt(velocity_x**2 + velocity_y**2)
acc_x = kinematics.linear_acceleration.x_val
acc_y = kinematics.linear_acceleration.y_val
acc = np.sqrt(acc_x**2 + acc_y**2)
kinematics_state = np.array([velocity, acc])
state = {}
state['image'] = img
state['kinematics_state'] = kinematics_state
steer = controller.axis_l.x
brake = controller.trigger_l.value
acc = controller.trigger_r.value
if steer >= -0.25 and steer <= 0.25:
steer = 0
elif steer > 0.25:
steer = 4 / 3.0 * steer - 1 / 3.0
elif steer < -0.25:
steer = 4 / 3.0 * steer + 1 / 3.0
print('acc: %f brake: %f steer: %f' % (acc, brake, steer))
print('reward:', reward_previous)
print('Recording: %d' % Recording)
print('memory: %d' % len(replay_experiences))
print('weather:', times_a, times_b, times_x, times_y)
action = np.array([acc, brake, steer])
if Recording:
replay_experiences.append(
(state_previous, action, reward_previous))
pbar.update(1)
state_previous = copy.deepcopy(state)
reward_previous = reward_step
time.sleep(0.1)
client.simSetTimeOfDay(False)
print(len(replay_experiences))
signal_back = store_transition(replay_experiences, 'store')
if signal_back:
print('store pkl file succcefully')
print(len(replay_experiences))
p = GPIO.PWM(signalPin, freq)
p.start(dc)
# def speed2dc():
# dc = dc_n + (speed*20)
# if dc > dc_n-1 and dc < dc_n+1:
# dc = 0
# elif dc < dc_min:
# dc = dc_min
# elif dc > dc_max:
# dc = dc_max
# p.ChangeDutyCycle(dc)
# print("Freqency: ", freq, "Duty Cycle: ", dc)
controller = Xbox360Controller(0, axis_threshold=0.2)
while (1):
# randomVal = controller.axis_l._value_y
# speed = float(input("Enter Speed [-1,1]: "))
speed = controller.axis_l._value_y
dc = dc_n + (speed * 20)
if dc > dc_n - 1 and dc < dc_n + 1:
dc = 0
elif dc < dc_min:
dc = dc_min
elif dc > dc_max:
dc = dc_max
p.ChangeDutyCycle(dc)
print("L Axis: ", controller.axis_l._value_y, "Freqency: ", freq,
"Duty Cycle: ", dc)
import time
from xbox360controller import Xbox360Controller
import signal
def create_callback(command):
def on_button_released(button):
print(command, flush=True)
return on_button_released
if __name__ == '__main__':
try:
with Xbox360Controller(0, axis_threshold=0.2) as controller:
controller.button_a.when_released = create_callback("playpause")
controller.button_b.when_released = create_callback(
"previous-context")
controller.button_y.when_released = create_callback(
"play-album-current")
controller.button_x.when_released = create_callback(
"stop-listening")
controller.button_trigger_r.when_released = create_callback(
"next-track")
controller.button_trigger_l.when_released = create_callback(
"previous-track")
controller.button_start.when_released = create_callback(
"start-on raspberry spotify:user:11102248483:playlist:3ar6blvho0KSTjNaYfzlt2"
)
controller.button_mode.when_released = create_callback(
async def main(args):
global controller_state
if not os.path.exists('/dev/input/js0'):
print('Controller not plugged')
return
# parse the spi flash
if args.spi_flash:
with open(args.spi_flash, 'rb') as spi_flash_file:
spi_flash = FlashMemory(spi_flash_file.read())
else:
# Create memory containing default controller stick calibration
spi_flash = FlashMemory()
controller = Controller.PRO_CONTROLLER
factory = controller_protocol_factory(controller, spi_flash=spi_flash)
ctl_psm, itr_psm = 17, 19
transport, protocol = await create_hid_server(
factory,
reconnect_bt_addr=args.reconnect_bt_addr,
ctl_psm=ctl_psm,
itr_psm=itr_psm,
capture_file=None,
device_id=args.device_id)
controller_state = protocol.get_controller_state()
with Xbox360Controller(0, axis_threshold=-1) as xcontroller:
if args.xbox_layout:
xcontroller.button_a.when_pressed = a_pressed
xcontroller.button_a.when_released = a_released
xcontroller.button_b.when_pressed = b_pressed
xcontroller.button_b.when_released = b_released
xcontroller.button_x.when_pressed = x_pressed
xcontroller.button_x.when_released = x_released
xcontroller.button_y.when_pressed = y_pressed
xcontroller.button_y.when_released = y_released
else:
xcontroller.button_a.when_pressed = b_pressed
xcontroller.button_a.when_released = b_released
xcontroller.button_b.when_pressed = a_pressed
xcontroller.button_b.when_released = a_released
xcontroller.button_x.when_pressed = y_pressed
xcontroller.button_x.when_released = y_released
xcontroller.button_y.when_pressed = x_pressed
xcontroller.button_y.when_released = x_released
xcontroller.button_trigger_l.when_pressed = l_pressed
xcontroller.button_trigger_l.when_released = l_released
xcontroller.button_trigger_r.when_pressed = r_pressed
xcontroller.button_trigger_r.when_released = r_released
xcontroller.button_thumb_l.when_pressed = ls_pressed
xcontroller.button_thumb_l.when_released = ls_released
xcontroller.button_thumb_r.when_pressed = rs_pressed
xcontroller.button_thumb_r.when_released = rs_released
xcontroller.button_mode.when_pressed = home_pressed
xcontroller.button_mode.when_released = home_released
xcontroller.button_start.when_pressed = plus_pressed
xcontroller.button_start.when_released = plus_released
xcontroller.button_select.when_pressed = minus_pressed
xcontroller.button_select.when_released = minus_released
xcontroller.trigger_l.when_moved = lt_moved
xcontroller.trigger_r.when_moved = rt_moved
xcontroller.hat.when_moved = hat_moved
xcontroller.axis_l.when_moved = axis_l_moved
xcontroller.axis_r.when_moved = axis_r_moved
while True:
try:
await controller_state.send()
except NotConnectedError:
print('Connection Lost')
return
x_axis = round(axis.x * 500)
y_axis = round(axis.y * -500)
if abs(x_axis) <= 150:
x_axis = 0
if abs(y_axis) <= 150:
y_axis = 0
print('Axis {0} moved to {1} {2}'.format(axis.name, x_axis, y_axis))
payload = json.dumps({
"axis": axis.name,
"x_axis": x_axis,
"y_axis": y_axis
})
client.publish(robotName, payload)
try:
with Xbox360Controller(int(robot_number) - 1,
axis_threshold=0.2) as controller:
# Button A events
controller.button_a.when_pressed = on_button_pressed
controller.button_a.when_released = on_button_released
# Left and right axis move event
controller.axis_l.when_moved = on_axis_moved
signal.pause()
except KeyboardInterrupt:
pass
def Start(self):
with Xbox360Controller(0, axis_threshold=self.deadzone) as controller:
# BUTTONS
controller.button_a.when_pressed = self.on_pressed # A
controller.button_a.when_released = self.on_released
controller.button_b.when_pressed = self.on_pressed # B
controller.button_b.when_released = self.on_released
controller.button_x.when_pressed = self.on_pressed # X
controller.button_x.when_released = self.on_released
controller.button_y.when_pressed = self.on_pressed # Y
controller.button_y.when_released = self.on_released
controller.button_trigger_l.when_pressed = self.on_pressed # LB
controller.button_trigger_l.when_released = self.on_released
controller.button_trigger_r.when_pressed = self.on_pressed # RB
controller.button_trigger_r.when_released = self.on_released
controller.button_thumb_l.when_pressed = self.on_pressed # LS
controller.button_thumb_l.when_released = self.on_released
controller.button_thumb_r.when_pressed = self.on_pressed # RS
controller.button_thumb_r.when_released = self.on_released
controller.button_select.when_pressed = self.on_pressed # back
controller.button_select.when_released = self.on_released
controller.button_start.when_pressed = self.on_pressed # start
controller.button_start.when_released = self.on_released
controller.button_mode.when_pressed = self.on_pressed # mode
controller.button_mode.when_released = self.on_released
# AXES
controller.axis_l.when_moved = self.on_moved # left
controller.axis_r.when_moved = self.on_moved # right
controller.hat.when_moved = self.on_moved # hat
controller.trigger_l.when_moved = self.on_moved # LT
controller.trigger_r.when_moved = self.on_moved # RT
# The Loop
counter =0
minA = 250 #kat do warunkow, z zapasem w góre zamiast 180
maxA = 350 #kat do warunkow, z zapasem w dół zamiast 360
minB = 100 #kat do warunkow, z zapasem w dół zamiast 450
maxB = 260 #kat do warunkow, z zapasem w dół zamiast 450
dodatnia = 0.5 #rotation speed
ujemna = -0.5 #predkosc obrotow przeciwnie z wskazówkami zegara
while self.run:
time.sleep(0.005)
try:
#print("przed metoda")
# self.updateAngle()
# if (self.AM2 < self.AM1 and self.AM1 - self.AM2 < 180):
self._run_in_thread2(self.silnik_1.Jedz, self.right_stick[1]) # Dzialania silnika sa uruchamiane w watku zeby mozna bylo sterowac np. dwoma silnikami jednoczenie bez
self._run_in_thread2(self.silnik_2.Jedz, self.left_stick[1]) # przerywania pracy programu i monitorowania pada
## elif (self.AM1 <= minA):
## self._run_in_thread2(self.silnik_1.Jedz, ujemna)
## print("A<minA")
##
## elif (sef.AM1 >= maxA):
## self._run_in_thread2(self.silnik_1.Jedz, dodatnia)
## print("A>=maxA")
# elif (self.AM2 + 5 >= self.AM1):
# self._run_in_thread2(self.silnik_2.Jedz, ujemna)
# print("B+5>=A")
## elif (self.AM2 <= minB):
## self._run_in_thread2(self.silnik_2.Jedz, dodatnia)
## print("B<=minB")
## elif (self.AM2 >= maxB):
## self._run_in_thread2(self.silnik_2.Jedz, ujemna)
## print("B>=maxB")
#elif (self.AM1 - self.AM2 >= 180):
# self._run_in_thread2(self.silnik_2.Jedz, dodatnia)
# print("A-B>=180")
## if(AM1 != oldAM1 or AM2 != oldAM2)
## if(self.stara_pozycja_1 != (self.silnik_1.pozycja.voltage-0.780)*360/(3.946 - 0.780) or self.stara_pozycja_2 != (self.silnik_2.pozycja.voltage-0.780)*360/(3.946 - 0.780)):
## os.system("clear")
## #print("Silnik 1: \t{:>5.3f}".format((self.silnik_1.pozycja.voltage-0.780)*360/(3.946 - 0.780))) #=========================================================
## #print("Silnik 2: \t{:>5.3f}".format((self.silnik_2.pozycja.voltage-0.780)*360/(3.946 - 0.780))
## print("Silnik 1: \t{:>5.3f}".format(AM1)
## print("Silnik 1: \t{:>5.3f}".format(AM2) #=MA RAZIE NIE DZIALA TRZEBA USTAWIC ODPOWIEDNIO ENKODERY=
## self.stara_pozycja_1 = (self.silnik_1.pozycja.voltage-0.780)*360/(3.946 - 0.780) #=WYSKALOWAC ENKODERY WZGLEDEM MOZLIWYCH OBROTOW MANIPU. =
## self.stara_pozycja_2 = (self.silnik_2.pozycja.voltage-0.780)*360/(3.946 - 0.780) #=ZEBY NIE BYLO W POLOWIE OBROTU RAMIENIA ZMIANY ODCZYTU =
## print("za metoda") #=Z 0 NA 2V. =
## #=========================================================
except Exception as e:
print(e)
pass
"button_y": Preset(4),
}
if __name__ == '__main__':
try:
name = ''.join([
x if x in string.printable else ''
for x in Xbox360Controller.get_available()[0].name
])
if name == "Logitech Logitech RumblePad 2 USB":
switchAxisR = True
else:
switchAxisR = False
print(switchAxisR)
with Xbox360Controller(
0, axis_threshold=0,
switch_axis_r_with_trigger_l=switchAxisR) as controller:
# Button A events
controller.button_trigger_l.when_pressed = zoomout
controller.button_trigger_r.when_pressed = zoomin
controller.button_trigger_l.when_released = zoomstop
controller.button_trigger_r.when_released = zoomstop
controller.button_start.when_pressed = switchCam
controller.button_select.when_pressed = toggleAF
controller.button_a.when_pressed = handlePresetStart
controller.button_b.when_pressed = handlePresetStart
controller.button_x.when_pressed = handlePresetStart
controller.button_y.when_pressed = handlePresetStart
def main(args):
with BotRecorder(
args.filename,
args.device) as recorder, Xbox360Controller(0) as controller:
input('hit enter to start recording\n')
# Axis
# Sticks
controller.axis_l.when_moved = lambda a: recorder.stick_move(
a, L_STICK)
controller.axis_r.when_moved = lambda a: recorder.stick_move(
a, R_STICK)
# DPAD
controller.hat.when_moved = lambda a: recorder.dpad_move(a)
# Raw Axis
# Trigger
controller.trigger_l.when_moved = lambda ra: recorder.trigger_move(
ra, ZL)
controller.trigger_r.when_moved = lambda ra: recorder.trigger_move(
ra, ZR)
# Buttons
# are mapped to the switch button layout
controller.button_b.when_pressed = lambda b: recorder.button_press(A)
controller.button_b.when_released = lambda b: recorder.button_release(A
)
controller.button_a.when_pressed = lambda b: recorder.button_press(B)
controller.button_a.when_released = lambda b: recorder.button_release(B
)
controller.button_y.when_pressed = lambda b: recorder.button_press(X)
controller.button_y.when_released = lambda b: recorder.button_release(X
)
controller.button_x.when_pressed = lambda b: recorder.button_press(Y)
controller.button_x.when_released = lambda b: recorder.button_release(Y
)
controller.button_trigger_l.when_pressed = lambda b: recorder.button_press(
L)
controller.button_trigger_l.when_released = lambda b: recorder.button_release(
L)
controller.button_trigger_r.when_pressed = lambda b: recorder.button_press(
R)
controller.button_trigger_r.when_released = lambda b: recorder.button_release(
R)
controller.button_mode.when_pressed = lambda b: recorder.button_press(
HOME)
controller.button_mode.when_released = lambda b: recorder.button_release(
HOME)
controller.button_select.when_pressed = lambda b: recorder.button_press(
MINUS)
controller.button_select.when_released = lambda b: recorder.button_release(
MINUS)
controller.button_start.when_pressed = lambda b: recorder.button_press(
PLUS)
controller.button_start.when_released = lambda b: recorder.button_release(
PLUS)
controller.button_thumb_l.when_pressed = lambda b: recorder.button_press(
LCLICK)
controller.button_thumb_l.when_released = lambda b: recorder.button_release(
LCLICK)
controller.button_thumb_r.when_pressed = lambda b: recorder.button_press(
RCLICK)
controller.button_thumb_r.when_released = lambda b: recorder.button_release(
RCLICK)
recorder.start()
print('setup recording started...\n')
input('hit enter to proceed to loop phase\n')
recorder.next_phase()
print('loop recording started...\n')
input('hit enter to finish\n')
print('done')
def on_button_a_pressed(button):
global fsm_state, data
fsm_state = "DATA_AVAILABLE"
data = "Button a was pressed"
print(data)
def o_axis_r_moved(axis):
global fsm_state, data
fsm_state = "DATA_AVAILABLE"
data = f"Axis {axis.name} moved to {axis.x} {axis.y}"
print(data)
i = 0
with Xbox360Controller(axis_threshold=0.05) as c:
c.button_a.when_pressed = on_button_a_pressed
# c.axis_r.when_moved = o_axis_r_moved
with Serial(port=PORT, baudrate=BAUDRATE) as s:
fsm_state = "SERIAL_OPEN"
fsm_state = "NO_DATA_TO_SEND"
while True:
if fsm_state == "DATA_AVAILABLE":
tic = time()
s.write(f"-{i}-".encode())
fsm_state = "NO_DATA_TO_SEND"
data = None
i += 1
def __init__(self):
#Extra Variables
self.gripper_load = 0.0
# #----------------------------------------------------------------------------------------------------
# #Pan/Tilt Camera IP Socket Camera #1
# #----------------------------------------------------------------------------------------------------
# self.IP_ADDRESS = "192.168.1.9"
# self.IP_PORT = 23000
# self.client = TCPClient(self.IP_ADDRESS, self.IP_PORT, stateChanged = self.onStateChangedOne)
# self.rc = self.client.connect()
# if(self.rc):
# print("Connected 1")
# else:
# print("Not connected 1")
# #----------------------------------------------------------------------------------------------------
# #Pan/Tilt Camera IP Socket Camera #2
# #----------------------------------------------------------------------------------------------------
# self.IP_ADDRESS_2 = "192.168.1.8"
# self.IP_PORT_2 = 24000
# self.client_2 = TCPClient(self.IP_ADDRESS_2, self.IP_PORT_2, stateChanged = self.onStateChangedTwo)
# self.rc_2 = self.client_2.connect()
# if(self.rc_2):
# print("Connected 2")
# else:
# print("Not connected 2")
#----------------------------------------------------------------------------------------------------
#Pygame/Joystick Initialization
#----------------------------------------------------------------------------------------------------
pygame.init()
# Initialize the joysticks
pygame.joystick.init()
#Initialize only one joystick connected
self.joystick = pygame.joystick.Joystick(0)
self.joystick.init()
#Initiate Xbox Controller
self.xbox = Xbox360Controller()
#----------------------------------------------------------------------------------------------------
#ROS Initialization
#----------------------------------------------------------------------------------------------------
rospy.init_node("joystick_controller")
#Initialize arm stuff
self.arm_pub = rospy.Publisher('control_signal', Point, queue_size=10)
self.arm_msg = Point()
rospy.Subscriber("xbox_vibrate", Int64, self.vibrateXbox)
#Initialize gripper stuff
self.gripper_pub = rospy.Publisher("/dual_gripper_controller/command",
Float64,
queue_size=10)
rospy.Subscriber("/dual_gripper_controller/state", JointState,
self.updateGripperLoad)
self.gripper_msg = Float64()
self.gripper_msg.data = 0.0
self.gripper_pub.publish(self.gripper_msg)
#Initialize rover stuff
self.rover_pub = rospy.Publisher("arlo_wheels", String, queue_size=10)
self.rover_msg = String()
self.rover_speed = 35.0 #Out of 200 max , Was 20
self.rover_msg.data = "rst\r"
self.rover_pub.publish(self.rover_msg)
#Rate for loop
self.rate = rospy.Rate(10) #10 Hertz