class ThrusterController:
def __init__(self, simulate=False):
# setup motor controller. The PWM controller can control up to 16
# different devices. We have to add devices, one for each thruster that
# we can control. The first parameter is the human-friendly name of the
# device. That is used for logging to the console and/or a database. The
# next parameter indicates which PWM connector this device is connected
# to. This is refered to as the PWM channel. The last two values
# indicate at what time intervals (ticks) the PWM should turn on and
# off, respectively. We simply start each device at 0 time and control
# the duration of the pulses by adjusting the off time. Note that we may
# be able to shuffle on/off times to even out the current draw from the
# thrusters, but so far, that hasn't been an issue. It's even possible
# that the PWM controller may do that for us already.
if simulate is False:
from pwm_controller import PWMController
self.motor_controller = PWMController()
self.motor_controller.add_device("HL", HL, 0, NEUTRAL)
self.motor_controller.add_device("VL", VL, 0, NEUTRAL)
self.motor_controller.add_device("VC", VC, 0, NEUTRAL)
self.motor_controller.add_device("VR", VR, 0, NEUTRAL)
self.motor_controller.add_device("HR", HR, 0, NEUTRAL)
self.motor_controller.add_device("LIGHT", LIGHT, 0, FULL_REVERSE)
else:
self.motor_controller = None
# setup the joysticks. We use a 2D vector to represent the x and y
# values of the joysticks.
self.j1 = Vector2D()
self.j2 = Vector2D()
# create interpolators
self.horizontal_left = Interpolator()
self.vertical_left = Interpolator()
self.vertical_center = Interpolator()
self.vertical_right = Interpolator()
self.horizontal_right = Interpolator()
# setup interpolators from a file or manually
if os.path.isfile(SETTINGS_FILE):
with open(SETTINGS_FILE, 'r') as f:
self.set_settings(json.load(f), False)
else:
# Set the sensitivity to be applied to each thruster. 0 indicates a
# linear response which is the default when no sensitivity is applied. 1
# indicates full sensitivity. Values between 0 and 1 can be used to
# increase and to decrease the overall sensitivity. Increasing sensivity
# dampens lower values and amplifies larger values giving more precision
# at lower power levels.
self.sensitivity = 0.7
# We use a cubic to apply sensitivity. If you find that full sensitivity
# (dampening) does not give you fine enough control, you can increase\
# the degree of the polynomial used for dampening. Note that this must
# be a positive odd number. Any other values will cause unexpected
# results.
self.power = 3
# setup the various interpolators for each thruster. Each item we add
# to the interpolator consists of two values: an angle in degrees and a
# thrust value. An interpolator works by returning a value for any given
# input value. More specifically in this case, we will give each
# interpolator an angle and it will return a thrust value for that
# angle. Since we have only given the interpolator values for very
# specific angles, it will have to determine values for angles we have
# not provided. It does this using linear interpolation.
self.horizontal_left.addIndexValue(0.0, -1.0)
self.horizontal_left.addIndexValue(90.0, 1.0)
self.horizontal_left.addIndexValue(180.0, 1.0)
self.horizontal_left.addIndexValue(270.0, -1.0)
self.horizontal_left.addIndexValue(360.0, -1.0)
self.vertical_left.addIndexValue(0.0, 1.0)
self.vertical_left.addIndexValue(90.0, -1.0)
self.vertical_left.addIndexValue(180.0, -1.0)
self.vertical_left.addIndexValue(270.0, 1.0)
self.vertical_left.addIndexValue(360.0, 1.0)
self.vertical_center.addIndexValue(0.0, 0.0)
self.vertical_center.addIndexValue(90.0, 1.0)
self.vertical_center.addIndexValue(180.0, 0.0)
self.vertical_center.addIndexValue(270.0, -1.0)
self.vertical_center.addIndexValue(360.0, 0.0)
self.vertical_right.addIndexValue(0.0, -1.0)
self.vertical_right.addIndexValue(90.0, -1.0)
self.vertical_right.addIndexValue(180.0, 1.0)
self.vertical_right.addIndexValue(270.0, 1.0)
self.vertical_right.addIndexValue(360.0, -1.0)
self.horizontal_right.addIndexValue(0.0, 1.0)
self.horizontal_right.addIndexValue(90.0, 1.0)
self.horizontal_right.addIndexValue(180.0, -1.0)
self.horizontal_right.addIndexValue(270.0, -1.0)
self.horizontal_right.addIndexValue(360.0, 1.0)
# setup ascent/descent controllers
self.ascent = -1.0
self.descent = -1.0
# setup light
self.light = 0.0
def __del__(self):
'''
When an instance of this class gets destroyed, we need to make sure that
we turn off all motors. Otherwise, we could end up in a situation where
the vehicle could have thrusters running when we don't have scripts
running to control it.
'''
self.set_motor(HL, 0.0)
self.set_motor(VL, 0.0)
self.set_motor(VC, 0.0)
self.set_motor(VL, 0.0)
self.set_motor(HR, 0.0)
def update_axis(self, axis, value):
'''
This is the main method of this class. It is responsible for taking an
controller input value (referred to as an axis value) and then
converting that into the appropriate thrust values for the appropriate
thrusters associated with that axis.
For the two joysticks, we convert the joystick position into an angle.
We know which thrusters each joystick controls, so we feed the
calculated angle into the thruster interpolators for that joystick. This
gives us the new thruster value for each thruster, which we then apply
to the PWM controller devices for those thrusters.
Note that the angle of the joystick does not give us all of the
information that we need. If the joystick is close to the center
position, then we don't need to apply as much thrust. If it is pushed
all the way to the edge, then we nee 100% thrust. So, we treat the
center as 0% and the edge as 100%. The values we get back from the
interpolators are 100% values, so we simply apply the joystick
percentage to the interpolator value to find the actual thrust value we
need to use.
Things get a bit more complicated for the vertical thrusters because it
is possible that we will be pitiching or rolling the vehicle while
simultaneously trying to move the vehicle directly up or down. If we
pitch or roll the vehicle only, then the process is exactly as we
described above. However, if are pithing and/or rolling AND moveing the
vehicle vertically, we need to combine the two operations into one set
of thruster values. We have to first determine the values for pitch and
roll, then we increase or decrease all thruster values equally in the up
or down direction. However it is possible that we will not be able to
increase/decrease all thrusters by the same amount since we are already
applying thrust for pitch and roll. This means we need to make sure our
values do not go outside the closed intervale [-1,1]. This means that as
we pitch or roll harder, the vehical will flattern out as we apply
vertical thrust.
'''
# We need to keep track of which thrusters need updating. We use the
# following flags for that purpose
update_horizontal_thrusters = False
update_vertical_thrusters = False
# Round the incoming value to the specified precision to reduce input
# noise
value = round(value, PRECISION)
# Update the appropriate joystick vector based on which controller axis
# has changed. Note that we make sure the value is different from what
# we have already to prevent unnecessary updates. Recall that the
# controller may send values whose differences are smaller than our
# precision. This means we will get an update from the controller, but
# we decided to ignore it since it won't result in a significant change
# to our thrusters.
if axis == JL_H:
if self.j1.x != value:
self.j1.x = value
update_horizontal_thrusters = True
elif axis == JL_V:
if self.j1.y != value:
self.j1.y = value
update_horizontal_thrusters = True
elif axis == JR_H:
if self.j2.x != value:
self.j2.x = value
update_vertical_thrusters = True
elif axis == JR_V:
if self.j2.y != value:
self.j2.y = value
update_vertical_thrusters = True
elif axis == AL:
if self.descent != value:
self.descent = value
update_vertical_thrusters = True
elif axis == AR:
if self.ascent != value:
self.ascent = value
update_vertical_thrusters = True
else:
pass
# print("unknown axis ", event.axis)
# updating horizontal thrusters is easy: find current angle, convert
# angle to thruster values, apply values
if update_horizontal_thrusters:
left_value = self.horizontal_left.valueAtIndex(self.j1.angle)
right_value = self.horizontal_right.valueAtIndex(self.j1.angle)
power = min(1.0, self.j1.length)
self.set_motor(HL, left_value * power)
self.set_motor(HR, right_value * power)
# updating vertical thrusters is trickier. We do the same as above, but
# then post-process the values if we are applying vertical up/down
# thrust. As mentioned above, we have to be careful to stay within our
# [-1,1] interval.
if update_vertical_thrusters:
power = min(1.0, self.j2.length)
back_value = self.vertical_center.valueAtIndex(self.j2.angle) * power
front_left_value = self.vertical_left.valueAtIndex(self.j2.angle) * power
front_right_value = self.vertical_right.valueAtIndex(self.j2.angle) * power
if self.ascent != -1.0:
percent = (1.0 + self.ascent) / 2.0
max_thrust = max(back_value, front_left_value, front_right_value)
max_adjust = (1.0 - max_thrust) * percent
# back_value += max_adjust
front_left_value += max_adjust
front_right_value += max_adjust
elif self.descent != -1.0:
percent = (1.0 + self.descent) / 2.0
min_thrust = min(back_value, front_left_value, front_right_value)
max_adjust = (min_thrust - -1.0) * percent
# back_value -= max_adjust
front_left_value -= max_adjust
front_right_value -= max_adjust
self.set_motor(VC, back_value)
self.set_motor(VL, front_left_value)
self.set_motor(VR, front_right_value)
def update_button(self, button, value):
if button == UP:
self.light = min(1.0, self.light + LIGHT_STEP)
elif button == DOWN:
self.light = max(0.0, self.light - LIGHT_STEP)
elif button == RESET:
self.light = 0.0
light_value = map_range(self.light, 0.0, 1.0, -1.0, 1.0)
print("button %s, light = %s, light_value = %s" % (button, self.light, light_value))
self.set_motor(LIGHT, light_value)
def set_motor(self, motor_number, value):
if self.motor_controller is not None:
motor = self.motor_controller.devices[motor_number]
value = self.apply_sensitivity(value)
pwm_value = int(map_range(value, -1.0, 1.0, FULL_REVERSE, FULL_FORWARD))
# print("setting motor {0} to {1}".format(motor_number, pwm_value))
motor.off = pwm_value
def apply_sensitivity(self, value):
return self.sensitivity * value**self.power + (1.0 - self.sensitivity) * value
def get_settings(self):
return {
'version': 1,
'sensitivity': {
'strength': self.sensitivity,
'power': self.power
},
'thrusters': [
self.horizontal_left.to_array(),
self.vertical_left.to_array(),
self.vertical_center.to_array(),
self.vertical_right.to_array(),
self.horizontal_right.to_array()
]
}
def set_settings(self, data, save=True):
if data['version'] == 1:
# save settings for future loading
if save:
if data['name'] == "":
filename = SETTINGS_FILE
else:
filename = os.path.join("settings", data['name'] + ".json")
with open(filename, 'w') as out:
out.write(json.dumps(data, indent=2))
# update current settings
self.sensitivity = float(data['sensitivity']['strength'])
self.power = float(data['sensitivity']['power'])
self.horizontal_left.from_array(data['thrusters'][0])
self.vertical_left.from_array(data['thrusters'][1])
self.vertical_center.from_array(data['thrusters'][2])
self.vertical_right.from_array(data['thrusters'][3])
self.horizontal_right.from_array(data['thrusters'][4])
else:
print("Unsupported data version number '{}'".format(data['version']))
