def load_effects(self):
#effect 1, light rumble
rumble = ff.Rumble(strong_magnitude=0x0000, weak_magnitude=0x500)
duration_ms = 300
effect = ff.Effect(ecodes.FF_RUMBLE, -1, 0, ff.Trigger(0, 0),
ff.Replay(duration_ms, 0),
ff.EffectType(ff_rumble_effect=rumble))
self.effect1_id = self.device_file.upload_effect(effect)
# effect 2, strong rumble
rumble = ff.Rumble(strong_magnitude=0xc000, weak_magnitude=0x0000)
duration_ms = 200
effect = ff.Effect(ecodes.FF_RUMBLE, -1, 0, ff.Trigger(0, 0),
ff.Replay(duration_ms, 0),
ff.EffectType(ff_rumble_effect=rumble))
self.effect2_id = self.device_file.upload_effect(effect)
def __init__(self, joystick_num, points, rumble_ms):
pygame.joystick.init()
self.joystick_num = joystick_num
self.joystick_count = pygame.joystick.get_count()
if self.joystick_num > self.joystick_count:
print(
"ERROR: Joystick index exceeds joystick count. i.e. no joystick detected"
)
exit(1)
self.joystick = pygame.joystick.Joystick(self.joystick_num)
self.joystick.init()
self.joystick_name = self.joystick.get_name()
self.axis = self.joystick.get_numaxes()
self.num_buttons = self.joystick.get_numbuttons()
self.num_hats = self.joystick.get_numhats()
self.screenwidth, self.screenheight = points.screenwidth, points.screenheight
self.scaling = points.radius
# Adding idle axis as a point for nearest neighbour
self.idle_axis = (self.input_scaling(self.get_controller_state()))
# Find first EV_FF capable event device (that we have permissions to use).
self.dev = None
for name in list_devices():
self.dev = InputDevice(name)
if e.EV_FF in self.dev.capabilities():
break
rumble = ff.Rumble(strong_magnitude=0x0000, weak_magnitude=0xffff)
effect_type = ff.EffectType(ff_rumble_effect=rumble)
effect = ff.Effect(e.FF_RUMBLE, -1, 0, ff.Trigger(0, 0),
ff.Replay(rumble_ms, 0), effect_type)
self.effect_id = self.dev.upload_effect(effect)
def __init__(self, device, duration_ms):
rumble = ff.Rumble(strong_magnitude=0xc000, weak_magnitude=0xc000)
self.effect = ff.Effect(
ecodes.FF_RUMBLE, # type
-1, # id (set by ioctl)
0, # direction
ff.Trigger(0, 0), # no triggers
ff.Replay(duration_ms, 0), # length and delay
ff.EffectType(ff_rumble_effect=rumble))
self.device = device
self.effect_id = self.device.upload_effect(self.effect)
self.device.write(ecodes.EV_FF, self.effect_id, 1)
def vibrate_controller():
rumble = ff.Rumble(strong_magnitude=0xc000, weak_magnitude=0xc000)
effect_type = ff.EffectType(ff_rumble_effect=rumble)
duration_ms = 500
effect = ff.Effect(
ecodes.FF_RUMBLE, # type
-1, # id (set by ioctl)
0, # direction
ff.Trigger(0, 0), # no triggers
ff.Replay(duration_ms, 0), # length and delay
ff.EffectType(ff_rumble_effect=rumble)
)
effect_id = dev.upload_effect(effect)
repeat_count = 1
gamepad.write(ecodes.EV_FF, effect_id, repeat_count)
time.sleep(1)
gamepad.erase_effect(effect_id)
def VibrateBase(time):
from evdev import ecodes, InputDevice, ff
# Find first EV_FF capable event device (that we have permissions to use).
for name in evdev.list_devices():
dev = InputDevice(name)
if ecodes.EV_FF in dev.capabilities():
break
rumble = ff.Rumble(strong_magnitude=0x0000, weak_magnitude=0xffff)
effect_type = ff.EffectType(ff_rumble_effect=rumble)
duration_ms = 1000 * time
effect = ff.Effect(
ecodes.FF_RUMBLE, -1, 0,
ff.Trigger(0, 0),
ff.Replay(duration_ms, 0),
ff.EffectType(ff_rumble_effect=rumble)
)
repeat_count = 1
effect_id = dev.upload_effect(effect)
dev.write(e.EV_FF, effect_id, repeat_count)
dev.erase_effect(effect_id)
import evdev
from evdev import ecodes, InputDevice, ff
# Find first EV_FF capable event device (that we have permissions to use).
for name in evdev.list_devices():
dev = InputDevice(name)
if ecodes.EV_FF in dev.capabilities():
break
rumble = ff.Rumble(strong_magnitude=0x0000, weak_magnitude=0xffff)
effect_type = ff.EffectType(ff_rumble_effect=rumble)
duration_ms = 1000
effect = ff.Effect(
ecodes.FF_RUMBLE, -1, 0,
ff.Trigger(0, 0),
ff.Replay(duration_ms, 0),
ff.EffectType(ff_rumble_effect=rumble)
)
# ie.type = EV_FF;
# ie.code = FF_GAIN;
# ie.value = 0xFFFFUL * gain / 100;
repeat_count = 10
effect_id = dev.upload_effect(effect)
dev.write(ecodes.EV_FF, effect_id, repeat_count)
input()
dev.erase_effect(effect_id)
def create_effect(strong, weak):
return ff.Effect(
ecodes.FF_RUMBLE, -1, 0, ff.Trigger(0, 0), ff.Replay(17, 0),
ff.EffectType(ff_rumble_effect=ff.Rumble(strong_magnitude=strong,
weak_magnitude=weak)))
def __init__(self,
options_dict=None,
autonomous=[],
print_freq=1.0,
joystick_id='??:??:??:??:??:??',
debug=False,
save_data=False):
"""
joystick_id: see instruction on how to pair the joystick to the RPI Zero W
"""
# List of inputs that will be taken over when in autonomous mode
# For testing purposes, it is possible to pass even an empty list
# or a combination of these possible values: ['roll', 'pitch', 'throttle']
self.autonomous_inputs_enabled = autonomous
self.joystick_id = joystick_id
self.print_freq = print_freq
self.debug = debug
self.save_data = save_data
if self.save_data:
self.DATA = deque(
[]) # list where the CMDS data will be temporarily stored
# Using MSP controller it's possible to have more auxiliary inputs than this.
# In fact, I think it's possible to have up to AUX14 using RX_MSP.
# Below are the default values for all the cmds:
self.CMDS_init = {
'roll': 1500,
'pitch': 1500,
'throttle': 1000, # throttle bellow a certain value disarms the FC
'yaw': 1500,
'CH5': 1000, # DISARMED (1000) / ARMED (1800)
'CH6': 1500, # ANGLE (1000) / NAV_POSHOLD (1500)
'CH7': 1000, # FAILSAFE (1800)
'CH8': 1000 # HEADING HOLD (1800)
}
# This order changes according to the configurations on betaflight:
# Receiver=>Channel Map=>AETR1234
# A=>roll, E=>pitch, T=>throttle, R=>yaw and 1234...
self.CMDS_ORDER = [
'roll', 'pitch', 'throttle', 'yaw', 'CH5', 'CH6', 'CH7', 'CH8'
]
self.controller_init = {
'kpx_v': 0.0,
'kix_v': 0.0,
'kdx_v': 0.0, # PID values velocity control: x
'kpx_p': 0.0,
'kix_p': 0.0,
'kdx_p': 0.0, # PID values position control: x
'kpy_v': 0.0,
'kiy_v': 0.0,
'kdy_v': 0.0, # PID values velocity control: y
'kpy_p': 0.0,
'kiy_p': 0.0,
'kdy_p': 0.0, # PID values position control: y
'kpz_v': 0.0,
'kiz_v': 0.0,
'kdz_v': 0.0, # PID values velocity control: z
'kpz_p': 0.0,
'kiz_p': 0.0,
'kdz_p': 0.0, # PID values position control: z
'kph_v': 0.0,
'kih_v': 0.0,
'kdh_v': 0.0, # PID values velocity control: heading
'kph_p': 0.0,
'kih_p': 0.0,
'kdh_p': 0.0, # PID values position control: heading
'beta_tof': 0.0,
'beta_optflow': 0.0
}
self.sensors_init = {'beta_tof': 0.0, 'beta_optflow': 0.0}
if options_dict != None:
if 'Input_Config' in options_dict:
self.CMDS_init = options_dict['Input_Config']
if 'Input_Order' in options_dict:
self.CMDS_ORDER = options_dict['Input_Order']
if 'Controller_Config' in options_dict:
self.controller_init = options_dict['Controller_Config']
if 'Sensors_Config' in options_dict:
self.sensors_init = options_dict['Sensors_Config']
self.CMDS_init['error_p'] = [0.0, 0.0, 0.0,
0.0] # error_x,error_y,error_z,error_h
self.CMDS_init['error_v'] = [0.0, 0.0, 0.0, 0.0
] # error_vx,error_vy,error_vz,error_vh
self.CMDS_init['velocity'] = [0.0, 0.0, 0.0, 0.0] # vx,vy,vz,vh
self.CMDS_init['time_stamp'] = 0.0
self.CMDS = self.CMDS_init.copy()
try:
self.gamepad = InputDevice('/dev/input/event2')
except FileNotFoundError:
raise Exception(
'I would bet the PS4 joystick is not connected... Did you turn it on?'
)
# Initialization of some global constants...
self.shutdown = False
self.board = None
self.mean_batt_voltage = -1
self.min_batt_voltage = -1
# Controls the order how things are printed
# from the dict frequencies_measurement
self.frequencies_keys = [
'send_cmds_to_fc', 'joystick_interface', 'autonomous', 'control',
'read_voltage_from_fc', 'print_values'
]
# It starts with 100000 just because 0 would crash and -1 looks weird...
# TODO: I think it will not crash with zero anymore because there's a barrier
self.frequencies_measurement = {
'joystick_interface': 100000,
'send_cmds_to_fc': 100000,
'autonomous': 100000,
'control': 100000,
'read_voltage_from_fc': 100000,
'print_values': 100000
}
# Setup for the joystick vibration used to alert the user (warnings)
rumble = ff.Rumble(strong_magnitude=0x0000, weak_magnitude=0xffff)
duration_ms = 1000
effect = ff.Effect(ecodes.FF_RUMBLE, -1, 0, ff.Trigger(0, 0),
ff.Replay(duration_ms, 0),
ff.EffectType(ff_rumble_effect=rumble))
self.effect_id = self.gamepad.upload_effect(effect)
self.NofCHEV = 50 # just used when printing info on the screen
# Limits the max frequency these nodes can run
self.joystick_min_period = 1 / 50.0
self.fc_min_period = 1 / 100.0 # too many cmds may overload the FC
# Find first EV_FF capable event device (that we have permissions to use).
for name in util.list_devices():
dev = InputDevice(name)
if ecodes.EV_FF in dev.capabilities():
break
if dev is None:
print("Sorry, no FF capable device found")
else:
print("found " + dev.name + " at " + dev.path)
print("Preparing FF effect...")
rumble = ff.Rumble(strong_magnitude=0xc000, weak_magnitude=0xc000)
effect_type = ff.EffectType(ff_rumble_effect=rumble)
duration_ms = 1000
effect = ff.Effect(
ecodes.FF_RUMBLE, # type
-1, # id (set by ioctl)
0, # direction
ff.Trigger(0, 0), # no triggers
ff.Replay(duration_ms, 0), # length and delay
ff.EffectType(ff_rumble_effect=rumble))
print("Uploading FF effect...")
effect_id = dev.upload_effect(effect)