Finish the pistol grip code
Change-Id: I95c03a95ac0ec64b4314ec310ad6535176b1d529
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index b6f6317..4737927 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -103,6 +103,19 @@
],
)
+# TODO(austin): Select isn't working right. We should be able to remove
+# logging conditionally with select and have CPU constraints work correctly.
+cc_library(
+ name = 'state_feedback_loop_uc',
+ hdrs = [
+ 'state_feedback_loop.h',
+ ],
+ deps = [
+ '//aos/common:macros',
+ '//third_party/eigen',
+ ],
+ restricted_to = ['//tools:cortex-m4f'],
+)
cc_library(
name = 'state_feedback_loop',
hdrs = [
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 05fc4a3..c499243 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -160,3 +160,62 @@
'//y2016/control_loops/drivetrain:polydrivetrain_plants',
],
)
+
+genrule(
+ name = 'genrule_haptic_wheel',
+ visibility = ['//visibility:private'],
+ cmd = '$(location //frc971/control_loops/python:haptic_wheel) $(OUTS)',
+ tools = [
+ '//frc971/control_loops/python:haptic_wheel',
+ ],
+ outs = [
+ 'haptic_wheel.h',
+ 'haptic_wheel.cc',
+ 'integral_haptic_wheel.h',
+ 'integral_haptic_wheel.cc',
+ 'haptic_trigger.h',
+ 'haptic_trigger.cc',
+ 'integral_haptic_trigger.h',
+ 'integral_haptic_trigger.cc',
+ ],
+ restricted_to = ["//tools:k8", "//tools:roborio", "//tools:armhf-debian", "//tools:cortex-m4f"],
+)
+
+cc_library(
+ name = 'haptic_input_uc',
+ hdrs = [
+ 'haptic_wheel.h',
+ 'integral_haptic_wheel.h',
+ 'haptic_trigger.h',
+ 'integral_haptic_trigger.h',
+ ],
+ srcs = [
+ 'haptic_wheel.cc',
+ 'integral_haptic_wheel.cc',
+ 'haptic_trigger.cc',
+ 'integral_haptic_trigger.cc',
+ ],
+ deps = [
+ '//frc971/control_loops:state_feedback_loop_uc',
+ ],
+ restricted_to = ["//tools:cortex-m4f"],
+)
+
+cc_library(
+ name = 'haptic_wheel',
+ hdrs = [
+ 'haptic_wheel.h',
+ 'integral_haptic_wheel.h',
+ 'haptic_trigger.h',
+ 'integral_haptic_trigger.h',
+ ],
+ srcs = [
+ 'haptic_wheel.cc',
+ 'integral_haptic_wheel.cc',
+ 'haptic_trigger.cc',
+ 'integral_haptic_trigger.cc',
+ ],
+ deps = [
+ '//frc971/control_loops:state_feedback_loop',
+ ],
+)
diff --git a/frc971/control_loops/python/BUILD b/frc971/control_loops/python/BUILD
index 4b25bb4..6ece1f4 100644
--- a/frc971/control_loops/python/BUILD
+++ b/frc971/control_loops/python/BUILD
@@ -1,5 +1,18 @@
package(default_visibility = ['//visibility:public'])
+py_binary(
+ name = 'haptic_wheel',
+ srcs = [
+ 'haptic_wheel.py',
+ ],
+ deps = [
+ '//external:python-gflags',
+ '//external:python-glog',
+ '//frc971/control_loops/python:controls',
+ ],
+ restricted_to = ['//tools:k8'],
+)
+
py_library(
name = 'controls',
srcs = [
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 82d138a..f88a5f2 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -535,3 +535,19 @@
self.Kv = (self.free_speed / (12.0 - self.resistance * self.free_current))
# Torque constant (N * m / A)
self.Kt = self.stall_torque / self.stall_current
+
+class MN3510(object):
+ def __init__(self):
+ # http://www.robotshop.com/en/t-motor-navigator-mn3510-360kv-brushless-motor.html#Specifications
+ # Free Current in Amps
+ self.free_current = 0.0
+ # Resistance of the motor
+ self.resistance = 0.188
+ # Stall Current in Amps
+ self.stall_current = 14.0 / self.resistance
+ # Motor velocity constant
+ self.Kv = 360.0 / 60.0 * (2.0 * numpy.pi)
+ # Torque constant Nm / A
+ self.Kt = 1.0 / self.Kv
+ # Stall Torque in N m
+ self.stall_torque = self.Kt * self.stall_current
diff --git a/frc971/control_loops/python/haptic_wheel.py b/frc971/control_loops/python/haptic_wheel.py
new file mode 100755
index 0000000..6c88e15
--- /dev/null
+++ b/frc971/control_loops/python/haptic_wheel.py
@@ -0,0 +1,406 @@
+#!/usr/bin/python
+
+from frc971.control_loops.python import control_loop
+from frc971.control_loops.python import controls
+import numpy
+import sys
+import copy
+import scipy.interpolate
+from matplotlib import pylab
+
+import gflags
+import glog
+
+FLAGS = gflags.FLAGS
+
+gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
+gflags.DEFINE_string('data', None, 'If defined, plot the provided CAN data')
+gflags.DEFINE_bool('rerun_kf', False, 'If true, rerun the KF. The torque in the data file will be interpreted as the commanded current.')
+
+class SystemParams(object):
+ def __init__(self, J, G, kP, kD, kCompensationTimeconstant, q_pos, q_vel,
+ q_torque, current_limit):
+ self.J = J
+ self.G = G
+ self.q_pos = q_pos
+ self.q_vel = q_vel
+ self.q_torque = q_torque
+ self.kP = kP
+ self.kD = kD
+ self.kCompensationTimeconstant = kCompensationTimeconstant
+ self.r_pos = 0.001
+ self.current_limit = current_limit
+
+ #[15.0, 0.25],
+ #[10.0, 0.2],
+ #[5.0, 0.13],
+ #[3.0, 0.10],
+ #[2.0, 0.08],
+ #[1.0, 0.06],
+ #[0.5, 0.05],
+ #[0.25, 0.025],
+
+kWheel = SystemParams(J=0.0008,
+ G=(1.25 + 0.02) / 0.35,
+ q_pos=0.001,
+ q_vel=0.20,
+ q_torque=0.005,
+ kP=7.0,
+ kD=0.0,
+ kCompensationTimeconstant=0.95,
+ current_limit=4.5)
+kTrigger = SystemParams(J=0.00025,
+ G=(0.925 * 2.0 + 0.02) / 0.35,
+ q_pos=0.001,
+ q_vel=0.1,
+ q_torque=0.005,
+ kP=120.0,
+ kD=1.8,
+ kCompensationTimeconstant=0.95,
+ current_limit=3.0)
+
+class HapticInput(control_loop.ControlLoop):
+ def __init__(self, params=None, name='HapticInput'):
+ # The defaults are for the steering wheel.
+ super(HapticInput, self).__init__(name)
+ motor = self.motor = control_loop.MN3510()
+
+ # Moment of inertia of the wheel in kg m^2
+ self.J = params.J
+
+ # Control loop time step
+ self.dt = 0.001
+
+ # Gear ratio from the motor to the input.
+ self.G = params.G
+
+ self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
+ self.A_continuous[1, 1] = 0
+ self.A_continuous[0, 1] = 1
+
+ self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
+ self.B_continuous[1, 0] = motor.Kt * self.G / self.J
+
+ # State feedback matrices
+ # [position, angular velocity]
+ self.C = numpy.matrix([[1.0, 0.0]])
+ self.D = numpy.matrix([[0.0]])
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ self.U_max = numpy.matrix([[2.5]])
+ self.U_min = numpy.matrix([[-2.5]])
+
+ self.L = numpy.matrix([[0.0], [0.0]])
+ self.K = numpy.matrix([[0.0, 0.0]])
+
+ self.InitializeState()
+
+
+class IntegralHapticInput(HapticInput):
+ def __init__(self, params=None, name="IntegralHapticInput"):
+ super(IntegralHapticInput, self).__init__(name=name, params=params)
+
+ self.A_continuous_unaugmented = self.A_continuous
+ self.B_continuous_unaugmented = self.B_continuous
+
+ self.A_continuous = numpy.matrix(numpy.zeros((3, 3)))
+ self.A_continuous[0:2, 0:2] = self.A_continuous_unaugmented
+ self.A_continuous[1, 2] = (1 / self.J)
+
+ self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
+ self.B_continuous[0:2, 0] = self.B_continuous_unaugmented
+
+ self.C_unaugmented = self.C
+ self.C = numpy.matrix(numpy.zeros((1, 3)))
+ self.C[0:1, 0:2] = self.C_unaugmented
+
+ self.A, self.B = self.ContinuousToDiscrete(
+ self.A_continuous, self.B_continuous, self.dt)
+
+ self.Q = numpy.matrix([[(params.q_pos ** 2.0), 0.0, 0.0],
+ [0.0, (params.q_vel ** 2.0), 0.0],
+ [0.0, 0.0, (params.q_torque ** 2.0)]])
+
+ self.R = numpy.matrix([[(params.r_pos ** 2.0)]])
+
+ self.KalmanGain, self.Q_steady = controls.kalman(
+ A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
+ self.L = self.A * self.KalmanGain
+
+ self.K_unaugmented = self.K
+ self.K = numpy.matrix(numpy.zeros((1, 3)))
+ self.K[0, 0:2] = self.K_unaugmented
+ self.K[0, 2] = 1.0 / (self.motor.Kt / (self.motor.resistance))
+
+ self.InitializeState()
+
+def ReadCan(filename):
+ """Reads the candump in filename and returns the 4 fields."""
+ trigger = []
+ trigger_velocity = []
+ trigger_torque = []
+ trigger_current = []
+ wheel = []
+ wheel_velocity = []
+ wheel_torque = []
+ wheel_current = []
+
+ trigger_request_time = [0.0]
+ trigger_request_current = [0.0]
+ wheel_request_time = [0.0]
+ wheel_request_current = [0.0]
+
+ with open(filename, 'r') as fd:
+ for line in fd:
+ data = line.split()
+ can_id = int(data[1], 16)
+ if can_id == 0:
+ data = [int(d, 16) for d in data[3:]]
+ trigger.append(((data[0] + (data[1] << 8)) - 32768) / 32768.0)
+ trigger_velocity.append(((data[2] + (data[3] << 8)) - 32768) / 32768.0)
+ trigger_torque.append(((data[4] + (data[5] << 8)) - 32768) / 32768.0)
+ trigger_current.append(((data[6] + ((data[7] & 0x3f) << 8)) - 8192) / 8192.0)
+ elif can_id == 1:
+ data = [int(d, 16) for d in data[3:]]
+ wheel.append(((data[0] + (data[1] << 8)) - 32768) / 32768.0)
+ wheel_velocity.append(((data[2] + (data[3] << 8)) - 32768) / 32768.0)
+ wheel_torque.append(((data[4] + (data[5] << 8)) - 32768) / 32768.0)
+ wheel_current.append(((data[6] + ((data[7] & 0x3f) << 8)) - 8192) / 8192.0)
+ elif can_id == 2:
+ data = [int(d, 16) for d in data[3:]]
+ trigger_request_current.append(((data[4] + (data[5] << 8)) - 32768) / 32768.0)
+ trigger_request_time.append(len(trigger) * 0.001)
+ elif can_id == 3:
+ data = [int(d, 16) for d in data[3:]]
+ wheel_request_current.append(((data[4] + (data[5] << 8)) - 32768) / 32768.0)
+ wheel_request_time.append(len(wheel) * 0.001)
+
+ trigger_data_time = numpy.arange(0, len(trigger)) * 0.001
+ wheel_data_time = numpy.arange(0, len(wheel)) * 0.001
+
+ # Extend out the data in the interpolation table.
+ trigger_request_time.append(trigger_data_time[-1])
+ trigger_request_current.append(trigger_request_current[-1])
+ wheel_request_time.append(wheel_data_time[-1])
+ wheel_request_current.append(wheel_request_current[-1])
+
+ return (trigger_data_time, wheel_data_time, trigger, wheel, trigger_velocity,
+ wheel_velocity, trigger_torque, wheel_torque, trigger_current,
+ wheel_current, trigger_request_time, trigger_request_current,
+ wheel_request_time, wheel_request_current)
+
+def rerun_and_plot_kf(data_time, data_radians, data_current, data_request_current,
+ params, run_correct=True):
+ kf_velocity = []
+ dt_velocity = []
+ kf_position = []
+ adjusted_position = []
+ last_angle = None
+ haptic_observer = IntegralHapticInput(params=params)
+
+ # Parameter sweep J.
+ num_kf = 1
+ min_J = max_J = params.J
+
+ # J = 0.0022
+ #num_kf = 15
+ #min_J = min_J / 2.0
+ #max_J = max_J * 2.0
+ initial_velocity = (data_radians[1] - data_radians[0]) * 1000.0
+
+ def DupParamsWithJ(params, J):
+ p = copy.copy(params)
+ p.J = J
+ return p
+ haptic_observers = [IntegralHapticInput(params=DupParamsWithJ(params, j)) for j in
+ numpy.logspace(numpy.log10(min_J),
+ numpy.log10(max_J), num=num_kf)]
+ # Initialize all the KF's.
+ haptic_observer.X_hat[1, 0] = initial_velocity
+ haptic_observer.X_hat[0, 0] = data_radians[0]
+ for observer in haptic_observers:
+ observer.X_hat[1, 0] = initial_velocity
+ observer.X_hat[0, 0] = data_radians[0]
+
+ last_request_current = data_request_current[0]
+ kf_torques = [[] for i in xrange(num_kf)]
+ for angle, current, request_current in zip(data_radians, data_current,
+ data_request_current):
+ # Predict and correct all the parameter swept observers.
+ for i, observer in enumerate(haptic_observers):
+ observer.Y = numpy.matrix([[angle]])
+ if run_correct:
+ observer.CorrectObserver(numpy.matrix([[current]]))
+ kf_torques[i].append(-observer.X_hat[2, 0])
+ observer.PredictObserver(numpy.matrix([[current]]))
+ observer.PredictObserver(numpy.matrix([[current]]))
+
+ # Predict and correct the main observer.
+ haptic_observer.Y = numpy.matrix([[angle]])
+ if run_correct:
+ haptic_observer.CorrectObserver(numpy.matrix([[current]]))
+ kf_position.append(haptic_observer.X_hat[0, 0])
+ adjusted_position.append(kf_position[-1] - last_request_current / params.kP)
+ last_request_current = last_request_current * params.kCompensationTimeconstant + request_current * (1.0 - params.kCompensationTimeconstant)
+ kf_velocity.append(haptic_observer.X_hat[1, 0])
+ if last_angle is None:
+ last_angle = angle
+ dt_velocity.append((angle - last_angle) / 0.001)
+
+ haptic_observer.PredictObserver(numpy.matrix([[current]]))
+ last_angle = angle
+
+ # Plot the wheel observers.
+ fig, ax1 = pylab.subplots()
+ ax1.plot(data_time, data_radians, '.', label='wheel')
+ ax1.plot(data_time, dt_velocity, '.', label='dt_velocity')
+ ax1.plot(data_time, kf_velocity, '.', label='kf_velocity')
+ ax1.plot(data_time, kf_position, '.', label='kf_position')
+ ax1.plot(data_time, adjusted_position, '.', label='adjusted_position')
+
+ ax2 = ax1.twinx()
+ ax2.plot(data_time, data_current, label='data_current')
+ ax2.plot(data_time, data_request_current, label='request_current')
+
+ for i, kf_torque in enumerate(kf_torques):
+ ax2.plot(data_time, kf_torque,
+ label='-kf_torque[%f]' % haptic_observers[i].J)
+ fig.tight_layout()
+ ax1.legend(loc=3)
+ ax2.legend(loc=4)
+
+def plot_input(data_time, data_radians, data_velocity, data_torque,
+ data_current, params, run_correct=True):
+ dt_velocity = []
+ last_angle = None
+ initial_velocity = (data_radians[1] - data_radians[0]) * 1000.0
+
+ for angle in data_radians:
+ if last_angle is None:
+ last_angle = angle
+ dt_velocity.append((angle - last_angle) / 0.001)
+
+ last_angle = angle
+
+ # Plot the wheel observers.
+ fig, ax1 = pylab.subplots()
+ ax1.plot(data_time, data_radians, '.', label='angle')
+ ax1.plot(data_time, data_velocity, '-', label='velocity')
+ ax1.plot(data_time, dt_velocity, '.', label='dt_velocity')
+
+ ax2 = ax1.twinx()
+ ax2.plot(data_time, data_torque, label='data_torque')
+ ax2.plot(data_time, data_current, label='data_current')
+ fig.tight_layout()
+ ax1.legend(loc=3)
+ ax2.legend(loc=4)
+
+def main(argv):
+ if FLAGS.plot:
+ if FLAGS.data is None:
+ haptic_wheel = HapticInput()
+ haptic_wheel_controller = IntegralHapticInput()
+ observer_haptic_wheel = IntegralHapticInput()
+ observer_haptic_wheel.X_hat[2, 0] = 0.01
+
+ R = numpy.matrix([[0.0], [0.0], [0.0]])
+
+ control_loop.TestSingleIntegralAxisSquareWave(
+ R, 1.0, haptic_wheel, haptic_wheel_controller, observer_haptic_wheel)
+ else:
+ # Read the CAN trace in.
+ trigger_data_time, wheel_data_time, trigger, wheel, trigger_velocity, \
+ wheel_velocity, trigger_torque, wheel_torque, trigger_current, \
+ wheel_current, trigger_request_time, trigger_request_current, \
+ wheel_request_time, wheel_request_current = ReadCan(FLAGS.data)
+
+ wheel_radians = [w * numpy.pi * (338.16 / 360.0) for w in wheel]
+ wheel_velocity = [w * 50.0 for w in wheel_velocity]
+ wheel_torque = [w / 2.0 for w in wheel_torque]
+ wheel_current = [w * 10.0 for w in wheel_current]
+ wheel_request_current = [w * 2.0 for w in wheel_request_current]
+ resampled_wheel_request_current = scipy.interpolate.interp1d(
+ wheel_request_time, wheel_request_current, kind="zero")(wheel_data_time)
+
+ trigger_radians = [t * numpy.pi * (45.0 / 360.0) for t in trigger]
+ trigger_velocity = [t * 50.0 for t in trigger_velocity]
+ trigger_torque = [t / 2.0 for t in trigger_torque]
+ trigger_current = [t * 10.0 for t in trigger_current]
+ trigger_request_current = [t * 2.0 for t in trigger_request_current]
+ resampled_trigger_request_current = scipy.interpolate.interp1d(
+ trigger_request_time, trigger_request_current, kind="zero")(trigger_data_time)
+
+ if FLAGS.rerun_kf:
+ rerun_and_plot_kf(trigger_data_time, trigger_radians, trigger_current,
+ resampled_trigger_request_current, kTrigger, run_correct=True)
+ rerun_and_plot_kf(wheel_data_time, wheel_radians, wheel_current,
+ resampled_wheel_request_current, kWheel, run_correct=True)
+ else:
+ plot_input(trigger_data_time, trigger_radians, trigger_velocity,
+ trigger_torque, trigger_current, kTrigger)
+ plot_input(wheel_data_time, wheel_radians, wheel_velocity, wheel_torque,
+ wheel_current, kWheel)
+ pylab.show()
+
+ return
+
+ if len(argv) != 9:
+ glog.fatal('Expected .h file name and .cc file name')
+ else:
+ namespaces = ['frc971', 'control_loops', 'drivetrain']
+ for name, params, filenames in [('HapticWheel', kWheel, argv[1:5]),
+ ('HapticTrigger', kTrigger, argv[5:9])]:
+ haptic_input = HapticInput(params=params, name=name)
+ loop_writer = control_loop.ControlLoopWriter(name, [haptic_input],
+ namespaces=namespaces,
+ scalar_type='float')
+ loop_writer.Write(filenames[0], filenames[1])
+
+ integral_haptic_input = IntegralHapticInput(params=params,
+ name='Integral' + name)
+ integral_loop_writer = control_loop.ControlLoopWriter(
+ 'Integral' + name, [integral_haptic_input], namespaces=namespaces,
+ scalar_type='float')
+
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "Dt", "%f",
+ integral_haptic_input.dt))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "FreeCurrent", "%f",
+ integral_haptic_input.motor.free_current))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "StallTorque", "%f",
+ integral_haptic_input.motor.stall_torque))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "J", "%f",
+ integral_haptic_input.J))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "R", "%f",
+ integral_haptic_input.motor.resistance))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "T", "%f",
+ integral_haptic_input.motor.Kt))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "V", "%f",
+ integral_haptic_input.motor.Kv))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "P", "%f",
+ params.kP))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "D", "%f",
+ params.kD))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "G", "%f",
+ params.G))
+ integral_loop_writer.AddConstant(
+ control_loop.Constant("k" + name + "CurrentLimit", "%f",
+ params.current_limit))
+
+ integral_loop_writer.Write(filenames[2], filenames[3])
+
+
+if __name__ == '__main__':
+ argv = FLAGS(sys.argv)
+ sys.exit(main(argv))
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index b9765fc..a1e6cd4 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -12,7 +12,9 @@
#include "Eigen/Dense"
#include "unsupported/Eigen/MatrixFunctions"
+#if defined(__linux__)
#include "aos/common/logging/logging.h"
+#endif
#include "aos/common/macros.h"
template <int number_of_states, int number_of_inputs, int number_of_outputs,
@@ -151,7 +153,11 @@
for (int i = 0; i < kNumInputs; ++i) {
if (U(i, 0) > U_max(i, 0) + static_cast<Scalar>(0.00001) ||
U(i, 0) < U_min(i, 0) - static_cast<Scalar>(0.00001)) {
+#if defined(__linux__)
LOG(FATAL, "U out of range\n");
+#else
+ abort();
+#endif
}
}
}
diff --git a/motors/algorithms.h b/motors/algorithms.h
index d1fce7a..8b397f8 100644
--- a/motors/algorithms.h
+++ b/motors/algorithms.h
@@ -26,6 +26,19 @@
// and the corresponding outputs will be inversely proportional to the weights.
BalancedReadings BalanceReadings(ReadingsToBalance to_balance);
+inline BalancedReadings BalanceSimpleReadings(const uint16_t readings[3]) {
+ float offset = 0;
+ for (int i = 0; i < 3; ++i) {
+ offset += static_cast<float>(readings[i]);
+ }
+
+ BalancedReadings r;
+ for (int i = 0; i < 3; ++i) {
+ r.readings[i] = static_cast<float>(readings[i]) + offset / -3;
+ }
+ return r;
+}
+
} // namespace salsa
} // namespace frc971
diff --git a/motors/core/kinetis.h b/motors/core/kinetis.h
index 36ab5f2..2961a4f 100644
--- a/motors/core/kinetis.h
+++ b/motors/core/kinetis.h
@@ -3228,8 +3228,8 @@
#define FTM0_CONF (*(volatile uint32_t *)0x40038084) // Configuration
#define FTM_CONF_GTBEOUT 0x400 // Global Time Base Output
#define FTM_CONF_GTBEEN 0x200 // Global Time Base Enable
-#define FTM_CONF_BDMMODE (((n) & 3) << 6) // Behavior when in debug mode
-#define FTM_CONF_NUMTOF (((n) & 31) << 0) // ratio of counter overflows to TOF bit set
+#define FTM_CONF_BDMMOD(n) (((n) & 3) << 6) // Behavior when in debug mode
+#define FTM_CONF_NUMTOF(n) (((n) & 31) << 0) // ratio of counter overflows to TOF bit set
#define FTM0_FLTPOL (*(volatile uint32_t *)0x40038088) // FTM Fault Input Polarity
#define FTM_FLTPOL_FLT3POL 0x08 // Fault Input 3 Polarity
#define FTM_FLTPOL_FLT2POL 0x04 // Fault Input 2 Polarity
diff --git a/motors/deploy_joystick.sh b/motors/deploy_joystick.sh
new file mode 100755
index 0000000..db0d855
--- /dev/null
+++ b/motors/deploy_joystick.sh
@@ -0,0 +1,4 @@
+#!/bin/bash
+# Deploy to the driver station side of the pistol grip.
+
+bazel build --cpu=cortex-m4f -c opt //motors/pistol_grip:drivers_station.hex && bazel run //motors/teensy_loader_cli -- --mcu=mk64fx512 -s $(readlink -f bazel-bin/motors/pistol_grip/drivers_station.hex)
diff --git a/motors/motor.cc b/motors/motor.cc
index d966e6a..af7a3c7 100644
--- a/motors/motor.cc
+++ b/motors/motor.cc
@@ -10,6 +10,7 @@
#include "motors/peripheral/can.h"
extern "C" float analog_ratio(uint16_t reading);
+extern "C" float absolute_wheel(uint16_t reading);
namespace frc971 {
namespace salsa {
@@ -194,7 +195,7 @@
// If we fire phase 2, we should go to -2.0 * PI / 3.0 radians.
// If we fire phase 3, we should go to 2.0 * PI / 3.0 radians.
// These numbers were confirmed by the python motor simulation.
- static int phase_to_fire_count = -500000;
+ static int phase_to_fire_count = -300000;
static int phase_to_fire = 0;
++phase_to_fire_count;
if (phase_to_fire_count > 200000) {
@@ -217,7 +218,8 @@
output_registers_[2][0] = 0;
output_registers_[2][2] = phase_to_fire == 2 ? kPhaseFireWidth : 0;
#endif
-
+ (void)balanced;
+ (void)captured_wrapped_encoder;
#if PRINT_READINGS
static int i = 0;
if (i == 1000) {
@@ -227,12 +229,19 @@
DisableInterrupts disable_interrupts;
readings = AdcReadSmallInit(disable_interrupts);
}
+ //printf(
+ //"enc %" PRIu32 " %d %d\n", captured_wrapped_encoder,
+ //static_cast<int>((1.0f - analog_ratio(readings.motor1_abs)) * 7000.0f),
+ //static_cast<int>(captured_wrapped_encoder * 7.0f / 4096.0f * 1000.0f));
+ float wheel_position = absolute_wheel(analog_ratio(readings.wheel_abs));
+
printf(
- "enc %d %d %d\n", captured_wrapped_encoder,
- static_cast<int>((1.0f - analog_ratio(readings.motor0_abs)) * 7000.0f),
- static_cast<int>(captured_wrapped_encoder * 7.0f / 4096.0f * 1000.0f));
+ "ecnt %" PRIu32 " arev:%d erev:%d %d\n", captured_wrapped_encoder,
+ static_cast<int>((analog_ratio(readings.motor1_abs)) * 7000.0f),
+ static_cast<int>(captured_wrapped_encoder * 7.0f / 4096.0f * 1000.0f),
+ static_cast<int>(wheel_position * 1000.0f));
} else if (i == 200) {
-#ifdef DO_CONTROLS
+#if DO_CONTROLS
printf("out %" PRIu32 " %" PRIu32 " %" PRIu32 "\n", switching_points[0],
switching_points[1], switching_points[2]);
#else
diff --git a/motors/motor.h b/motors/motor.h
index 8161f5e..c59458c 100644
--- a/motors/motor.h
+++ b/motors/motor.h
@@ -63,24 +63,50 @@
void set_switching_divisor(int switching_divisor) {
switching_divisor_ = switching_divisor;
}
- void set_encoder_offset(int encoder_offset) {
+ void set_encoder_offset(int32_t encoder_offset) {
encoder_offset_ = encoder_offset;
+ last_wrapped_encoder_reading_ = wrapped_encoder();
+ }
+ int32_t encoder_offset() const { return encoder_offset_; }
+
+ void set_encoder_calibration_offset(int encoder_offset) {
+ encoder_calibration_offset_ = encoder_offset;
// Add mechanical_counts_per_revolution to the offset so that when we mod
// below, we are guaranteed to be > 0 regardless of the encoder multiplier.
// % isn't well-defined with negative numbers.
- while (encoder_offset_ < controls_->mechanical_counts_per_revolution()) {
- encoder_offset_ += controls_->mechanical_counts_per_revolution();
+ while (encoder_calibration_offset_ <
+ controls_->mechanical_counts_per_revolution()) {
+ encoder_calibration_offset_ +=
+ controls_->mechanical_counts_per_revolution();
}
}
void set_encoder_multiplier(int encoder_multiplier) {
encoder_multiplier_ = encoder_multiplier;
}
+ int32_t absolute_encoder(uint32_t wrapped_encoder_reading) {
+ const uint32_t counts_per_revolution =
+ controls_->mechanical_counts_per_revolution();
+ const uint32_t wrap_down = counts_per_revolution / 4;
+ const uint32_t wrap_up = wrap_down * 3;
+ if (last_wrapped_encoder_reading_ > wrap_up &&
+ wrapped_encoder_reading < wrap_down) {
+ encoder_offset_ += counts_per_revolution;
+ } else if (last_wrapped_encoder_reading_ < wrap_down &&
+ wrapped_encoder_reading > wrap_up) {
+ encoder_offset_ -= counts_per_revolution;
+ }
+
+ last_wrapped_encoder_reading_ = wrapped_encoder_reading;
+
+ return static_cast<int32_t>(wrapped_encoder_reading) + encoder_offset_;
+ }
+
int encoder() {
return encoder_multiplier_ * encoder_ftm_->CNT;
}
uint32_t wrapped_encoder() {
- return (encoder() + encoder_offset_) %
+ return (encoder() + encoder_calibration_offset_) %
controls_->mechanical_counts_per_revolution();
}
@@ -148,8 +174,10 @@
float goal_current_ = 0;
uint32_t last_current_set_time_ = 0;
int switching_divisor_ = 1;
- int encoder_offset_ = 0;
+ int encoder_calibration_offset_ = 0;
+ int32_t encoder_offset_ = 0;
int encoder_multiplier_ = 1;
+ uint32_t last_wrapped_encoder_reading_ = 0;
teensy::AcmTty *debug_tty_ = nullptr;
};
diff --git a/motors/pistol_grip/BUILD b/motors/pistol_grip/BUILD
index 9eda585..503291f 100644
--- a/motors/pistol_grip/BUILD
+++ b/motors/pistol_grip/BUILD
@@ -24,6 +24,32 @@
)
cc_binary(
+ name = 'controller.elf',
+ srcs = [
+ 'vtable_wheel.cc',
+ 'vtable_trigger.cc',
+ 'controller.cc',
+ ],
+ deps = [
+ ':motor_controls',
+ '//motors:util',
+ '//motors:motor',
+ '//motors/core',
+ '//motors/peripheral:can',
+ '//motors/peripheral:adc',
+ '//motors/usb',
+ '//motors/usb:cdc',
+ '//frc971/control_loops/drivetrain:haptic_input_uc',
+ ],
+ restricted_to = mcu_cpus,
+)
+
+hex_from_elf(
+ name = 'controller',
+ restricted_to = mcu_cpus,
+)
+
+cc_binary(
name = 'usb_forward_linux',
srcs = [
'usb_forward.cc',
@@ -56,3 +82,21 @@
]),
output_to_bindir = True,
)
+
+cc_library(
+ name = 'motor_controls',
+ visibility = ['//visibility:public'],
+ hdrs = [
+ 'motor_controls.h',
+ ],
+ srcs = [
+ 'motor_controls.cc',
+ ],
+ deps = [
+ '//motors:math',
+ '//motors:motor',
+ '//motors/peripheral:configuration',
+ '//third_party/eigen',
+ ],
+ restricted_to = mcu_cpus,
+)
diff --git a/motors/pistol_grip/controller.cc b/motors/pistol_grip/controller.cc
new file mode 100644
index 0000000..4460355
--- /dev/null
+++ b/motors/pistol_grip/controller.cc
@@ -0,0 +1,849 @@
+#include "motors/core/kinetis.h"
+
+#include <stdio.h>
+#include <inttypes.h>
+
+#include <atomic>
+#include <cmath>
+
+#include "motors/core/time.h"
+#include "motors/motor.h"
+#include "motors/peripheral/adc.h"
+#include "motors/peripheral/can.h"
+#include "motors/pistol_grip/motor_controls.h"
+#include "motors/usb/cdc.h"
+#include "motors/usb/usb.h"
+#include "motors/util.h"
+#include "frc971/control_loops/drivetrain/integral_haptic_wheel.h"
+#include "frc971/control_loops/drivetrain/integral_haptic_trigger.h"
+
+#define MOTOR0_PWM_FTM FTM3
+#define MOTOR0_ENCODER_FTM FTM2
+#define MOTOR1_PWM_FTM FTM0
+#define MOTOR1_ENCODER_FTM FTM1
+
+extern const float kWheelCoggingTorque[4096];
+extern const float kTriggerCoggingTorque[4096];
+
+namespace frc971 {
+namespace salsa {
+namespace {
+
+using ::frc971::control_loops::drivetrain::MakeIntegralHapticTriggerPlant;
+using ::frc971::control_loops::drivetrain::MakeIntegralHapticTriggerObserver;
+using ::frc971::control_loops::drivetrain::MakeIntegralHapticWheelPlant;
+using ::frc971::control_loops::drivetrain::MakeIntegralHapticWheelObserver;
+
+constexpr float kHapticWheelCurrentLimit = static_cast<float>(
+ ::frc971::control_loops::drivetrain::kHapticWheelCurrentLimit);
+constexpr float kHapticTriggerCurrentLimit = static_cast<float>(
+ ::frc971::control_loops::drivetrain::kHapticTriggerCurrentLimit);
+
+::std::atomic<Motor *> global_motor0{nullptr}, global_motor1{nullptr};
+::std::atomic<teensy::AcmTty *> global_stdout{nullptr};
+
+// Angle last time the current loop ran.
+::std::atomic<float> global_wheel_angle{0.0f};
+::std::atomic<float> global_trigger_angle{0.0f};
+
+// Wheel observer/plant.
+::std::atomic<StateFeedbackObserver<3, 1, 1, float> *> global_wheel_observer{
+ nullptr};
+::std::atomic<StateFeedbackPlant<3, 1, 1, float> *> global_wheel_plant{nullptr};
+// Throttle observer/plant.
+::std::atomic<StateFeedbackObserver<3, 1, 1, float> *> global_trigger_observer{
+ nullptr};
+::std::atomic<StateFeedbackPlant<3, 1, 1, float> *> global_trigger_plant{
+ nullptr};
+
+// Torques for the current loop to apply.
+::std::atomic<float> global_wheel_current{0.0f};
+::std::atomic<float> global_trigger_torque{0.0f};
+
+constexpr int kSwitchingDivisor = 2;
+
+float analog_ratio(uint16_t reading) {
+ static constexpr uint16_t kMin = 260, kMax = 3812;
+ return static_cast<float>(::std::max(::std::min(reading, kMax), kMin) -
+ kMin) /
+ static_cast<float>(kMax - kMin);
+}
+
+constexpr float InterpolateFloat(float x1, float x0, float y1, float y0, float x) {
+ return (x - x0) * (y1 - y0) / (x1 - x0) + y0;
+}
+
+float absolute_wheel(float wheel_position) {
+ if (wheel_position < 0.43f) {
+ wheel_position += 1.0f;
+ }
+ wheel_position -= 0.462f + 0.473f;
+ return wheel_position;
+}
+
+extern "C" {
+
+void *__stack_chk_guard = (void *)0x67111971;
+void __stack_chk_fail() {
+ while (true) {
+ GPIOC_PSOR = (1 << 5);
+ printf("Stack corruption detected\n");
+ delay(1000);
+ GPIOC_PCOR = (1 << 5);
+ delay(1000);
+ }
+}
+
+int _write(int /*file*/, char *ptr, int len) {
+ teensy::AcmTty *const tty = global_stdout.load(::std::memory_order_acquire);
+ if (tty != nullptr) {
+ return tty->Write(ptr, len);
+ }
+ return 0;
+}
+
+extern uint32_t __bss_ram_start__[], __bss_ram_end__[];
+extern uint32_t __data_ram_start__[], __data_ram_end__[];
+extern uint32_t __heap_start__[], __heap_end__[];
+extern uint32_t __stack_end__[];
+
+} // extern "C"
+
+constexpr float kWheelMaxExtension = 1.0f;
+constexpr float kWheelFrictionMax = 0.2f;
+float WheelCenteringCurrent(float scalar, float angle, float velocity) {
+ float friction_goal_current = -angle * 10.0f;
+ if (friction_goal_current > kWheelFrictionMax) {
+ friction_goal_current = kWheelFrictionMax;
+ } else if (friction_goal_current < -kWheelFrictionMax) {
+ friction_goal_current = -kWheelFrictionMax;
+ }
+
+ constexpr float kWheelSpringNonlinearity = 0.45f;
+
+ float goal_current = -((1.0f - kWheelSpringNonlinearity) * angle +
+ kWheelSpringNonlinearity * angle * angle * angle) *
+ 6.0f -
+ velocity * 0.04f;
+ if (goal_current > 5.0f - scalar) {
+ goal_current = 5.0f - scalar;
+ } else if (goal_current < -5.0f + scalar) {
+ goal_current = -5.0f + scalar;
+ }
+
+ return goal_current * scalar + friction_goal_current;
+}
+
+extern "C" void ftm0_isr() {
+ SmallAdcReadings readings;
+ {
+ DisableInterrupts disable_interrupts;
+ readings = AdcReadSmall1(disable_interrupts);
+ }
+ uint32_t encoder =
+ global_motor1.load(::std::memory_order_relaxed)->wrapped_encoder();
+ int32_t absolute_encoder = global_motor1.load(::std::memory_order_relaxed)
+ ->absolute_encoder(encoder);
+
+ const float angle = absolute_encoder / static_cast<float>((15320 - 1488) / 2);
+ global_wheel_angle.store(angle);
+
+ float goal_current = -global_wheel_current.load(::std::memory_order_relaxed) +
+ kWheelCoggingTorque[encoder];
+
+ global_motor1.load(::std::memory_order_relaxed)->SetGoalCurrent(goal_current);
+ global_motor1.load(::std::memory_order_relaxed)
+ ->HandleInterrupt(BalanceSimpleReadings(readings.currents), encoder);
+}
+
+constexpr float kTriggerMaxExtension = -1.00f;
+constexpr float kTriggerCenter = 0.0f;
+float TriggerCenteringCurrent(float trigger_angle) {
+ float goal_current = (kTriggerCenter - trigger_angle) * 3.0f;
+ if (goal_current < -1.0f) {
+ goal_current = -1.0f;
+ } else if (goal_current > 1.0f) {
+ goal_current = 1.0f;
+ if (trigger_angle < kTriggerMaxExtension) {
+ goal_current -= (30.0f * (trigger_angle - kTriggerMaxExtension));
+ if (goal_current > 2.0f) {
+ goal_current = 2.0f;
+ }
+ }
+ }
+ return goal_current;
+}
+
+extern "C" void ftm3_isr() {
+ SmallAdcReadings readings;
+ {
+ DisableInterrupts disable_interrupts;
+ readings = AdcReadSmall0(disable_interrupts);
+ }
+ uint32_t encoder =
+ global_motor0.load(::std::memory_order_relaxed)->wrapped_encoder();
+ int32_t absolute_encoder = global_motor0.load(::std::memory_order_relaxed)
+ ->absolute_encoder(encoder);
+
+ float trigger_angle = absolute_encoder / 1370.f;
+
+ const float goal_current =
+ -global_trigger_torque.load(::std::memory_order_relaxed) +
+ kTriggerCoggingTorque[encoder];
+
+ global_motor0.load(::std::memory_order_relaxed)->SetGoalCurrent(goal_current);
+ global_motor0.load(::std::memory_order_relaxed)
+ ->HandleInterrupt(BalanceSimpleReadings(readings.currents), encoder);
+
+
+ global_trigger_angle.store(trigger_angle);
+}
+
+
+int ConvertFloat16(float val) {
+ int result = static_cast<int>(val * 32768.0f) + 32768;
+ if (result > 0xffff) {
+ result = 0xffff;
+ } else if (result < 0) {
+ result = 0;
+ }
+ return result;
+}
+int ConvertFloat14(float val) {
+ int result = static_cast<int>(val * 8192.0f) + 8192;
+ if (result > 0x3fff) {
+ result = 0x3fff;
+ } else if (result < 0) {
+ result = 0;
+ }
+ return result;
+}
+
+extern "C" void pit3_isr() {
+ PIT_TFLG3 = 1;
+ const float absolute_trigger_angle =
+ global_trigger_angle.load(::std::memory_order_relaxed);
+ const float absolute_wheel_angle =
+ global_wheel_angle.load(::std::memory_order_relaxed);
+
+ // Force a barrier here so we sample everything guaranteed at the beginning.
+ __asm__("" ::: "memory");
+ const float absolute_wheel_angle_radians =
+ absolute_wheel_angle * static_cast<float>(M_PI) * (338.16f / 360.0f);
+ const float absolute_trigger_angle_radians =
+ absolute_trigger_angle * static_cast<float>(M_PI) * (45.0f / 360.0f);
+
+ static uint32_t last_command_time = 0;
+ static float trigger_goal_position = 0.0f;
+ static float trigger_goal_velocity = 0.0f;
+ static float trigger_haptic_current = 0.0f;
+ static bool trigger_centering = true;
+ static bool trigger_haptics = false;
+ {
+ uint8_t data[8];
+ int length;
+ can_receive_command(data, &length, 0);
+ if (length > 0) {
+ last_command_time = micros();
+ trigger_goal_position =
+ static_cast<float>(
+ static_cast<int32_t>(static_cast<uint32_t>(data[0]) |
+ (static_cast<uint32_t>(data[1]) << 8)) -
+ 32768) /
+ 32768.0f * M_PI / 8.0;
+ trigger_goal_velocity =
+ static_cast<float>(
+ static_cast<int32_t>(static_cast<uint32_t>(data[2]) |
+ (static_cast<uint32_t>(data[3]) << 8)) -
+ 32768) /
+ 32768.0f * 4.0f;
+
+ trigger_haptic_current =
+ static_cast<float>(
+ static_cast<int32_t>(static_cast<uint32_t>(data[4]) |
+ (static_cast<uint32_t>(data[5]) << 8)) -
+ 32768) /
+ 32768.0f * 2.0f;
+ if (trigger_haptic_current > kHapticTriggerCurrentLimit) {
+ trigger_haptic_current = kHapticTriggerCurrentLimit;
+ } else if (trigger_haptic_current < -kHapticTriggerCurrentLimit) {
+ trigger_haptic_current = -kHapticTriggerCurrentLimit;
+ }
+ trigger_centering = !!(data[7] & 0x01);
+ trigger_haptics = !!(data[7] & 0x02);
+ }
+ }
+
+ static float wheel_goal_position = 0.0f;
+ static float wheel_goal_velocity = 0.0f;
+ static float wheel_haptic_current = 0.0f;
+ static float wheel_kp = 0.0f;
+ static bool wheel_centering = true;
+ static float wheel_centering_scalar = 0.25f;
+ {
+ uint8_t data[8];
+ int length;
+ can_receive_command(data, &length, 1);
+ if (length == 8) {
+ last_command_time = micros();
+ wheel_goal_position =
+ static_cast<float>(
+ static_cast<int32_t>(static_cast<uint32_t>(data[0]) |
+ (static_cast<uint32_t>(data[1]) << 8)) -
+ 32768) /
+ 32768.0f * M_PI;
+ wheel_goal_velocity =
+ static_cast<float>(
+ static_cast<int32_t>(static_cast<uint32_t>(data[2]) |
+ (static_cast<uint32_t>(data[3]) << 8)) -
+ 32768) /
+ 32768.0f * 10.0f;
+
+ wheel_haptic_current =
+ static_cast<float>(
+ static_cast<int32_t>(static_cast<uint32_t>(data[4]) |
+ (static_cast<uint32_t>(data[5]) << 8)) -
+ 32768) /
+ 32768.0f * 2.0f;
+ if (wheel_haptic_current > kHapticWheelCurrentLimit) {
+ wheel_haptic_current = kHapticWheelCurrentLimit;
+ } else if (wheel_haptic_current < -kHapticWheelCurrentLimit) {
+ wheel_haptic_current = -kHapticWheelCurrentLimit;
+ }
+ wheel_kp = static_cast<float>(data[6]) * 30.0f / 255.0f;
+ wheel_centering = !!(data[7] & 0x01);
+ wheel_centering_scalar = ((data[7] >> 1) & 0x7f) / 127.0f;
+ }
+ }
+
+ static constexpr uint32_t kTimeout = 100000;
+ if (!time_after(time_add(last_command_time, kTimeout), micros())) {
+ last_command_time = time_subtract(micros(), kTimeout);
+ trigger_goal_position = 0.0f;
+ trigger_goal_velocity = 0.0f;
+ trigger_haptic_current = 0.0f;
+ trigger_centering = true;
+ trigger_haptics = false;
+
+ wheel_goal_position = 0.0f;
+ wheel_goal_velocity = 0.0f;
+ wheel_haptic_current = 0.0f;
+ wheel_centering = true;
+ wheel_centering_scalar = 0.25f;
+ }
+
+ StateFeedbackPlant<3, 1, 1, float> *const trigger_plant =
+ global_trigger_plant.load(::std::memory_order_relaxed);
+ StateFeedbackObserver<3, 1, 1, float> *const trigger_observer =
+ global_trigger_observer.load(::std::memory_order_relaxed);
+ ::Eigen::Matrix<float, 1, 1> trigger_Y;
+ trigger_Y << absolute_trigger_angle_radians;
+ trigger_observer->Correct(*trigger_plant,
+ ::Eigen::Matrix<float, 1, 1>::Zero(), trigger_Y);
+
+ StateFeedbackPlant<3, 1, 1, float> *const wheel_plant =
+ global_wheel_plant.load(::std::memory_order_relaxed);
+ StateFeedbackObserver<3, 1, 1, float> *const wheel_observer =
+ global_wheel_observer.load(::std::memory_order_relaxed);
+ ::Eigen::Matrix<float, 1, 1> wheel_Y;
+ wheel_Y << absolute_wheel_angle_radians;
+ wheel_observer->Correct(*wheel_plant, ::Eigen::Matrix<float, 1, 1>::Zero(),
+ wheel_Y);
+
+ float kWheelD = (wheel_kp - 10.0f) * (0.25f - 0.20f) / 5.0f + 0.20f;
+ if (wheel_kp < 0.5f) {
+ kWheelD = wheel_kp * 0.05f / 0.5f;
+ } else if (wheel_kp < 1.0f) {
+ kWheelD = InterpolateFloat(1.0f, 0.5f, 0.06f, 0.05f, wheel_kp);
+ } else if (wheel_kp < 2.0f) {
+ kWheelD = InterpolateFloat(2.0f, 1.0f, 0.08f, 0.06f, wheel_kp);
+ } else if (wheel_kp < 3.0f) {
+ kWheelD = InterpolateFloat(3.0f, 2.0f, 0.10f, 0.08f, wheel_kp);
+ } else if (wheel_kp < 5.0f) {
+ kWheelD = InterpolateFloat(5.0f, 3.0f, 0.13f, 0.10f, wheel_kp);
+ } else if (wheel_kp < 10.0f) {
+ kWheelD = InterpolateFloat(10.0f, 5.0f, 0.20f, 0.13f, wheel_kp);
+ }
+
+ float wheel_goal_current = wheel_haptic_current;
+
+ wheel_goal_current +=
+ (wheel_goal_position - absolute_wheel_angle_radians) * wheel_kp +
+ (wheel_goal_velocity - wheel_observer->X_hat()(1, 0)) * kWheelD;
+
+ // Compute the torques to apply to each motor.
+ if (wheel_centering) {
+ wheel_goal_current +=
+ WheelCenteringCurrent(wheel_centering_scalar, absolute_wheel_angle,
+ wheel_observer->X_hat()(1, 0));
+ }
+
+ if (wheel_goal_current > kHapticWheelCurrentLimit) {
+ wheel_goal_current = kHapticWheelCurrentLimit;
+ } else if (wheel_goal_current < -kHapticWheelCurrentLimit) {
+ wheel_goal_current = -kHapticWheelCurrentLimit;
+ }
+ global_wheel_current.store(wheel_goal_current, ::std::memory_order_relaxed);
+
+ constexpr float kTriggerP =
+ static_cast<float>(::frc971::control_loops::drivetrain::kHapticTriggerP);
+ constexpr float kTriggerD =
+ static_cast<float>(::frc971::control_loops::drivetrain::kHapticTriggerD);
+ float trigger_goal_current = trigger_haptic_current;
+ if (trigger_haptics) {
+ trigger_goal_current +=
+ (trigger_goal_position - absolute_trigger_angle_radians) * kTriggerP +
+ (trigger_goal_velocity - trigger_observer->X_hat()(1, 0)) * kTriggerD;
+ }
+
+ if (trigger_centering) {
+ trigger_goal_current += TriggerCenteringCurrent(absolute_trigger_angle);
+ }
+
+ if (trigger_goal_current > kHapticTriggerCurrentLimit) {
+ trigger_goal_current = kHapticTriggerCurrentLimit;
+ } else if (trigger_goal_current < -kHapticTriggerCurrentLimit) {
+ trigger_goal_current = -kHapticTriggerCurrentLimit;
+ }
+ global_trigger_torque.store(trigger_goal_current,
+ ::std::memory_order_relaxed);
+
+ uint8_t buttons = 0;
+ if (!GPIO_BITBAND(GPIOA_PDIR, 14)) {
+ buttons |= 0x1;
+ }
+ if (!GPIO_BITBAND(GPIOE_PDIR, 26)) {
+ buttons |= 0x2;
+ }
+ if (!GPIO_BITBAND(GPIOC_PDIR, 7)) {
+ buttons |= 0x4;
+ }
+ if (!GPIO_BITBAND(GPIOD_PDIR, 0)) {
+ buttons |= 0x8;
+ }
+
+ float trigger_angle = absolute_trigger_angle;
+
+ // Adjust the trigger range for reporting back.
+ // TODO(austin): We'll likely need to make this symmetric for the controls to
+ // work out well.
+ if (trigger_angle > kTriggerCenter) {
+ trigger_angle = (trigger_angle - kTriggerCenter) / (1.0f - kTriggerCenter);
+ } else {
+ trigger_angle = (trigger_angle - kTriggerCenter) /
+ (kTriggerCenter - kTriggerMaxExtension);
+ }
+
+ // TODO(austin): Class + fns. This is a mess.
+ // TODO(austin): Move this to a separate file. It's too big.
+ int can_trigger = ConvertFloat16(absolute_trigger_angle);
+ int can_trigger_velocity =
+ ConvertFloat16(trigger_observer->X_hat()(1, 0) / 50.0f);
+ int can_trigger_torque =
+ ConvertFloat16(trigger_observer->X_hat()(2, 0) * 2.0f);
+ int can_trigger_current = ConvertFloat14(trigger_goal_current / 10.0f);
+
+ int can_wheel = ConvertFloat16(absolute_wheel_angle);
+ int can_wheel_velocity =
+ ConvertFloat16(wheel_observer->X_hat()(1, 0) / 50.0f);
+ int can_wheel_torque = ConvertFloat16(wheel_observer->X_hat()(2, 0) * 2.0f);
+ int can_wheel_current = ConvertFloat14(wheel_goal_current / 10.0f);
+
+ {
+ const uint8_t trigger_joystick_values[8] = {
+ static_cast<uint8_t>(can_trigger & 0xff),
+ static_cast<uint8_t>((can_trigger >> 8) & 0xff),
+ static_cast<uint8_t>(can_trigger_velocity & 0xff),
+ static_cast<uint8_t>((can_trigger_velocity >> 8) & 0xff),
+ static_cast<uint8_t>(can_trigger_torque & 0xff),
+ static_cast<uint8_t>((can_trigger_torque >> 8) & 0xff),
+ static_cast<uint8_t>(can_trigger_current & 0xff),
+ static_cast<uint8_t>(((buttons & 0x3) << 6) |
+ (can_trigger_current >> 8))};
+ const uint8_t wheel_joystick_values[8] = {
+ static_cast<uint8_t>(can_wheel & 0xff),
+ static_cast<uint8_t>((can_wheel >> 8) & 0xff),
+ static_cast<uint8_t>(can_wheel_velocity & 0xff),
+ static_cast<uint8_t>((can_wheel_velocity >> 8) & 0xff),
+ static_cast<uint8_t>(can_wheel_torque & 0xff),
+ static_cast<uint8_t>((can_wheel_torque >> 8) & 0xff),
+ static_cast<uint8_t>(can_wheel_current & 0xff),
+ static_cast<uint8_t>(((buttons & 0xc) << 4) |
+ (can_wheel_current >> 8))};
+
+ can_send(0, trigger_joystick_values, 8, 2);
+ can_send(1, wheel_joystick_values, 8, 3);
+ }
+
+ ::Eigen::Matrix<float, 1, 1> trigger_U;
+ trigger_U << trigger_goal_current;
+ ::Eigen::Matrix<float, 1, 1> wheel_U;
+ wheel_U << wheel_goal_current;
+ trigger_observer->Predict(trigger_plant, trigger_U,
+ ::std::chrono::milliseconds(1));
+ wheel_observer->Predict(wheel_plant, wheel_U, ::std::chrono::milliseconds(1));
+}
+
+void ConfigurePwmFtm(BigFTM *pwm_ftm) {
+ // Put them all into combine active-high mode, and all the low ones staying
+ // off all the time by default. We'll then use only the low ones.
+ pwm_ftm->C0SC = FTM_CSC_ELSB;
+ pwm_ftm->C0V = 0;
+ pwm_ftm->C1SC = FTM_CSC_ELSB;
+ pwm_ftm->C1V = 0;
+ pwm_ftm->C2SC = FTM_CSC_ELSB;
+ pwm_ftm->C2V = 0;
+ pwm_ftm->C3SC = FTM_CSC_ELSB;
+ pwm_ftm->C3V = 0;
+ pwm_ftm->C4SC = FTM_CSC_ELSB;
+ pwm_ftm->C4V = 0;
+ pwm_ftm->C5SC = FTM_CSC_ELSB;
+ pwm_ftm->C5V = 0;
+ pwm_ftm->C6SC = FTM_CSC_ELSB;
+ pwm_ftm->C6V = 0;
+ pwm_ftm->C7SC = FTM_CSC_ELSB;
+ pwm_ftm->C7V = 0;
+
+ pwm_ftm->COMBINE = FTM_COMBINE_SYNCEN3 /* Synchronize updates usefully */ |
+ FTM_COMBINE_COMP3 /* Make them complementary */ |
+ FTM_COMBINE_COMBINE3 /* Combine the channels */ |
+ FTM_COMBINE_SYNCEN2 /* Synchronize updates usefully */ |
+ FTM_COMBINE_COMP2 /* Make them complementary */ |
+ FTM_COMBINE_COMBINE2 /* Combine the channels */ |
+ FTM_COMBINE_SYNCEN1 /* Synchronize updates usefully */ |
+ FTM_COMBINE_COMP1 /* Make them complementary */ |
+ FTM_COMBINE_COMBINE1 /* Combine the channels */ |
+ FTM_COMBINE_SYNCEN0 /* Synchronize updates usefully */ |
+ FTM_COMBINE_COMP0 /* Make them complementary */ |
+ FTM_COMBINE_COMBINE0 /* Combine the channels */;
+}
+
+bool CountValid(uint32_t count) {
+ static constexpr int kMaxMovement = 1;
+ return count <= kMaxMovement || count >= (4096 - kMaxMovement);
+}
+
+bool ZeroMotors(uint16_t *motor0_offset, uint16_t *motor1_offset,
+ uint16_t *wheel_offset) {
+ static constexpr int kNumberSamples = 1024;
+ static_assert(UINT16_MAX * kNumberSamples <= UINT32_MAX, "Too many samples");
+ uint32_t motor0_sum = 0, motor1_sum = 0, wheel_sum = 0;
+
+ // First clear both encoders.
+ MOTOR0_ENCODER_FTM->CNT = MOTOR1_ENCODER_FTM->CNT = 0;
+ for (int i = 0; i < kNumberSamples; ++i) {
+ delay(1);
+
+ if (!CountValid(MOTOR0_ENCODER_FTM->CNT)) {
+ printf("Motor 0 moved too much\n");
+ return false;
+ }
+ if (!CountValid(MOTOR1_ENCODER_FTM->CNT)) {
+ printf("Motor 1 moved too much\n");
+ return false;
+ }
+
+ DisableInterrupts disable_interrupts;
+ const SmallInitReadings readings = AdcReadSmallInit(disable_interrupts);
+ motor0_sum += readings.motor0_abs;
+ motor1_sum += readings.motor1_abs;
+ wheel_sum += readings.wheel_abs;
+ }
+
+ *motor0_offset = (motor0_sum + kNumberSamples / 2) / kNumberSamples;
+ *motor1_offset = (motor1_sum + kNumberSamples / 2) / kNumberSamples;
+ *wheel_offset = (wheel_sum + kNumberSamples / 2) / kNumberSamples;
+
+ return true;
+}
+
+} // namespace
+
+extern "C" int main() {
+ // for background about this startup delay, please see these conversations
+ // https://forum.pjrc.com/threads/36606-startup-time-(400ms)?p=113980&viewfull=1#post113980
+ // https://forum.pjrc.com/threads/31290-Teensey-3-2-Teensey-Loader-1-24-Issues?p=87273&viewfull=1#post87273
+ delay(400);
+
+ // Set all interrupts to the second-lowest priority to start with.
+ for (int i = 0; i < NVIC_NUM_INTERRUPTS; i++) NVIC_SET_SANE_PRIORITY(i, 0xD);
+
+ // Now set priorities for all the ones we care about. They only have meaning
+ // relative to each other, which means centralizing them here makes it a lot
+ // more manageable.
+ NVIC_SET_SANE_PRIORITY(IRQ_USBOTG, 0x7);
+ NVIC_SET_SANE_PRIORITY(IRQ_FTM0, 0x3);
+ NVIC_SET_SANE_PRIORITY(IRQ_FTM3, 0x3);
+ NVIC_SET_SANE_PRIORITY(IRQ_PIT_CH3, 0x5);
+
+ // Set the LED's pin to output mode.
+ GPIO_BITBAND(GPIOC_PDDR, 5) = 1;
+ PORTC_PCR5 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+
+ // Set up the CAN pins.
+ PORTA_PCR12 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+ PORTA_PCR13 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+
+ // BTN0
+ PORTC_PCR7 = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(1);
+ // BTN1
+ PORTE_PCR26 = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(1);
+ // BTN2
+ PORTA_PCR14 = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(1);
+ // BTN3
+ PORTD_PCR0 = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(1);
+
+ PORTA_PCR5 = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(1);
+
+ DMA_CR = DMA_CR_EMLM;
+
+ teensy::UsbDevice usb_device(0, 0x16c0, 0x0490);
+ usb_device.SetManufacturer("FRC 971 Spartan Robotics");
+ usb_device.SetProduct("Pistol Grip Controller debug");
+ teensy::AcmTty tty1(&usb_device);
+ teensy::AcmTty tty2(&usb_device);
+ global_stdout.store(&tty1, ::std::memory_order_release);
+ usb_device.Initialize();
+
+ AdcInitSmall();
+ MathInit();
+ delay(100);
+ can_init(2, 3);
+
+ GPIOD_PCOR = 1 << 3;
+ GPIO_BITBAND(GPIOD_PDDR, 3) = 1;
+ PORTD_PCR3 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ GPIOD_PSOR = 1 << 3;
+
+ GPIOC_PCOR = 1 << 4;
+ GPIO_BITBAND(GPIOC_PDDR, 4) = 1;
+ PORTC_PCR4 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ GPIOC_PSOR = 1 << 4;
+
+ LittleMotorControlsImplementation controls0, controls1;
+
+ delay(100);
+
+ // M0_EA = FTM1_QD_PHB
+ PORTB_PCR19 = PORT_PCR_MUX(6);
+ // M0_EB = FTM1_QD_PHA
+ PORTB_PCR18 = PORT_PCR_MUX(6);
+
+ // M1_EA = FTM1_QD_PHA
+ PORTB_PCR0 = PORT_PCR_MUX(6);
+ // M1_EB = FTM1_QD_PHB
+ PORTB_PCR1 = PORT_PCR_MUX(6);
+
+ // M0_CH0 = FTM3_CH4
+ PORTC_PCR8 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+ // M0_CH1 = FTM3_CH2
+ PORTD_PCR2 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+ // M0_CH2 = FTM3_CH6
+ PORTC_PCR10 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+
+ // M1_CH0 = FTM0_CH0
+ PORTC_PCR1 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+ // M1_CH1 = FTM0_CH2
+ PORTC_PCR3 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+ // M1_CH2 = FTM0_CH4
+ PORTD_PCR4 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+
+ Motor motor0(
+ MOTOR0_PWM_FTM, MOTOR0_ENCODER_FTM, &controls0,
+ {&MOTOR0_PWM_FTM->C4V, &MOTOR0_PWM_FTM->C2V, &MOTOR0_PWM_FTM->C6V});
+ motor0.set_debug_tty(&tty2);
+ motor0.set_switching_divisor(kSwitchingDivisor);
+ Motor motor1(
+ MOTOR1_PWM_FTM, MOTOR1_ENCODER_FTM, &controls1,
+ {&MOTOR1_PWM_FTM->C0V, &MOTOR1_PWM_FTM->C2V, &MOTOR1_PWM_FTM->C4V});
+ motor1.set_debug_tty(&tty2);
+ motor1.set_switching_divisor(kSwitchingDivisor);
+ ConfigurePwmFtm(MOTOR0_PWM_FTM);
+ ConfigurePwmFtm(MOTOR1_PWM_FTM);
+ motor0.Init();
+ motor1.Init();
+ global_motor0.store(&motor0, ::std::memory_order_relaxed);
+ global_motor1.store(&motor1, ::std::memory_order_relaxed);
+
+ SIM_SCGC6 |= SIM_SCGC6_PIT;
+ PIT_MCR = 0;
+ PIT_LDVAL3 = BUS_CLOCK_FREQUENCY / 1000;
+ PIT_TCTRL3 = PIT_TCTRL_TIE | PIT_TCTRL_TEN;
+
+ // Have them both wait for the GTB signal.
+ FTM0->CONF = FTM3->CONF =
+ FTM_CONF_GTBEEN | FTM_CONF_NUMTOF(kSwitchingDivisor - 1);
+ // Make FTM3's period half of what it should be so we can get it a half-cycle
+ // out of phase.
+ const uint32_t original_mod = FTM3->MOD;
+ FTM3->MOD = ((original_mod + 1) / 2) - 1;
+ FTM3->SYNC |= FTM_SYNC_SWSYNC;
+
+ // Output triggers to things like the PDBs on initialization.
+ FTM0_EXTTRIG = FTM_EXTTRIG_INITTRIGEN;
+ FTM3_EXTTRIG = FTM_EXTTRIG_INITTRIGEN;
+ // Don't let any memory accesses sneak past here, because we actually
+ // need everything to be starting up.
+ __asm__("" ::: "memory");
+
+ // Give everything a chance to get going.
+ delay(100);
+
+ printf("BSS: %p-%p\n", __bss_ram_start__, __bss_ram_end__);
+ printf("data: %p-%p\n", __data_ram_start__, __data_ram_end__);
+ printf("heap start: %p\n", __heap_start__);
+ printf("stack start: %p\n", __stack_end__);
+
+ printf("Zeroing motors\n");
+ uint16_t motor0_offset, motor1_offset, wheel_offset;
+ while (!ZeroMotors(&motor0_offset, &motor1_offset, &wheel_offset)) {
+ }
+ printf("Done zeroing\n");
+
+ const float motor0_offset_scaled = -analog_ratio(motor0_offset);
+ const float motor1_offset_scaled = analog_ratio(motor1_offset);
+ // Good for the initial trigger.
+ {
+ constexpr float kZeroOffset0 = 0.27f;
+ const int motor0_starting_point = static_cast<int>(
+ (motor0_offset_scaled + (kZeroOffset0 / 7.0f)) * 4096.0f);
+ printf("Motor 0 starting at %d\n", motor0_starting_point);
+ motor0.set_encoder_calibration_offset(motor0_starting_point);
+ motor0.set_encoder_multiplier(-1);
+
+ // Calibrate neutral here.
+ motor0.set_encoder_offset(motor0.encoder_offset() - 2065 + 20);
+
+ uint32_t new_encoder = motor0.wrapped_encoder();
+ int32_t absolute_encoder = motor0.absolute_encoder(new_encoder);
+ printf("Motor 0 encoder %d absolute %d\n", static_cast<int>(new_encoder),
+ static_cast<int>(absolute_encoder));
+ }
+
+ {
+ constexpr float kZeroOffset1 = 0.26f;
+ const int motor1_starting_point = static_cast<int>(
+ (motor1_offset_scaled + (kZeroOffset1 / 7.0f)) * 4096.0f);
+ printf("Motor 1 starting at %d\n", motor1_starting_point);
+ motor1.set_encoder_calibration_offset(motor1_starting_point);
+ motor1.set_encoder_multiplier(-1);
+
+ float wheel_position = absolute_wheel(analog_ratio(wheel_offset));
+
+ uint32_t encoder = motor1.wrapped_encoder();
+
+ printf("Wheel starting at %d, encoder %" PRId32 "\n",
+ static_cast<int>(wheel_position * 1000.0f), encoder);
+
+ constexpr float kWheelGearRatio = (1.25f + 0.02f) / 0.35f;
+ constexpr float kWrappedWheelAtZero = 0.6586310546875f;
+
+ const int encoder_wraps =
+ static_cast<int>(lround(wheel_position * kWheelGearRatio -
+ (encoder / 4096.f) + kWrappedWheelAtZero));
+
+ printf("Wraps: %d\n", encoder_wraps);
+ motor1.set_encoder_offset(4096 * encoder_wraps + motor1.encoder_offset() -
+ static_cast<int>(kWrappedWheelAtZero * 4096));
+ printf("Wheel encoder now at %d\n",
+ static_cast<int>(1000.f / 4096.f *
+ motor1.absolute_encoder(motor1.wrapped_encoder())));
+ }
+
+ // Turn an LED on for Austin.
+ GPIO_BITBAND(GPIOC_PDDR, 6) = 1;
+ GPIOC_PCOR = 1 << 6;
+ PORTC_PCR6 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+
+ // M0_THW
+ PORTC_PCR11 = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(1);
+ // M0_FAULT
+ PORTD_PCR6 = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(1);
+ // M1_THW
+ PORTC_PCR2 = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(1);
+ // M1_FAULT
+ PORTD_PCR5 = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(1);
+
+ motor0.Start();
+ motor1.Start();
+ {
+ // We rely on various things happening faster than the timer period, so make
+ // sure slow USB or whatever interrupts don't prevent that.
+ DisableInterrupts disable_interrupts;
+
+ // First clear the overflow flag.
+ FTM3->SC &= ~FTM_SC_TOF;
+
+ // Now poke the GTB to actually start both timers.
+ FTM0->CONF = FTM_CONF_GTBEEN | FTM_CONF_GTBEOUT |
+ FTM_CONF_NUMTOF(kSwitchingDivisor - 1);
+
+ // Wait for it to overflow twice. For some reason, just once doesn't work.
+ while (!(FTM3->SC & FTM_SC_TOF)) {
+ }
+ FTM3->SC &= ~FTM_SC_TOF;
+ while (!(FTM3->SC & FTM_SC_TOF)) {
+ }
+
+ // Now put the MOD value back to what it was.
+ FTM3->MOD = original_mod;
+ FTM3->PWMLOAD = FTM_PWMLOAD_LDOK;
+
+ // And then clear the overflow flags before enabling interrupts so we
+ // actually wait until the next overflow to start doing interrupts.
+ FTM0->SC &= ~FTM_SC_TOF;
+ FTM3->SC &= ~FTM_SC_TOF;
+ NVIC_ENABLE_IRQ(IRQ_FTM0);
+ NVIC_ENABLE_IRQ(IRQ_FTM3);
+ }
+ global_trigger_plant.store(
+ new StateFeedbackPlant<3, 1, 1, float>(MakeIntegralHapticTriggerPlant()));
+ global_trigger_observer.store(new StateFeedbackObserver<3, 1, 1, float>(
+ MakeIntegralHapticTriggerObserver()));
+ global_trigger_observer.load(::std::memory_order_relaxed)
+ ->Reset(global_trigger_plant.load(::std::memory_order_relaxed));
+
+ global_wheel_plant.store(
+ new StateFeedbackPlant<3, 1, 1, float>(MakeIntegralHapticWheelPlant()));
+ global_wheel_observer.store(new StateFeedbackObserver<3, 1, 1, float>(
+ MakeIntegralHapticWheelObserver()));
+ global_wheel_observer.load(::std::memory_order_relaxed)
+ ->Reset(global_wheel_plant.load(::std::memory_order_relaxed));
+
+ delay(1000);
+
+ NVIC_ENABLE_IRQ(IRQ_PIT_CH3);
+
+ // TODO(Brian): Use SLEEPONEXIT to reduce interrupt latency?
+ while (true) {
+ if (!GPIO_BITBAND(GPIOC_PDIR, 11)) {
+ if (!GPIO_BITBAND(GPIOC_PDOR, 5)) {
+ printf("M0_THW\n");
+ }
+ GPIOC_PSOR = 1 << 5;
+ }
+ if (!GPIO_BITBAND(GPIOD_PDIR, 6)) {
+ if (!GPIO_BITBAND(GPIOC_PDOR, 5)) {
+ printf("M0_FAULT\n");
+ }
+ GPIOC_PSOR = 1 << 5;
+ }
+ if (!GPIO_BITBAND(GPIOC_PDIR, 2)) {
+ if (!GPIO_BITBAND(GPIOC_PDOR, 5)) {
+ printf("M1_THW\n");
+ }
+ GPIOC_PSOR = 1 << 5;
+ }
+ if (!GPIO_BITBAND(GPIOD_PDIR, 5)) {
+ if (!GPIO_BITBAND(GPIOC_PDOR, 5)) {
+ printf("M1_FAULT\n");
+ }
+ GPIOC_PSOR = 1 << 5;
+ }
+ }
+
+ return 0;
+}
+
+} // namespace salsa
+} // namespace frc971
diff --git a/motors/pistol_grip/generate_cogging.py b/motors/pistol_grip/generate_cogging.py
new file mode 100644
index 0000000..9cecfe2
--- /dev/null
+++ b/motors/pistol_grip/generate_cogging.py
@@ -0,0 +1,75 @@
+#!/usr/bin/python
+
+import sys
+from matplotlib import pylab
+
+# TODO(austin): Plot flag.
+
+def main(argv):
+ if len(argv) < 4:
+ print 'Args: input output.cc struct_name'
+ return 1
+ data_sum = [0.0] * 4096
+ data_count = [0] * 4096
+ data_list_absolute = []
+ data_list_current = []
+
+ with open(argv[1], 'r') as fd:
+ for line in fd:
+ if line.startswith('reading'):
+ split_line = line.split()
+ data_absolute = int(split_line[1])
+ data_index = int(split_line[3][2:])
+ data_current = int(split_line[2]) / 10000.0
+ data_sum[data_index] += data_current
+ data_count[data_index] += 1
+ data_list_absolute.append(data_absolute)
+ data_list_current.append(data_current)
+ data = [0.0] * 4096
+ min_zero = 4096
+ max_zero = 0
+ for i in xrange(0, 4096):
+ if data_count[i] == 0:
+ min_zero = min(i, min_zero)
+ max_zero = max(i, min_zero)
+
+ for i in xrange(0, 4096):
+ if data_count[i] != 0:
+ data[i] = data_sum[i] / data_count[i]
+ if min_zero == 0 and max_zero == 4095:
+ for i in xrange(0, 4096):
+ if data_count[i] != 0:
+ while i > 0:
+ data[i - 1] = data[i]
+ i -= 1
+ break;
+
+ for i in reversed(xrange(0, 4096)):
+ if data_count[i] != 0:
+ while i < 4095:
+ data[i + 1] = data[i]
+ i += 1
+ break;
+ else:
+ for i in xrange(0, 4096):
+ if data_count[i] == 0:
+ if i < (min_zero + max_zero) / 2:
+ data[i] = data[min_zero - 1]
+ else:
+ data[i] = data[max_zero + 1]
+
+ pylab.plot(range(0, 4096), data)
+ pylab.figure()
+ pylab.plot(data_list_absolute, data_list_current)
+ pylab.show()
+ with open(argv[2], 'w') as out_fd:
+ out_fd.write('extern const float %s[4096];\n' % argv[3])
+ out_fd.write('const float %s[4096] = {\n' % argv[3])
+ for datapoint in data:
+ out_fd.write(' %ff,\n' % datapoint)
+ out_fd.write('};')
+
+ return 0
+
+if __name__ == '__main__':
+ sys.exit(main(sys.argv))
diff --git a/motors/pistol_grip/motor_controls.cc b/motors/pistol_grip/motor_controls.cc
new file mode 100644
index 0000000..b123378
--- /dev/null
+++ b/motors/pistol_grip/motor_controls.cc
@@ -0,0 +1,212 @@
+#include "motors/pistol_grip/motor_controls.h"
+
+#include "motors/peripheral/configuration.h"
+
+namespace frc971 {
+namespace salsa {
+namespace {
+
+template <int kRows, int kCols>
+using ComplexMatrix =
+ LittleMotorControlsImplementation::ComplexMatrix<kRows, kCols>;
+
+using Complex = ::std::complex<float>;
+
+constexpr int kCountsPerRevolution =
+ LittleMotorControlsImplementation::constant_counts_per_revolution();
+constexpr int kPolesPerRevolution =
+ LittleMotorControlsImplementation::poles_per_revolution();
+
+#if 1
+constexpr double kMaxDutyCycle = 0.98;
+#elif 1
+constexpr double kMaxDutyCycle = 0.6;
+#elif 0
+constexpr double kMaxDutyCycle = 0.2;
+#endif
+
+constexpr int kPhaseBOffset = kCountsPerRevolution / 3;
+constexpr int kPhaseCOffset =
+ 2 * kCountsPerRevolution / 3;
+
+// volts
+constexpr double vcc = 14.0;
+
+constexpr double Kv = 37.6;
+
+// Henries
+constexpr double L = 8e-05;
+
+constexpr double M = 8e-06;
+
+// ohms for system
+constexpr double R = 0.207393;
+
+::Eigen::Matrix<float, 3, 3> A_discrete() {
+ ::Eigen::Matrix<float, 3, 3> r;
+ static constexpr float kDiagonal = 0.87645f;
+ static constexpr float kOffDiagonal = 0.0105814f;
+ r << kDiagonal, kOffDiagonal, kOffDiagonal, kOffDiagonal, kDiagonal,
+ kOffDiagonal, kOffDiagonal, kOffDiagonal, kDiagonal;
+ return r;
+}
+
+::Eigen::Matrix<float, 3, 3> B_discrete_inverse() {
+ return ::Eigen::Matrix<float, 1, 3>::Constant(1.54618f).asDiagonal();
+}
+
+// The number to divide the product of the unit BEMF and the per phase current
+// by to get motor current.
+constexpr double kOneAmpScalar = 1.5;
+
+constexpr double kMaxOneAmpDrivingVoltage = 0.208257;
+
+// Use FluxLinkageTable() to access this with a const so you don't accidentally
+// modify it.
+float flux_linkage_table[kCountsPerRevolution];
+
+void MakeFluxLinkageTable() {
+ for (int i = 0; i < kCountsPerRevolution; ++i) {
+ const double theta = static_cast<double>(i) /
+ static_cast<double>(kCountsPerRevolution) *
+ static_cast<double>(kPolesPerRevolution) * 2.0 * M_PI;
+ flux_linkage_table[i] = sin(theta);
+ }
+}
+
+// theta doesn't have to be less than kCountsPerRevolution.
+::Eigen::Matrix<float, 3, 1> FluxLinkageAt(uint32_t theta) {
+ ::Eigen::Matrix<float, 3, 1> r;
+ r(0) = flux_linkage_table[theta % kCountsPerRevolution];
+ r(1) = flux_linkage_table[(theta + kPhaseBOffset) % kCountsPerRevolution];
+ r(2) = flux_linkage_table[(theta + kPhaseCOffset) % kCountsPerRevolution];
+ return r;
+}
+
+::Eigen::Matrix<float, 3, 3> MakeK() {
+ ::Eigen::Matrix<float, 3, 3> Vconv;
+ Vconv << 2.0f, -1.0f, -1.0f, -1.0f, 2.0f, -1.0f, -1.0f, -1.0f, 2.0f;
+ static constexpr float kControllerGain = 0.07f;
+ return kControllerGain * (Vconv / 3.0f);
+}
+
+ComplexMatrix<3, 1> MakeE1Unrotated() {
+ ComplexMatrix<3, 1> rotation;
+ rotation << Complex(0, -1), Complex(::std::sqrt(3) / 2, 0.5),
+ Complex(-::std::sqrt(3) / 2, 0.5);
+ return rotation;
+}
+
+ComplexMatrix<3, 3> Hn(float omega, int scalar) {
+ const Complex a(static_cast<float>(R),
+ omega * static_cast<float>(scalar * L));
+ const Complex b(0, omega * static_cast<float>(scalar * M));
+ const Complex temp1 = a + b;
+ const Complex temp2 = -b;
+ ComplexMatrix<3, 3> matrix;
+ matrix << temp1, temp2, temp2, temp2, temp1, temp2, temp2, temp2, temp1;
+ return matrix *
+ -(omega / static_cast<float>(Kv) / (a * a + a * b - 2.0f * b * b));
+}
+
+} // namespace
+
+LittleMotorControlsImplementation::LittleMotorControlsImplementation()
+ : E1Unrotated_(MakeE1Unrotated()) {
+ MakeFluxLinkageTable();
+}
+
+::std::array<uint32_t, 3> LittleMotorControlsImplementation::DoIteration(
+ const float raw_currents[3], const uint32_t theta_in,
+ const float command_current) {
+ static constexpr float kCurrentSlewRate = 0.05f;
+ if (command_current > filtered_current_ + kCurrentSlewRate) {
+ filtered_current_ += kCurrentSlewRate;
+ } else if (command_current < filtered_current_ - kCurrentSlewRate) {
+ filtered_current_ -= kCurrentSlewRate;
+ } else {
+ filtered_current_ = command_current;
+ }
+ const float goal_current_in = filtered_current_;
+ static constexpr float kMaxEffectiveVcc = static_cast<float>(vcc * kMaxDutyCycle);
+ const float estimated_velocity_voltage =
+ estimated_velocity_ / static_cast<float>(Kv / 2.0);
+ const float max_current = (kMaxEffectiveVcc - estimated_velocity_voltage) /
+ static_cast<float>(kMaxOneAmpDrivingVoltage);
+ const float min_current = (-kMaxEffectiveVcc - estimated_velocity_voltage) /
+ static_cast<float>(kMaxOneAmpDrivingVoltage);
+ const float goal_current =
+ ::std::max(min_current, ::std::min(max_current, goal_current_in));
+
+ const uint32_t theta = theta_in;
+
+ const ::Eigen::Matrix<float, 3, 1> measured_current =
+ (::Eigen::Matrix<float, 3, 1>() << scale_current_reading(raw_currents[0]),
+ scale_current_reading(raw_currents[1]),
+ scale_current_reading(raw_currents[2])).finished();
+
+ const ComplexMatrix<3, 1> E1 =
+ E1Unrotated_ *
+ ImaginaryExpInt<::std::ratio<kPolesPerRevolution, kCountsPerRevolution>>(
+ theta);
+
+ const float overall_measured_current =
+ (E1.real().transpose() * measured_current /
+ static_cast<float>(kOneAmpScalar))(0);
+ const float current_error = goal_current - overall_measured_current;
+ estimated_velocity_ += current_error * 0.1f;
+
+ const float omega = estimated_velocity_;
+
+ const ::Eigen::Matrix<float, 3, 1> I_now = I_last_;
+ const ::Eigen::Matrix<float, 3, 1> I_next =
+ FluxLinkageAt(theta) * goal_current;
+
+ const ComplexMatrix<3, 3> H1 = Hn(omega, 1);
+
+ const ComplexMatrix<3, 1> p_imaginary = H1 * E1;
+ const ComplexMatrix<3, 1> p_next_imaginary =
+ ImaginaryExpFloat(omega / SWITCHING_FREQUENCY) * p_imaginary;
+ const ::Eigen::Matrix<float, 3, 1> p = p_imaginary.real();
+ const ::Eigen::Matrix<float, 3, 1> p_next = p_next_imaginary.real();
+
+ const ::Eigen::Matrix<float, 3, 1> Vn_ff =
+ B_discrete_inverse() * (I_next - A_discrete() * (I_now - p) - p_next);
+ const ::Eigen::Matrix<float, 3, 1> Vn =
+ Vn_ff + MakeK() * (I_now - measured_current);
+
+ debug_[0] = (I_next)(0) * 100;
+ debug_[1] = (I_next)(1) * 100;
+ debug_[2] = (I_next)(2) * 100;
+
+ debug_[5] = Vn(0) * 100;
+ debug_[6] = Vn(1) * 100;
+ debug_[7] = Vn(2) * 100;
+
+ ::Eigen::Matrix<float, 3, 1> times = Vn / vcc;
+ {
+ const float min_time = times.minCoeff();
+ times -= ::Eigen::Matrix<float, 3, 1>::Constant(min_time);
+ }
+ {
+ const float max_time = times.maxCoeff();
+ const float scalar =
+ static_cast<float>(kMaxDutyCycle) /
+ ::std::max(static_cast<float>(kMaxDutyCycle), max_time);
+ times *= scalar;
+ }
+
+ I_last_ = I_next;
+
+ // TODO(Austin): Figure out why we need the min here.
+ return {static_cast<uint32_t>(::std::max(0.0f, times(0)) * 1500.0f),
+ static_cast<uint32_t>(::std::max(0.0f, times(1)) * 1500.0f),
+ static_cast<uint32_t>(::std::max(0.0f, times(2)) * 1500.0f)};
+}
+
+int16_t LittleMotorControlsImplementation::Debug(uint32_t theta) {
+ return debug_[theta];
+}
+
+} // namespace salsa
+} // namespace frc971
diff --git a/motors/pistol_grip/motor_controls.h b/motors/pistol_grip/motor_controls.h
new file mode 100644
index 0000000..ca965d0
--- /dev/null
+++ b/motors/pistol_grip/motor_controls.h
@@ -0,0 +1,63 @@
+#ifndef MOTORS_PISTOL_GRIP_MOTOR_CONTROLS_H_
+#define MOTORS_PISTOL_GRIP_MOTOR_CONTROLS_H_
+
+#include <array>
+#include <complex>
+
+#include "motors/math.h"
+#include "motors/motor.h"
+
+#include "Eigen/Dense"
+
+namespace frc971 {
+namespace salsa {
+
+class LittleMotorControlsImplementation : public MotorControls {
+ public:
+ template <int kRows, int kCols>
+ using ComplexMatrix = ::Eigen::Matrix<::std::complex<float>, kRows, kCols>;
+
+ LittleMotorControlsImplementation();
+ ~LittleMotorControlsImplementation() override = default;
+
+ static constexpr int constant_counts_per_revolution() { return 4096; }
+ static constexpr int poles_per_revolution() { return 7; }
+
+ int mechanical_counts_per_revolution() const override {
+ return constant_counts_per_revolution();
+ }
+ int electrical_counts_per_revolution() const override {
+ return constant_counts_per_revolution() / poles_per_revolution();
+ }
+
+ float scale_current_reading(float reading) const override {
+ return reading *
+ static_cast<float>(1.0 / 4096.0 /* Full-scale ADC reading */ *
+ 3.3 /* ADC reference voltage */ /
+ (18.0 / (10.8 + 18.0)) /* 5V -> 3.3V divider */ /
+ 0.195 /* V/A */);
+ }
+
+ ::std::array<uint32_t, 3> DoIteration(const float raw_currents[3],
+ uint32_t theta,
+ const float command_current) override;
+
+ int16_t Debug(uint32_t theta) override;
+
+ float estimated_velocity() const override { return estimated_velocity_; }
+
+ private:
+ const ComplexMatrix<3, 1> E1Unrotated_;
+
+ float estimated_velocity_ = 0;
+ float filtered_current_ = 0;
+
+ ::Eigen::Matrix<float, 3, 1> I_last_ = ::Eigen::Matrix<float, 3, 1>::Zero();
+
+ int16_t debug_[9];
+};
+
+} // namespace salsa
+} // namespace frc971
+
+#endif // MOTORS_PISTOL_GRIP_MOTOR_CONTROLS_H_
diff --git a/motors/pistol_grip/vtable_trigger.cc b/motors/pistol_grip/vtable_trigger.cc
new file mode 100644
index 0000000..44306f1
--- /dev/null
+++ b/motors/pistol_grip/vtable_trigger.cc
@@ -0,0 +1,4099 @@
+extern const float kTriggerCoggingTorque[4096];
+const float kTriggerCoggingTorque[4096] = {
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.095850f,
+ 0.086438f,
+ 0.037663f,
+ 0.004088f,
+ -0.018050f,
+ -0.037200f,
+ -0.055087f,
+ -0.067375f,
+ -0.079263f,
+ -0.090475f,
+ -0.099812f,
+ -0.108475f,
+ -0.114700f,
+ -0.121000f,
+ -0.126575f,
+ -0.131513f,
+ -0.138275f,
+ -0.140188f,
+ -0.089575f,
+ -0.090725f,
+ -0.091975f,
+ -0.093525f,
+ -0.095687f,
+ -0.085038f,
+ -0.075912f,
+ -0.038337f,
+ 0.020487f,
+ 0.018800f,
+ 0.015163f,
+ 0.021087f,
+ 0.019475f,
+ 0.017563f,
+ 0.015663f,
+ 0.013562f,
+ 0.011463f,
+ 0.009313f,
+ 0.006513f,
+ 0.004063f,
+ -0.000300f,
+ -0.005000f,
+ -0.011125f,
+ -0.015825f,
+ -0.025237f,
+ -0.033537f,
+ -0.041925f,
+ -0.049800f,
+ -0.058250f,
+ -0.063200f,
+ -0.068875f,
+ -0.075713f,
+ -0.078550f,
+ -0.081937f,
+ -0.085050f,
+ -0.087250f,
+ -0.084975f,
+ -0.086725f,
+ -0.088300f,
+ -0.083563f,
+ -0.076812f,
+ -0.045963f,
+ -0.039338f,
+ -0.033738f,
+ 0.016263f,
+ 0.014975f,
+ 0.041362f,
+ 0.071925f,
+ 0.097300f,
+ 0.095462f,
+ 0.093775f,
+ 0.128713f,
+ 0.160250f,
+ 0.179962f,
+ 0.210862f,
+ 0.209350f,
+ 0.215088f,
+ 0.234825f,
+ 0.249500f,
+ 0.284813f,
+ 0.284037f,
+ 0.281750f,
+ 0.278487f,
+ 0.287712f,
+ 0.285712f,
+ 0.281788f,
+ 0.276450f,
+ 0.271850f,
+ 0.266775f,
+ 0.260288f,
+ 0.252338f,
+ 0.243812f,
+ 0.230750f,
+ 0.220963f,
+ 0.205525f,
+ 0.191488f,
+ 0.177575f,
+ 0.161500f,
+ 0.151337f,
+ 0.138650f,
+ 0.125238f,
+ 0.116425f,
+ 0.107787f,
+ 0.099488f,
+ 0.093688f,
+ 0.087200f,
+ 0.083525f,
+ 0.080738f,
+ 0.078363f,
+ 0.076625f,
+ 0.082588f,
+ 0.081275f,
+ 0.080125f,
+ 0.079225f,
+ 0.094587f,
+ 0.119413f,
+ 0.145288f,
+ 0.142837f,
+ 0.140788f,
+ 0.162325f,
+ 0.176125f,
+ 0.196550f,
+ 0.223712f,
+ 0.225900f,
+ 0.241613f,
+ 0.249988f,
+ 0.263437f,
+ 0.285087f,
+ 0.283513f,
+ 0.281413f,
+ 0.279313f,
+ 0.277250f,
+ 0.274587f,
+ 0.271388f,
+ 0.267250f,
+ 0.263862f,
+ 0.257437f,
+ 0.248650f,
+ 0.237338f,
+ 0.220875f,
+ 0.204050f,
+ 0.186075f,
+ 0.168587f,
+ 0.152138f,
+ 0.130925f,
+ 0.109488f,
+ 0.090725f,
+ 0.069775f,
+ 0.048738f,
+ 0.029588f,
+ 0.009537f,
+ -0.006150f,
+ -0.022812f,
+ -0.037187f,
+ -0.051350f,
+ -0.063488f,
+ -0.071962f,
+ -0.077400f,
+ -0.082212f,
+ -0.086075f,
+ -0.088587f,
+ -0.084737f,
+ -0.078788f,
+ -0.080238f,
+ -0.081325f,
+ -0.070538f,
+ -0.071813f,
+ -0.034112f,
+ -0.001700f,
+ 0.011362f,
+ 0.009462f,
+ 0.016212f,
+ 0.050000f,
+ 0.057725f,
+ 0.071137f,
+ 0.069988f,
+ 0.085550f,
+ 0.112325f,
+ 0.121325f,
+ 0.139550f,
+ 0.138637f,
+ 0.136887f,
+ 0.135200f,
+ 0.133338f,
+ 0.131300f,
+ 0.128712f,
+ 0.126287f,
+ 0.123650f,
+ 0.121213f,
+ 0.116075f,
+ 0.109487f,
+ 0.104675f,
+ 0.095062f,
+ 0.089487f,
+ 0.078462f,
+ 0.067963f,
+ 0.056787f,
+ 0.045475f,
+ 0.038225f,
+ 0.029613f,
+ 0.022287f,
+ 0.015137f,
+ 0.011813f,
+ 0.008137f,
+ 0.003500f,
+ 0.000537f,
+ -0.002138f,
+ -0.004312f,
+ -0.006300f,
+ -0.004125f,
+ -0.005300f,
+ -0.001175f,
+ 0.006263f,
+ 0.019537f,
+ 0.048550f,
+ 0.053125f,
+ 0.065400f,
+ 0.078650f,
+ 0.097363f,
+ 0.109213f,
+ 0.126813f,
+ 0.148375f,
+ 0.179625f,
+ 0.187575f,
+ 0.204825f,
+ 0.207988f,
+ 0.230262f,
+ 0.254038f,
+ 0.261525f,
+ 0.260475f,
+ 0.259050f,
+ 0.264487f,
+ 0.262563f,
+ 0.260125f,
+ 0.257362f,
+ 0.254775f,
+ 0.251300f,
+ 0.246063f,
+ 0.237525f,
+ 0.228075f,
+ 0.218362f,
+ 0.203425f,
+ 0.188587f,
+ 0.172575f,
+ 0.155413f,
+ 0.138237f,
+ 0.118150f,
+ 0.100975f,
+ 0.082625f,
+ 0.065188f,
+ 0.047250f,
+ 0.030250f,
+ 0.013050f,
+ 0.021267f,
+ 0.008389f,
+ -0.001156f,
+ -0.009322f,
+ -0.015844f,
+ -0.021689f,
+ -0.024911f,
+ -0.028489f,
+ -0.031400f,
+ -0.030633f,
+ -0.032733f,
+ -0.034544f,
+ -0.036167f,
+ -0.034833f,
+ -0.032344f,
+ -0.017200f,
+ 0.022933f,
+ 0.026033f,
+ 0.030800f,
+ 0.029389f,
+ 0.028100f,
+ 0.033556f,
+ 0.072667f,
+ 0.110722f,
+ 0.114011f,
+ 0.112756f,
+ 0.111311f,
+ 0.114944f,
+ 0.113156f,
+ 0.111011f,
+ 0.108700f,
+ 0.105811f,
+ 0.102067f,
+ 0.098978f,
+ 0.095389f,
+ 0.089022f,
+ 0.081656f,
+ 0.075067f,
+ 0.065411f,
+ 0.054156f,
+ 0.039533f,
+ 0.028378f,
+ 0.016389f,
+ 0.002178f,
+ -0.012389f,
+ -0.025756f,
+ -0.036489f,
+ -0.047878f,
+ -0.057678f,
+ -0.061489f,
+ -0.068633f,
+ -0.073333f,
+ -0.076822f,
+ -0.078844f,
+ -0.081778f,
+ -0.083989f,
+ -0.086200f,
+ -0.088478f,
+ -0.090522f,
+ -0.092333f,
+ -0.087111f,
+ -0.071678f,
+ -0.019722f,
+ -0.020311f,
+ 0.009078f,
+ 0.016033f,
+ 0.014800f,
+ 0.025644f,
+ 0.031744f,
+ 0.061611f,
+ 0.105722f,
+ 0.105589f,
+ 0.125400f,
+ 0.133600f,
+ 0.137522f,
+ 0.141978f,
+ 0.140278f,
+ 0.138156f,
+ 0.135900f,
+ 0.133022f,
+ 0.129411f,
+ 0.124267f,
+ 0.117989f,
+ 0.111867f,
+ 0.101911f,
+ 0.086778f,
+ 0.072178f,
+ 0.057489f,
+ 0.038333f,
+ 0.017733f,
+ -0.000589f,
+ -0.023233f,
+ -0.042589f,
+ -0.064822f,
+ -0.086356f,
+ -0.107956f,
+ -0.127911f,
+ -0.148400f,
+ -0.164389f,
+ -0.180933f,
+ -0.198878f,
+ -0.210544f,
+ -0.219044f,
+ -0.224578f,
+ -0.230256f,
+ -0.234744f,
+ -0.237422f,
+ -0.240133f,
+ -0.242733f,
+ -0.244967f,
+ -0.246367f,
+ -0.227211f,
+ -0.220067f,
+ -0.214133f,
+ -0.175744f,
+ -0.165400f,
+ -0.166856f,
+ -0.168422f,
+ -0.141333f,
+ -0.121200f,
+ -0.081567f,
+ -0.073078f,
+ -0.055167f,
+ -0.056178f,
+ -0.056889f,
+ -0.015822f,
+ -0.009767f,
+ -0.010356f,
+ -0.001033f,
+ -0.002289f,
+ -0.003911f,
+ -0.005711f,
+ -0.006200f,
+ -0.008122f,
+ -0.010767f,
+ -0.013311f,
+ -0.019389f,
+ -0.022478f,
+ -0.025167f,
+ -0.030189f,
+ -0.036000f,
+ -0.041389f,
+ -0.046122f,
+ -0.051022f,
+ -0.056489f,
+ -0.059667f,
+ -0.064511f,
+ -0.069000f,
+ -0.072100f,
+ -0.074867f,
+ -0.077222f,
+ -0.079644f,
+ -0.082122f,
+ -0.080467f,
+ -0.081700f,
+ -0.075933f,
+ -0.047489f,
+ -0.039900f,
+ -0.005811f,
+ -0.006311f,
+ 0.002911f,
+ 0.011711f,
+ 0.024567f,
+ 0.053167f,
+ 0.053067f,
+ 0.089644f,
+ 0.118100f,
+ 0.132167f,
+ 0.139067f,
+ 0.138278f,
+ 0.150322f,
+ 0.203078f,
+ 0.237000f,
+ 0.236322f,
+ 0.234822f,
+ 0.233156f,
+ 0.235522f,
+ 0.233556f,
+ 0.231400f,
+ 0.228867f,
+ 0.226622f,
+ 0.220889f,
+ 0.212100f,
+ 0.208944f,
+ 0.199933f,
+ 0.191333f,
+ 0.181733f,
+ 0.162056f,
+ 0.152500f,
+ 0.131122f,
+ 0.109444f,
+ 0.094500f,
+ 0.073700f,
+ 0.061667f,
+ 0.045456f,
+ 0.027656f,
+ 0.012456f,
+ 0.004033f,
+ -0.005844f,
+ -0.015900f,
+ -0.023944f,
+ -0.028744f,
+ -0.033811f,
+ -0.039256f,
+ -0.041622f,
+ -0.043900f,
+ -0.046756f,
+ -0.042889f,
+ -0.044278f,
+ -0.032956f,
+ -0.033467f,
+ -0.024533f,
+ -0.016456f,
+ 0.004022f,
+ 0.020544f,
+ 0.024867f,
+ 0.043033f,
+ 0.047811f,
+ 0.067222f,
+ 0.087400f,
+ 0.094467f,
+ 0.100067f,
+ 0.104956f,
+ 0.103978f,
+ 0.106767f,
+ 0.105100f,
+ 0.102789f,
+ 0.104733f,
+ 0.100211f,
+ 0.097044f,
+ 0.092833f,
+ 0.088533f,
+ 0.079822f,
+ 0.075144f,
+ 0.066511f,
+ 0.054233f,
+ 0.040744f,
+ 0.029889f,
+ 0.005089f,
+ -0.010267f,
+ -0.022122f,
+ -0.038322f,
+ -0.061789f,
+ -0.076700f,
+ -0.096778f,
+ -0.109967f,
+ -0.119511f,
+ -0.129011f,
+ -0.136044f,
+ -0.142933f,
+ -0.148989f,
+ -0.153289f,
+ -0.156922f,
+ -0.159911f,
+ -0.162022f,
+ -0.166022f,
+ -0.167700f,
+ -0.169200f,
+ -0.155044f,
+ -0.155778f,
+ -0.143989f,
+ -0.134411f,
+ -0.114233f,
+ -0.111356f,
+ -0.106989f,
+ -0.087267f,
+ -0.051256f,
+ -0.033867f,
+ -0.009789f,
+ -0.006789f,
+ 0.016556f,
+ 0.016122f,
+ 0.034411f,
+ 0.053689f,
+ 0.056700f,
+ 0.055800f,
+ 0.054611f,
+ 0.056122f,
+ 0.056289f,
+ 0.054244f,
+ 0.052167f,
+ 0.049422f,
+ 0.046944f,
+ 0.043400f,
+ 0.037522f,
+ 0.032711f,
+ 0.028267f,
+ 0.020489f,
+ 0.013389f,
+ 0.006600f,
+ -0.005867f,
+ -0.013044f,
+ -0.025333f,
+ -0.035811f,
+ -0.043167f,
+ -0.049056f,
+ -0.053400f,
+ -0.057122f,
+ -0.063100f,
+ -0.066833f,
+ -0.070300f,
+ -0.072844f,
+ -0.074956f,
+ -0.077044f,
+ -0.079167f,
+ -0.081067f,
+ -0.082567f,
+ -0.076267f,
+ -0.058244f,
+ -0.052867f,
+ -0.026878f,
+ 0.008500f,
+ 0.013889f,
+ 0.029656f,
+ 0.028200f,
+ 0.037356f,
+ 0.044756f,
+ 0.059767f,
+ 0.106800f,
+ 0.117189f,
+ 0.130078f,
+ 0.129467f,
+ 0.128100f,
+ 0.126622f,
+ 0.130733f,
+ 0.128733f,
+ 0.125789f,
+ 0.123489f,
+ 0.120422f,
+ 0.113567f,
+ 0.109289f,
+ 0.101100f,
+ 0.093967f,
+ 0.082478f,
+ 0.068222f,
+ 0.052022f,
+ 0.037433f,
+ 0.016367f,
+ -0.004478f,
+ -0.020589f,
+ -0.040200f,
+ -0.057778f,
+ -0.077122f,
+ -0.096767f,
+ -0.114933f,
+ -0.132389f,
+ -0.145189f,
+ -0.157633f,
+ -0.164967f,
+ -0.171278f,
+ -0.176956f,
+ -0.181344f,
+ -0.185000f,
+ -0.187456f,
+ -0.190111f,
+ -0.192067f,
+ -0.194244f,
+ -0.196422f,
+ -0.200167f,
+ -0.201933f,
+ -0.193633f,
+ -0.154867f,
+ -0.139411f,
+ -0.131567f,
+ -0.112178f,
+ -0.106633f,
+ -0.096167f,
+ -0.080278f,
+ -0.072400f,
+ -0.059167f,
+ -0.036633f,
+ -0.015378f,
+ -0.016367f,
+ -0.012067f,
+ -0.010044f,
+ -0.011600f,
+ -0.013367f,
+ -0.015389f,
+ -0.016444f,
+ -0.019156f,
+ -0.022422f,
+ -0.025933f,
+ -0.029744f,
+ -0.035422f,
+ -0.043456f,
+ -0.052322f,
+ -0.062911f,
+ -0.073900f,
+ -0.083567f,
+ -0.096122f,
+ -0.110822f,
+ -0.137422f,
+ -0.138344f,
+ -0.202422f,
+ -0.145478f,
+ -0.114122f,
+ -0.122889f,
+ -0.129000f,
+ -0.134589f,
+ -0.164567f,
+ -0.133222f,
+ -0.173778f,
+ -0.107011f,
+ 0.011456f,
+ -0.008111f,
+ -0.040733f,
+ 0.001767f,
+ 0.137378f,
+ 0.089244f,
+ 0.111556f,
+ 0.107078f,
+ 0.111367f,
+ 0.159856f,
+ 0.158533f,
+ 0.186556f,
+ 0.204511f,
+ 0.218489f,
+ 0.232200f,
+ 0.243889f,
+ 0.244356f,
+ 0.241300f,
+ 0.237844f,
+ 0.234100f,
+ 0.230767f,
+ 0.228300f,
+ 0.225733f,
+ 0.222911f,
+ 0.218133f,
+ 0.213356f,
+ 0.206711f,
+ 0.198878f,
+ 0.188178f,
+ 0.175100f,
+ 0.158556f,
+ 0.141922f,
+ 0.124556f,
+ 0.107500f,
+ 0.090211f,
+ 0.069722f,
+ 0.053956f,
+ 0.035733f,
+ 0.019933f,
+ 0.003978f,
+ -0.006311f,
+ -0.015589f,
+ -0.023511f,
+ -0.028433f,
+ -0.033744f,
+ -0.036522f,
+ -0.039411f,
+ -0.041322f,
+ -0.038856f,
+ -0.040433f,
+ -0.041922f,
+ -0.043178f,
+ -0.038022f,
+ -0.030544f,
+ -0.018567f,
+ 0.024578f,
+ 0.029122f,
+ 0.031011f,
+ 0.045722f,
+ 0.067189f,
+ 0.073278f,
+ 0.092700f,
+ 0.096589f,
+ 0.135889f,
+ 0.136000f,
+ 0.166200f,
+ 0.175422f,
+ 0.179922f,
+ 0.178478f,
+ 0.176867f,
+ 0.175433f,
+ 0.173978f,
+ 0.171900f,
+ 0.169822f,
+ 0.167222f,
+ 0.164867f,
+ 0.161300f,
+ 0.155678f,
+ 0.151522f,
+ 0.144978f,
+ 0.137200f,
+ 0.131789f,
+ 0.123033f,
+ 0.112722f,
+ 0.105911f,
+ 0.098778f,
+ 0.094433f,
+ 0.088778f,
+ 0.084578f,
+ 0.081922f,
+ 0.078344f,
+ 0.075156f,
+ 0.073222f,
+ 0.071633f,
+ 0.072733f,
+ 0.071356f,
+ 0.070000f,
+ 0.072211f,
+ 0.080578f,
+ 0.098289f,
+ 0.119644f,
+ 0.126411f,
+ 0.148322f,
+ 0.158778f,
+ 0.189244f,
+ 0.201022f,
+ 0.208878f,
+ 0.229167f,
+ 0.245189f,
+ 0.263267f,
+ 0.297644f,
+ 0.305256f,
+ 0.312122f,
+ 0.336344f,
+ 0.364100f,
+ 0.366511f,
+ 0.366911f,
+ 0.365411f,
+ 0.363744f,
+ 0.365956f,
+ 0.363978f,
+ 0.361333f,
+ 0.356722f,
+ 0.353000f,
+ 0.346744f,
+ 0.336456f,
+ 0.331322f,
+ 0.320778f,
+ 0.308789f,
+ 0.290522f,
+ 0.275222f,
+ 0.259433f,
+ 0.246500f,
+ 0.229689f,
+ 0.212467f,
+ 0.198067f,
+ 0.184300f,
+ 0.172944f,
+ 0.161211f,
+ 0.154111f,
+ 0.147489f,
+ 0.141922f,
+ 0.139044f,
+ 0.136411f,
+ 0.133867f,
+ 0.131456f,
+ 0.129378f,
+ 0.131311f,
+ 0.129533f,
+ 0.131000f,
+ 0.132700f,
+ 0.135322f,
+ 0.163711f,
+ 0.194578f,
+ 0.200278f,
+ 0.211744f,
+ 0.212156f,
+ 0.223467f,
+ 0.229933f,
+ 0.232178f,
+ 0.271522f,
+ 0.300856f,
+ 0.300300f,
+ 0.311533f,
+ 0.310011f,
+ 0.307722f,
+ 0.305011f,
+ 0.302033f,
+ 0.298900f,
+ 0.295656f,
+ 0.290856f,
+ 0.286244f,
+ 0.279644f,
+ 0.272100f,
+ 0.262400f,
+ 0.248778f,
+ 0.232333f,
+ 0.211367f,
+ 0.193467f,
+ 0.174144f,
+ 0.154900f,
+ 0.134567f,
+ 0.114922f,
+ 0.093156f,
+ 0.076622f,
+ 0.058656f,
+ 0.039589f,
+ 0.025178f,
+ 0.010256f,
+ 0.000000f,
+ -0.009822f,
+ -0.017011f,
+ -0.021611f,
+ -0.025867f,
+ -0.028856f,
+ -0.032022f,
+ -0.034600f,
+ -0.036889f,
+ -0.038822f,
+ -0.032822f,
+ -0.030067f,
+ -0.022944f,
+ -0.018000f,
+ 0.020189f,
+ 0.019178f,
+ 0.022222f,
+ 0.020256f,
+ 0.022200f,
+ 0.021444f,
+ 0.069300f,
+ 0.100689f,
+ 0.107500f,
+ 0.106200f,
+ 0.103267f,
+ 0.101122f,
+ 0.102078f,
+ 0.099800f,
+ 0.097333f,
+ 0.094356f,
+ 0.090811f,
+ 0.084789f,
+ 0.080200f,
+ 0.072978f,
+ 0.065089f,
+ 0.055722f,
+ 0.040433f,
+ 0.027511f,
+ 0.015322f,
+ 0.000267f,
+ -0.016111f,
+ -0.028744f,
+ -0.045389f,
+ -0.055944f,
+ -0.071444f,
+ -0.081378f,
+ -0.092933f,
+ -0.098378f,
+ -0.105900f,
+ -0.110444f,
+ -0.114522f,
+ -0.117200f,
+ -0.119311f,
+ -0.121322f,
+ -0.123278f,
+ -0.125222f,
+ -0.127100f,
+ -0.130144f,
+ -0.133467f,
+ -0.134611f,
+ -0.094533f,
+ -0.068278f,
+ -0.048878f,
+ -0.043689f,
+ -0.045200f,
+ -0.037078f,
+ -0.032011f,
+ -0.033067f,
+ -0.011278f,
+ 0.043733f,
+ 0.049433f,
+ 0.058367f,
+ 0.056844f,
+ 0.054944f,
+ 0.052844f,
+ 0.050511f,
+ 0.047944f,
+ 0.045178f,
+ 0.041144f,
+ 0.036422f,
+ 0.028956f,
+ 0.022256f,
+ 0.012822f,
+ -0.000744f,
+ -0.014300f,
+ -0.031900f,
+ -0.047544f,
+ -0.067500f,
+ -0.084967f,
+ -0.104667f,
+ -0.122133f,
+ -0.141322f,
+ -0.158400f,
+ -0.178056f,
+ -0.194256f,
+ -0.209067f,
+ -0.222778f,
+ -0.234067f,
+ -0.243211f,
+ -0.249778f,
+ -0.255311f,
+ -0.261022f,
+ -0.265578f,
+ -0.268800f,
+ -0.271400f,
+ -0.266489f,
+ -0.252733f,
+ -0.254789f,
+ -0.256133f,
+ -0.202756f,
+ -0.203667f,
+ -0.187256f,
+ -0.188267f,
+ -0.176400f,
+ -0.144522f,
+ -0.132356f,
+ -0.116289f,
+ -0.105144f,
+ -0.094544f,
+ -0.084511f,
+ -0.064478f,
+ -0.051522f,
+ -0.029800f,
+ -0.016167f,
+ -0.011344f,
+ -0.008956f,
+ -0.011122f,
+ -0.013556f,
+ -0.016089f,
+ -0.019011f,
+ -0.022278f,
+ -0.025333f,
+ -0.027989f,
+ -0.032711f,
+ -0.038044f,
+ -0.043767f,
+ -0.049589f,
+ -0.056956f,
+ -0.061244f,
+ -0.066922f,
+ -0.073500f,
+ -0.077267f,
+ -0.080856f,
+ -0.083589f,
+ -0.087444f,
+ -0.089500f,
+ -0.091722f,
+ -0.093833f,
+ -0.095644f,
+ -0.096967f,
+ -0.090300f,
+ -0.087778f,
+ -0.084256f,
+ -0.063011f,
+ -0.047833f,
+ -0.048533f,
+ -0.047289f,
+ -0.017389f,
+ -0.003900f,
+ 0.015422f,
+ 0.053400f,
+ 0.083189f,
+ 0.092222f,
+ 0.105000f,
+ 0.126644f,
+ 0.137767f,
+ 0.137111f,
+ 0.148533f,
+ 0.190211f,
+ 0.210156f,
+ 0.209033f,
+ 0.207333f,
+ 0.204933f,
+ 0.202422f,
+ 0.199044f,
+ 0.194700f,
+ 0.188300f,
+ 0.181289f,
+ 0.173078f,
+ 0.160556f,
+ 0.145144f,
+ 0.128422f,
+ 0.109433f,
+ 0.090489f,
+ 0.073078f,
+ 0.049322f,
+ 0.026233f,
+ 0.002933f,
+ -0.016644f,
+ -0.039167f,
+ -0.059356f,
+ -0.079889f,
+ -0.096822f,
+ -0.116778f,
+ -0.132478f,
+ -0.146567f,
+ -0.154011f,
+ -0.161300f,
+ -0.165678f,
+ -0.168844f,
+ -0.172389f,
+ -0.174944f,
+ -0.174478f,
+ -0.176167f,
+ -0.177856f,
+ -0.175856f,
+ -0.173233f,
+ -0.157267f,
+ -0.119156f,
+ -0.119822f,
+ -0.099189f,
+ -0.083578f,
+ -0.072411f,
+ -0.059444f,
+ -0.058711f,
+ -0.047744f,
+ -0.024600f,
+ -0.025278f,
+ 0.033144f,
+ 0.041789f,
+ 0.040556f,
+ 0.039078f,
+ 0.037567f,
+ 0.038811f,
+ 0.040811f,
+ 0.038622f,
+ 0.036167f,
+ 0.033733f,
+ 0.031344f,
+ 0.025867f,
+ 0.021544f,
+ 0.017767f,
+ 0.013256f,
+ 0.006811f,
+ 0.002056f,
+ -0.003767f,
+ -0.009756f,
+ -0.014156f,
+ -0.017789f,
+ -0.021100f,
+ -0.025978f,
+ -0.030100f,
+ -0.033022f,
+ -0.035333f,
+ -0.037356f,
+ -0.039256f,
+ -0.034000f,
+ -0.035511f,
+ -0.036789f,
+ -0.037744f,
+ -0.005267f,
+ 0.005389f,
+ 0.007611f,
+ 0.022344f,
+ 0.037556f,
+ 0.081911f,
+ 0.098700f,
+ 0.114422f,
+ 0.125133f,
+ 0.128622f,
+ 0.163000f,
+ 0.187656f,
+ 0.212056f,
+ 0.217978f,
+ 0.217722f,
+ 0.224389f,
+ 0.286856f,
+ 0.286644f,
+ 0.285922f,
+ 0.287622f,
+ 0.290033f,
+ 0.288000f,
+ 0.285689f,
+ 0.283100f,
+ 0.279989f,
+ 0.276700f,
+ 0.271622f,
+ 0.263944f,
+ 0.256978f,
+ 0.247078f,
+ 0.234011f,
+ 0.220289f,
+ 0.205867f,
+ 0.189111f,
+ 0.174056f,
+ 0.158189f,
+ 0.140322f,
+ 0.129289f,
+ 0.112756f,
+ 0.102133f,
+ 0.091256f,
+ 0.082989f,
+ 0.077189f,
+ 0.071222f,
+ 0.067800f,
+ 0.063789f,
+ 0.060300f,
+ 0.063156f,
+ 0.061389f,
+ 0.059567f,
+ 0.063056f,
+ 0.061500f,
+ 0.064122f,
+ 0.067489f,
+ 0.082311f,
+ 0.109367f,
+ 0.114956f,
+ 0.129789f,
+ 0.142978f,
+ 0.141722f,
+ 0.158244f,
+ 0.187322f,
+ 0.186233f,
+ 0.184878f,
+ 0.190033f,
+ 0.188233f,
+ 0.186189f,
+ 0.187233f,
+ 0.184744f,
+ 0.181389f,
+ 0.176422f,
+ 0.171211f,
+ 0.164233f,
+ 0.157011f,
+ 0.146489f,
+ 0.130900f,
+ 0.114389f,
+ 0.097856f,
+ 0.076933f,
+ 0.057467f,
+ 0.037644f,
+ 0.016322f,
+ -0.004289f,
+ -0.025489f,
+ -0.044978f,
+ -0.066200f,
+ -0.083833f,
+ -0.105700f,
+ -0.117978f,
+ -0.135178f,
+ -0.146467f,
+ -0.153711f,
+ -0.161500f,
+ -0.165156f,
+ -0.170133f,
+ -0.173400f,
+ -0.176211f,
+ -0.179044f,
+ -0.182422f,
+ -0.186389f,
+ -0.188222f,
+ -0.172867f,
+ -0.172656f,
+ -0.166667f,
+ -0.114867f,
+ -0.095067f,
+ -0.082922f,
+ -0.084911f,
+ -0.075311f,
+ -0.077089f,
+ -0.068400f,
+ -0.054367f,
+ -0.016578f,
+ -0.013156f,
+ -0.008744f,
+ -0.010111f,
+ -0.011711f,
+ -0.013522f,
+ -0.015344f,
+ -0.017400f,
+ -0.015356f,
+ -0.019678f,
+ -0.023333f,
+ -0.027056f,
+ -0.030478f,
+ -0.039578f,
+ -0.046300f,
+ -0.053844f,
+ -0.065111f,
+ -0.074722f,
+ -0.083489f,
+ -0.094878f,
+ -0.102533f,
+ -0.111200f,
+ -0.116767f,
+ -0.123011f,
+ -0.126322f,
+ -0.130233f,
+ -0.134022f,
+ -0.130756f,
+ -0.132767f,
+ -0.134711f,
+ -0.129133f,
+ -0.130844f,
+ -0.126311f,
+ -0.124389f,
+ -0.112467f,
+ -0.073467f,
+ -0.074544f,
+ -0.075500f,
+ -0.045500f,
+ -0.011111f,
+ -0.000222f,
+ 0.010811f,
+ 0.037044f,
+ 0.047811f,
+ 0.059256f,
+ 0.082211f,
+ 0.095378f,
+ 0.113922f,
+ 0.127211f,
+ 0.167389f,
+ 0.166589f,
+ 0.165544f,
+ 0.163722f,
+ 0.166467f,
+ 0.164289f,
+ 0.162044f,
+ 0.159000f,
+ 0.154933f,
+ 0.149333f,
+ 0.145344f,
+ 0.137689f,
+ 0.129933f,
+ 0.119011f,
+ 0.106411f,
+ 0.091022f,
+ 0.075700f,
+ 0.060111f,
+ 0.045956f,
+ 0.028733f,
+ 0.016122f,
+ 0.000456f,
+ -0.008844f,
+ -0.021433f,
+ -0.027822f,
+ -0.037800f,
+ -0.043456f,
+ -0.046278f,
+ -0.050422f,
+ -0.054033f,
+ -0.056922f,
+ -0.059178f,
+ -0.061178f,
+ -0.051067f,
+ -0.052189f,
+ -0.052933f,
+ -0.036522f,
+ -0.002267f,
+ 0.011256f,
+ 0.016722f,
+ 0.025822f,
+ 0.026389f,
+ 0.041378f,
+ 0.063489f,
+ 0.091756f,
+ 0.095667f,
+ 0.111178f,
+ 0.110911f,
+ 0.138200f,
+ 0.154422f,
+ 0.167322f,
+ 0.166678f,
+ 0.165444f,
+ 0.172978f,
+ 0.170989f,
+ 0.168667f,
+ 0.165911f,
+ 0.162311f,
+ 0.158300f,
+ 0.154300f,
+ 0.148878f,
+ 0.142022f,
+ 0.135111f,
+ 0.128011f,
+ 0.114556f,
+ 0.104456f,
+ 0.091233f,
+ 0.081500f,
+ 0.072100f,
+ 0.067722f,
+ 0.061144f,
+ 0.056956f,
+ 0.053444f,
+ 0.049611f,
+ 0.046044f,
+ 0.048300f,
+ 0.046567f,
+ 0.044844f,
+ 0.050533f,
+ 0.054056f,
+ 0.053867f,
+ 0.057667f,
+ 0.075333f,
+ 0.092289f,
+ 0.104878f,
+ 0.118544f,
+ 0.148133f,
+ 0.164289f,
+ 0.175767f,
+ 0.179678f,
+ 0.205100f,
+ 0.240022f,
+ 0.249722f,
+ 0.264844f,
+ 0.282267f,
+ 0.308311f,
+ 0.307533f,
+ 0.312278f,
+ 0.315011f,
+ 0.317444f,
+ 0.315356f,
+ 0.311189f,
+ 0.308800f,
+ 0.305378f,
+ 0.298456f,
+ 0.291322f,
+ 0.282078f,
+ 0.269656f,
+ 0.254633f,
+ 0.223600f,
+ 0.206937f,
+ 0.186637f,
+ 0.168212f,
+ 0.147600f,
+ 0.125762f,
+ 0.106925f,
+ 0.087950f,
+ 0.068825f,
+ 0.048275f,
+ 0.033750f,
+ 0.020487f,
+ 0.013150f,
+ 0.003463f,
+ -0.000300f,
+ -0.004688f,
+ -0.009862f,
+ -0.012937f,
+ -0.014512f,
+ -0.016362f,
+ -0.017800f,
+ -0.019025f,
+ -0.013850f,
+ -0.014350f,
+ -0.014625f,
+ 0.008650f,
+ 0.042788f,
+ 0.042350f,
+ 0.046075f,
+ 0.075900f,
+ 0.075612f,
+ 0.104062f,
+ 0.115525f,
+ 0.124863f,
+ 0.130425f,
+ 0.153550f,
+ 0.180412f,
+ 0.184850f,
+ 0.184150f,
+ 0.188550f,
+ 0.187450f,
+ 0.186200f,
+ 0.184375f,
+ 0.182925f,
+ 0.181513f,
+ 0.180075f,
+ 0.177537f,
+ 0.174738f,
+ 0.166488f,
+ 0.162538f,
+ 0.158687f,
+ 0.151350f,
+ 0.141400f,
+ 0.131050f,
+ 0.126537f,
+ 0.118325f,
+ 0.110800f,
+ 0.105075f,
+ 0.099450f,
+ 0.095562f,
+ 0.091587f,
+ 0.087787f,
+ 0.086038f,
+ 0.083962f,
+ 0.081988f,
+ 0.080713f,
+ 0.079387f,
+ 0.092400f,
+ 0.091650f,
+ 0.096300f,
+ 0.102363f,
+ 0.130788f,
+ 0.142975f,
+ 0.156062f,
+ 0.177737f,
+ 0.181750f,
+ 0.194938f,
+ 0.218562f,
+ 0.237150f,
+ 0.241675f,
+ 0.263600f,
+ 0.278925f,
+ 0.280550f,
+ 0.285737f,
+ 0.293287f,
+ 0.292162f,
+ 0.290687f,
+ 0.288775f,
+ 0.286700f,
+ 0.283950f,
+ 0.279587f,
+ 0.273450f,
+ 0.264912f,
+ 0.260500f,
+ 0.253000f,
+ 0.242262f,
+ 0.228337f,
+ 0.205113f,
+ 0.189350f,
+ 0.167513f,
+ 0.152163f,
+ 0.137012f,
+ 0.109750f,
+ 0.090500f,
+ 0.073212f,
+ 0.055225f,
+ 0.039313f,
+ 0.030138f,
+ 0.020412f,
+ 0.010712f,
+ 0.005900f,
+ -0.003062f,
+ -0.006125f,
+ -0.012237f,
+ -0.015137f,
+ -0.017588f,
+ -0.019413f,
+ -0.020788f,
+ -0.022250f,
+ -0.019425f,
+ -0.016475f,
+ -0.016550f,
+ -0.010287f,
+ 0.007863f,
+ 0.010088f,
+ 0.015863f,
+ 0.025475f,
+ 0.054750f,
+ 0.054762f,
+ 0.054750f,
+ 0.063125f,
+ 0.061950f,
+ 0.060675f,
+ 0.059250f,
+ 0.057775f,
+ 0.055425f,
+ 0.052138f,
+ 0.049225f,
+ 0.044250f,
+ 0.041063f,
+ 0.034250f,
+ 0.020950f,
+ 0.013100f,
+ -0.000088f,
+ -0.018775f,
+ -0.032112f,
+ -0.051988f,
+ -0.066225f,
+ -0.090050f,
+ -0.106900f,
+ -0.127850f,
+ -0.147450f,
+ -0.160662f,
+ -0.185425f,
+ -0.197650f,
+ -0.212438f,
+ -0.221537f,
+ -0.230100f,
+ -0.235375f,
+ -0.242938f,
+ -0.248425f,
+ -0.252638f,
+ -0.256400f,
+ -0.258512f,
+ -0.260412f,
+ -0.262063f,
+ -0.263437f,
+ -0.250412f,
+ -0.247262f,
+ -0.243762f,
+ -0.216513f,
+ -0.188000f,
+ -0.187738f,
+ -0.166950f,
+ -0.157925f,
+ -0.144262f,
+ -0.141987f,
+ -0.126812f,
+ -0.114813f,
+ -0.078900f,
+ -0.066388f,
+ -0.059825f,
+ -0.054700f,
+ -0.051788f,
+ -0.046537f,
+ -0.048375f,
+ -0.050350f,
+ -0.052612f,
+ -0.054575f,
+ -0.056838f,
+ -0.059225f,
+ -0.066025f,
+ -0.069225f,
+ -0.072987f,
+ -0.078925f,
+ -0.084250f,
+ -0.092325f,
+ -0.100650f,
+ -0.108437f,
+ -0.113962f,
+ -0.125875f,
+ -0.129862f,
+ -0.136650f,
+ -0.141538f,
+ -0.144613f,
+ -0.148025f,
+ -0.150888f,
+ -0.153487f,
+ -0.151950f,
+ -0.154062f,
+ -0.155637f,
+ -0.156587f,
+ -0.139875f,
+ -0.132288f,
+ -0.122637f,
+ -0.099613f,
+ -0.080550f,
+ -0.065850f,
+ -0.065825f,
+ -0.041462f,
+ -0.022937f,
+ -0.003950f,
+ 0.004137f,
+ 0.016862f,
+ 0.031600f,
+ 0.043437f,
+ 0.082725f,
+ 0.093387f,
+ 0.093388f,
+ 0.095537f,
+ 0.094713f,
+ 0.093700f,
+ 0.092488f,
+ 0.098988f,
+ 0.097225f,
+ 0.095450f,
+ 0.089813f,
+ 0.082825f,
+ 0.075225f,
+ 0.067675f,
+ 0.055787f,
+ 0.042925f,
+ 0.027287f,
+ 0.008050f,
+ -0.008725f,
+ -0.029712f,
+ -0.046450f,
+ -0.066787f,
+ -0.085337f,
+ -0.106137f,
+ -0.122488f,
+ -0.138212f,
+ -0.152013f,
+ -0.166063f,
+ -0.174412f,
+ -0.182100f,
+ -0.186800f,
+ -0.193225f,
+ -0.195700f,
+ -0.199112f,
+ -0.202075f,
+ -0.204363f,
+ -0.201075f,
+ -0.202300f,
+ -0.198900f,
+ -0.199625f,
+ -0.189287f,
+ -0.154800f,
+ -0.140088f,
+ -0.121388f,
+ -0.121438f,
+ -0.099450f,
+ -0.090275f,
+ -0.090938f,
+ -0.076000f,
+ -0.051950f,
+ -0.035850f,
+ -0.005587f,
+ 0.001900f,
+ 0.001263f,
+ 0.000263f,
+ 0.004600f,
+ 0.003487f,
+ 0.005013f,
+ 0.003325f,
+ 0.001587f,
+ -0.000687f,
+ -0.003950f,
+ -0.007675f,
+ -0.011225f,
+ -0.016912f,
+ -0.021625f,
+ -0.026088f,
+ -0.034425f,
+ -0.039475f,
+ -0.047012f,
+ -0.052100f,
+ -0.056488f,
+ -0.061825f,
+ -0.066187f,
+ -0.069775f,
+ -0.072862f,
+ -0.075025f,
+ -0.077150f,
+ -0.078963f,
+ -0.080587f,
+ -0.079225f,
+ -0.079563f,
+ -0.061700f,
+ -0.047800f,
+ -0.029513f,
+ -0.014063f,
+ -0.001800f,
+ -0.000688f,
+ 0.033037f,
+ 0.050812f,
+ 0.072775f,
+ 0.079725f,
+ 0.098238f,
+ 0.125113f,
+ 0.140263f,
+ 0.148400f,
+ 0.177525f,
+ 0.186750f,
+ 0.212800f,
+ 0.232737f,
+ 0.233075f,
+ 0.232575f,
+ 0.231687f,
+ 0.230262f,
+ 0.231563f,
+ 0.229538f,
+ 0.226575f,
+ 0.222437f,
+ 0.219188f,
+ 0.210638f,
+ 0.204063f,
+ 0.191300f,
+ 0.174425f,
+ 0.157862f,
+ 0.140088f,
+ 0.121700f,
+ 0.102163f,
+ 0.080450f,
+ 0.064613f,
+ 0.044737f,
+ 0.023037f,
+ 0.006525f,
+ -0.013575f,
+ -0.029563f,
+ -0.041750f,
+ -0.051112f,
+ -0.059325f,
+ -0.065188f,
+ -0.068837f,
+ -0.074200f,
+ -0.076050f,
+ -0.077925f,
+ -0.080325f,
+ -0.082263f,
+ -0.084050f,
+ -0.081638f,
+ -0.082762f,
+ -0.079338f,
+ -0.074625f,
+ -0.051850f,
+ -0.032313f,
+ -0.023350f,
+ -0.004000f,
+ -0.000813f,
+ 0.018113f,
+ 0.023000f,
+ 0.029138f,
+ 0.036663f,
+ 0.063213f,
+ 0.080213f,
+ 0.079475f,
+ 0.083937f,
+ 0.080738f,
+ 0.078775f,
+ 0.076675f,
+ 0.074038f,
+ 0.071038f,
+ 0.066175f,
+ 0.063562f,
+ 0.061000f,
+ 0.054487f,
+ 0.051287f,
+ 0.042675f,
+ 0.035787f,
+ 0.023750f,
+ 0.015000f,
+ 0.003325f,
+ -0.007275f,
+ -0.015387f,
+ -0.022000f,
+ -0.027650f,
+ -0.033312f,
+ -0.038712f,
+ -0.041313f,
+ -0.044338f,
+ -0.046750f,
+ -0.048700f,
+ -0.050750f,
+ -0.044525f,
+ -0.045988f,
+ -0.047425f,
+ -0.048412f,
+ -0.043750f,
+ -0.013875f,
+ -0.006463f,
+ 0.023662f,
+ 0.029788f,
+ 0.029213f,
+ 0.036150f,
+ 0.072737f,
+ 0.114838f,
+ 0.121875f,
+ 0.121262f,
+ 0.136550f,
+ 0.136000f,
+ 0.161687f,
+ 0.176138f,
+ 0.189462f,
+ 0.203175f,
+ 0.202125f,
+ 0.200700f,
+ 0.200925f,
+ 0.198600f,
+ 0.196275f,
+ 0.192925f,
+ 0.189675f,
+ 0.184463f,
+ 0.176425f,
+ 0.171925f,
+ 0.157600f,
+ 0.147738f,
+ 0.132038f,
+ 0.121762f,
+ 0.102563f,
+ 0.089875f,
+ 0.074962f,
+ 0.056525f,
+ 0.045975f,
+ 0.030050f,
+ 0.018488f,
+ 0.006763f,
+ 0.000412f,
+ -0.006663f,
+ -0.011038f,
+ -0.016600f,
+ -0.020463f,
+ -0.022750f,
+ -0.018350f,
+ -0.020437f,
+ -0.022363f,
+ -0.023750f,
+ -0.021138f,
+ -0.011175f,
+ 0.008463f,
+ 0.016275f,
+ 0.018663f,
+ 0.030888f,
+ 0.057112f,
+ 0.072813f,
+ 0.098937f,
+ 0.098625f,
+ 0.110062f,
+ 0.120450f,
+ 0.124863f,
+ 0.158075f,
+ 0.174750f,
+ 0.192425f,
+ 0.191588f,
+ 0.190075f,
+ 0.187800f,
+ 0.185475f,
+ 0.182975f,
+ 0.180788f,
+ 0.178125f,
+ 0.174312f,
+ 0.167237f,
+ 0.164225f,
+ 0.153512f,
+ 0.144025f,
+ 0.128862f,
+ 0.115825f,
+ 0.099613f,
+ 0.083500f,
+ 0.067888f,
+ 0.050450f,
+ 0.035087f,
+ 0.019650f,
+ 0.002350f,
+ -0.010450f,
+ -0.024850f,
+ -0.032662f,
+ -0.039637f,
+ -0.045400f,
+ -0.051650f,
+ -0.054325f,
+ -0.056438f,
+ -0.058600f,
+ -0.060750f,
+ -0.062887f,
+ -0.065038f,
+ -0.067000f,
+ -0.068488f,
+ -0.069650f,
+ -0.064513f,
+ -0.017200f,
+ 0.006775f,
+ 0.020487f,
+ 0.019225f,
+ 0.023488f,
+ 0.046962f,
+ 0.054788f,
+ 0.059300f,
+ 0.084175f,
+ 0.092287f,
+ 0.118262f,
+ 0.136662f,
+ 0.138813f,
+ 0.136550f,
+ 0.134950f,
+ 0.141637f,
+ 0.139563f,
+ 0.136525f,
+ 0.133113f,
+ 0.129725f,
+ 0.126687f,
+ 0.121375f,
+ 0.117100f,
+ 0.107187f,
+ 0.101688f,
+ 0.093413f,
+ 0.078363f,
+ 0.069425f,
+ 0.055688f,
+ 0.044225f,
+ 0.028625f,
+ 0.020200f,
+ 0.011188f,
+ 0.002813f,
+ -0.003375f,
+ -0.008463f,
+ -0.015050f,
+ -0.019188f,
+ -0.022775f,
+ -0.025062f,
+ -0.018350f,
+ -0.020063f,
+ -0.021425f,
+ -0.017513f,
+ -0.018125f,
+ -0.010225f,
+ -0.004325f,
+ 0.023425f,
+ 0.036163f,
+ 0.046950f,
+ 0.060050f,
+ 0.070350f,
+ 0.099275f,
+ 0.115138f,
+ 0.129988f,
+ 0.143175f,
+ 0.150838f,
+ 0.184525f,
+ 0.184575f,
+ 0.216062f,
+ 0.221425f,
+ 0.224500f,
+ 0.223413f,
+ 0.222275f,
+ 0.220312f,
+ 0.218487f,
+ 0.216062f,
+ 0.213913f,
+ 0.210287f,
+ 0.202875f,
+ 0.199200f,
+ 0.190713f,
+ 0.182188f,
+ 0.167950f,
+ 0.155375f,
+ 0.137213f,
+ 0.121762f,
+ 0.105263f,
+ 0.088688f,
+ 0.072300f,
+ 0.055337f,
+ 0.039837f,
+ 0.024125f,
+ 0.009388f,
+ -0.000500f,
+ -0.014412f,
+ -0.022262f,
+ -0.027613f,
+ -0.035575f,
+ -0.038112f,
+ -0.040375f,
+ -0.042363f,
+ -0.044512f,
+ -0.041813f,
+ -0.043025f,
+ -0.044163f,
+ -0.041062f,
+ -0.034412f,
+ -0.014275f,
+ 0.000012f,
+ 0.005525f,
+ 0.010688f,
+ 0.031000f,
+ 0.042688f,
+ 0.073125f,
+ 0.072937f,
+ 0.099725f,
+ 0.102700f,
+ 0.112787f,
+ 0.146363f,
+ 0.161625f,
+ 0.164212f,
+ 0.163800f,
+ 0.166475f,
+ 0.164925f,
+ 0.163288f,
+ 0.161187f,
+ 0.159125f,
+ 0.159675f,
+ 0.157300f,
+ 0.155175f,
+ 0.149850f,
+ 0.143613f,
+ 0.139025f,
+ 0.134013f,
+ 0.125250f,
+ 0.116400f,
+ 0.105912f,
+ 0.096587f,
+ 0.083300f,
+ 0.075938f,
+ 0.068475f,
+ 0.061650f,
+ 0.057813f,
+ 0.053725f,
+ 0.047625f,
+ 0.043575f,
+ 0.040925f,
+ 0.038600f,
+ 0.036337f,
+ 0.034425f,
+ 0.037738f,
+ 0.042400f,
+ 0.042875f,
+ 0.058275f,
+ 0.060125f,
+ 0.103038f,
+ 0.110363f,
+ 0.115300f,
+ 0.134862f,
+ 0.134900f,
+ 0.159350f,
+ 0.180437f,
+ 0.200050f,
+ 0.203575f,
+ 0.208737f,
+ 0.249450f,
+ 0.260750f,
+ 0.262925f,
+ 0.268087f,
+ 0.266800f,
+ 0.265100f,
+ 0.263175f,
+ 0.260988f,
+ 0.258300f,
+ 0.253775f,
+ 0.246387f,
+ 0.243275f,
+ 0.230887f,
+ 0.215525f,
+ 0.197950f,
+ 0.181237f,
+ 0.160200f,
+ 0.139775f,
+ 0.118975f,
+ 0.095188f,
+ 0.075350f,
+ 0.052950f,
+ 0.028250f,
+ 0.005338f,
+ -0.016325f,
+ -0.039600f,
+ -0.059150f,
+ -0.080138f,
+ -0.096725f,
+ -0.114687f,
+ -0.128388f,
+ -0.137050f,
+ -0.143900f,
+ -0.148650f,
+ -0.153088f,
+ -0.156762f,
+ -0.158988f,
+ -0.160900f,
+ -0.157575f,
+ -0.158950f,
+ -0.160100f,
+ -0.160963f,
+ -0.161362f,
+ -0.131887f,
+ -0.118062f,
+ -0.113050f,
+ -0.095688f,
+ -0.090425f,
+ -0.067463f,
+ -0.064038f,
+ -0.059925f,
+ -0.031637f,
+ -0.020388f,
+ -0.016300f,
+ -0.017262f,
+ -0.015462f,
+ -0.011475f,
+ -0.012987f,
+ -0.014487f,
+ -0.016350f,
+ -0.019450f,
+ -0.022912f,
+ -0.026238f,
+ -0.030000f,
+ -0.032987f,
+ -0.039638f,
+ -0.046350f,
+ -0.053213f,
+ -0.061763f,
+ -0.070675f,
+ -0.080213f,
+ -0.091487f,
+ -0.097625f,
+ -0.105812f,
+ -0.109213f,
+ -0.114425f,
+ -0.119025f,
+ -0.122312f,
+ -0.125062f,
+ -0.127488f,
+ -0.129888f,
+ -0.132537f,
+ -0.135212f,
+ -0.136800f,
+ -0.132712f,
+ -0.114687f,
+ -0.069275f,
+ -0.059112f,
+ -0.058838f,
+ -0.044100f,
+ -0.025938f,
+ -0.018613f,
+ 0.007450f,
+ 0.021038f,
+ 0.025988f,
+ 0.083925f,
+ 0.084562f,
+ 0.097950f,
+ 0.115175f,
+ 0.129750f,
+ 0.136862f,
+ 0.163925f,
+ 0.176900f,
+ 0.181975f,
+ 0.186225f,
+ 0.185175f,
+ 0.200425f,
+ 0.198412f,
+ 0.195738f,
+ 0.192887f,
+ 0.190038f,
+ 0.185062f,
+ 0.176275f,
+ 0.171687f,
+ 0.158488f,
+ 0.145875f,
+ 0.132925f,
+ 0.120012f,
+ 0.107663f,
+ 0.090800f,
+ 0.083587f,
+ 0.061800f,
+ 0.050263f,
+ 0.034325f,
+ 0.022875f,
+ 0.012637f,
+ 0.001262f,
+ -0.003725f,
+ -0.009513f,
+ -0.013037f,
+ -0.016463f,
+ -0.019312f,
+ -0.021200f,
+ -0.022713f,
+ -0.024113f,
+ -0.025150f,
+ -0.025975f,
+ -0.020212f,
+ -0.008600f,
+ -0.008175f,
+ 0.045013f,
+ 0.048537f,
+ 0.052350f,
+ 0.058800f,
+ 0.074213f,
+ 0.081850f,
+ 0.089987f,
+ 0.112763f,
+ 0.137238f,
+ 0.139925f,
+ 0.147663f,
+ 0.153250f,
+ 0.151563f,
+ 0.149250f,
+ 0.153425f,
+ 0.150625f,
+ 0.146962f,
+ 0.143238f,
+ 0.139437f,
+ 0.129700f,
+ 0.120913f,
+ 0.111312f,
+ 0.096125f,
+ 0.080138f,
+ 0.064750f,
+ 0.045450f,
+ 0.024738f,
+ 0.005800f,
+ -0.012950f,
+ -0.033088f,
+ -0.053312f,
+ -0.072325f,
+ -0.092187f,
+ -0.110075f,
+ -0.126625f,
+ -0.143575f,
+ -0.155463f,
+ -0.163938f,
+ -0.171850f,
+ -0.176312f,
+ -0.180200f,
+ -0.185600f,
+ -0.188150f,
+ -0.190450f,
+ -0.192637f,
+ -0.194725f,
+ -0.196350f,
+ -0.197825f,
+ -0.192238f,
+ -0.185450f,
+ -0.173137f,
+ -0.127525f,
+ -0.117425f,
+ -0.107262f,
+ -0.095425f,
+ -0.092562f,
+ -0.088850f,
+ -0.076025f,
+ -0.067812f,
+ -0.018400f,
+ 0.004163f,
+ 0.003400f,
+ 0.002600f,
+ 0.001375f,
+ 0.000338f,
+ 0.010775f,
+ 0.008925f,
+ 0.006625f,
+ 0.004413f,
+ 0.002337f,
+ 0.000038f,
+ -0.004512f,
+ -0.008838f,
+ -0.011550f,
+ -0.017475f,
+ -0.025875f,
+ -0.032712f,
+ -0.044713f,
+ -0.052850f,
+ -0.062775f,
+ -0.074038f,
+ -0.079375f,
+ -0.086413f,
+ -0.094488f,
+ -0.096963f,
+ -0.100112f,
+ -0.102950f,
+ -0.105450f,
+ -0.107088f,
+ -0.108525f,
+ -0.109625f,
+ -0.110775f,
+ -0.109063f,
+ -0.109563f,
+ -0.103688f,
+ -0.089012f,
+ -0.083550f,
+ -0.049250f,
+ -0.046087f,
+ -0.034612f,
+ -0.013375f,
+ -0.006125f,
+ 0.011387f,
+ 0.046050f,
+ 0.057000f,
+ 0.061863f,
+ 0.079800f,
+ 0.087550f,
+ 0.136538f,
+ 0.136625f,
+ 0.155313f,
+ 0.154575f,
+ 0.157150f,
+ 0.155738f,
+ 0.154175f,
+ 0.156938f,
+ 0.155287f,
+ 0.153300f,
+ 0.149425f,
+ 0.143537f,
+ 0.139375f,
+ 0.131738f,
+ 0.123087f,
+ 0.109700f,
+ 0.097713f,
+ 0.079038f,
+ 0.066412f,
+ 0.049200f,
+ 0.030525f,
+ 0.015125f,
+ -0.004813f,
+ -0.020037f,
+ -0.036187f,
+ -0.051925f,
+ -0.064075f,
+ -0.078537f,
+ -0.086975f,
+ -0.094537f,
+ -0.101650f,
+ -0.107513f,
+ -0.111175f,
+ -0.114513f,
+ -0.117425f,
+ -0.119650f,
+ -0.121525f,
+ -0.112625f,
+ -0.113513f,
+ -0.101988f,
+ -0.099850f,
+ -0.090175f,
+ -0.072737f,
+ -0.066037f,
+ -0.050200f,
+ -0.040138f,
+ -0.026650f,
+ -0.022425f,
+ 0.004013f,
+ 0.003850f,
+ 0.039737f,
+ 0.043313f,
+ 0.045250f,
+ 0.056850f,
+ 0.055550f,
+ 0.053763f,
+ 0.051700f,
+ 0.049287f,
+ 0.047025f,
+ 0.044087f,
+ 0.042000f,
+ 0.037225f,
+ 0.032175f,
+ 0.027463f,
+ 0.019963f,
+ 0.014612f,
+ 0.007550f,
+ -0.005275f,
+ -0.016713f,
+ -0.025475f,
+ -0.039850f,
+ -0.049088f,
+ -0.057587f,
+ -0.064887f,
+ -0.068100f,
+ -0.073963f,
+ -0.076975f,
+ -0.080125f,
+ -0.082387f,
+ -0.084563f,
+ -0.086138f,
+ -0.087438f,
+ -0.082563f,
+ -0.082775f,
+ -0.081013f,
+ -0.066875f,
+ -0.047413f,
+ -0.034025f,
+ -0.017025f,
+ -0.010100f,
+ -0.007262f,
+ 0.013900f,
+ 0.061188f,
+ 0.066925f,
+ 0.069087f,
+ 0.080075f,
+ 0.083837f,
+ 0.131162f,
+ 0.158263f,
+ 0.165212f,
+ 0.166687f,
+ 0.165125f,
+ 0.163250f,
+ 0.161025f,
+ 0.158863f,
+ 0.156537f,
+ 0.154375f,
+ 0.151113f,
+ 0.143775f,
+ 0.139937f,
+ 0.129500f,
+ 0.119700f,
+ 0.102600f,
+ 0.085450f,
+ 0.069050f,
+ 0.048700f,
+ 0.029112f,
+ 0.009163f,
+ -0.008150f,
+ -0.027475f,
+ -0.047100f,
+ -0.068700f,
+ -0.082475f,
+ -0.102462f,
+ -0.113900f,
+ -0.125312f,
+ -0.133812f,
+ -0.138500f,
+ -0.145613f,
+ -0.149400f,
+ -0.152712f,
+ -0.154688f,
+ -0.156525f,
+ -0.158163f,
+ -0.154463f,
+ -0.156025f,
+ -0.154700f,
+ -0.151437f,
+ -0.118212f,
+ -0.097475f,
+ -0.084588f,
+ -0.084762f,
+ -0.066162f,
+ -0.036838f,
+ -0.034000f,
+ -0.005913f,
+ 0.014775f,
+ 0.027100f,
+ 0.031738f,
+ 0.045937f,
+ 0.078675f,
+ 0.091587f,
+ 0.092475f,
+ 0.093200f,
+ 0.092175f,
+ 0.101337f,
+ 0.100062f,
+ 0.100387f,
+ 0.098500f,
+ 0.096225f,
+ 0.094350f,
+ 0.091987f,
+ 0.088112f,
+ 0.082962f,
+ 0.078613f,
+ 0.075412f,
+ 0.070825f,
+ 0.067725f,
+ 0.064825f,
+ 0.061775f,
+ 0.058237f,
+ 0.055162f,
+ 0.051812f,
+ 0.049713f,
+ 0.047875f,
+ 0.046188f,
+ 0.050287f,
+ 0.049362f,
+ 0.048925f,
+ 0.051962f,
+ 0.060012f,
+ 0.082500f,
+ 0.095175f,
+ 0.099963f,
+ 0.107975f,
+ 0.108400f,
+ 0.167925f,
+ 0.181687f,
+ 0.184638f,
+ 0.197625f,
+ 0.206987f,
+ 0.252200f,
+ 0.270950f,
+ 0.276888f,
+ 0.292000f,
+ 0.299625f,
+ 0.339000f,
+ 0.359450f,
+ 0.359425f,
+ 0.362475f,
+ 0.365912f,
+ 0.364663f,
+ 0.362900f,
+ 0.360262f,
+ 0.357913f,
+ 0.354112f,
+ 0.350513f,
+ 0.343600f,
+ 0.336562f,
+ 0.326425f,
+ 0.318225f,
+ 0.305538f,
+ 0.284250f,
+ 0.274000f,
+ 0.249038f,
+ 0.234050f,
+ 0.215700f,
+ 0.204187f,
+ 0.184800f,
+ 0.165238f,
+ 0.150413f,
+ 0.138900f,
+ 0.125437f,
+ 0.119075f,
+ 0.110050f,
+ 0.106763f,
+ 0.101050f,
+ 0.098037f,
+ 0.095412f,
+ 0.097737f,
+ 0.095975f,
+ 0.094425f,
+ 0.096875f,
+ 0.102287f,
+ 0.101525f,
+ 0.106862f,
+ 0.122450f,
+ 0.134000f,
+ 0.159637f,
+ 0.184362f,
+ 0.195150f,
+ 0.200075f,
+ 0.199550f,
+ 0.229087f,
+ 0.230088f,
+ 0.230637f,
+ 0.229400f,
+ 0.232800f,
+ 0.231325f,
+ 0.231725f,
+ 0.229463f,
+ 0.226900f,
+ 0.222438f,
+ 0.219350f,
+ 0.210725f,
+ 0.205587f,
+ 0.197750f,
+ 0.192213f,
+ 0.181925f,
+ 0.168900f,
+ 0.150113f,
+ 0.138662f,
+ 0.123075f,
+ 0.097338f,
+ 0.087312f,
+ 0.062512f,
+ 0.048587f,
+ 0.034588f,
+ 0.014875f,
+ 0.000638f,
+ -0.011213f,
+ -0.022638f,
+ -0.029513f,
+ -0.034750f,
+ -0.040025f,
+ -0.044163f,
+ -0.047138f,
+ -0.050100f,
+ -0.052600f,
+ -0.054600f,
+ -0.056013f,
+ -0.054087f,
+ -0.048488f,
+ -0.042362f,
+ -0.024250f,
+ 0.001600f,
+ 0.011725f,
+ 0.022537f,
+ 0.040050f,
+ 0.051862f,
+ 0.063113f,
+ 0.075700f,
+ 0.084200f,
+ 0.100562f,
+ 0.130488f,
+ 0.147713f,
+ 0.148475f,
+ 0.157512f,
+ 0.161175f,
+ 0.160100f,
+ 0.158675f,
+ 0.157037f,
+ 0.154938f,
+ 0.152963f,
+ 0.148050f,
+ 0.144575f,
+ 0.139588f,
+ 0.132400f,
+ 0.124475f,
+ 0.113888f,
+ 0.100175f,
+ 0.092237f,
+ 0.078988f,
+ 0.065763f,
+ 0.052600f,
+ 0.037575f,
+ 0.026888f,
+ 0.015087f,
+ 0.002125f,
+ -0.003938f,
+ -0.008188f,
+ -0.015200f,
+ -0.019975f,
+ -0.024562f,
+ -0.027237f,
+ -0.029875f,
+ -0.023762f,
+ -0.025887f,
+ -0.027537f,
+ -0.028562f,
+ -0.026150f,
+ 0.000675f,
+ 0.015212f,
+ 0.031075f,
+ 0.044213f,
+ 0.046575f,
+ 0.058150f,
+ 0.072325f,
+ 0.084112f,
+ 0.116425f,
+ 0.119425f,
+ 0.134263f,
+ 0.161775f,
+ 0.164688f,
+ 0.164150f,
+ 0.164125f,
+ 0.162625f,
+ 0.160737f,
+ 0.158300f,
+ 0.155350f,
+ 0.150800f,
+ 0.144687f,
+ 0.136175f,
+ 0.127100f,
+ 0.111775f,
+ 0.095613f,
+ 0.077687f,
+ 0.059125f,
+ 0.038837f,
+ 0.016837f,
+ -0.006000f,
+ -0.028800f,
+ -0.048088f,
+ -0.069888f,
+ -0.092025f,
+ -0.109012f,
+ -0.131113f,
+ -0.147413f,
+ -0.165062f,
+ -0.177937f,
+ -0.189513f,
+ -0.200650f,
+ -0.206837f,
+ -0.212188f,
+ -0.215713f,
+ -0.217975f,
+ -0.220138f,
+ -0.221962f,
+ -0.223538f,
+ -0.224963f,
+ -0.226038f,
+ -0.227125f,
+ -0.208425f,
+ -0.208812f,
+ -0.192362f,
+ -0.168000f,
+ -0.160275f,
+ -0.127475f,
+ -0.123963f,
+ -0.121900f,
+ -0.114875f,
+ -0.088975f,
+ -0.061513f,
+ -0.050650f,
+ -0.050725f,
+ -0.049513f,
+ -0.050350f,
+ -0.051713f,
+ -0.053063f,
+ -0.054937f,
+ -0.058225f,
+ -0.062125f,
+ -0.067262f,
+ -0.073212f,
+ -0.080538f,
+ -0.088737f,
+ -0.102025f,
+ -0.111775f,
+ -0.127037f,
+ -0.140463f,
+ -0.154250f,
+ -0.165587f,
+ -0.180100f,
+ -0.189550f,
+ -0.198237f,
+ -0.206662f,
+ -0.211675f,
+ -0.214675f,
+ -0.217175f,
+ -0.219337f,
+ -0.221463f,
+ -0.222962f,
+ -0.223875f,
+ -0.203913f,
+ -0.187225f,
+ -0.177487f,
+ -0.164138f,
+ -0.154775f,
+ -0.127562f,
+ -0.108088f,
+ -0.078275f,
+ -0.068013f,
+ -0.042975f,
+ -0.034138f,
+ 0.009063f,
+ 0.042650f,
+ 0.057725f,
+ 0.064750f,
+ 0.063650f,
+ 0.068750f,
+ 0.071000f,
+ 0.069213f,
+ 0.071712f,
+ 0.065487f,
+ 0.059387f,
+ 0.050763f,
+ 0.040837f,
+ 0.029288f,
+ 0.018363f,
+ 0.002488f,
+ -0.015600f,
+ -0.039250f,
+ -0.063975f,
+ -0.094375f,
+ -0.126625f,
+ -0.164513f,
+ -0.196662f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+ -0.204700f,
+};
\ No newline at end of file
diff --git a/motors/pistol_grip/vtable_wheel.cc b/motors/pistol_grip/vtable_wheel.cc
new file mode 100644
index 0000000..92e9a19
--- /dev/null
+++ b/motors/pistol_grip/vtable_wheel.cc
@@ -0,0 +1,4099 @@
+extern const float kWheelCoggingTorque[4096];
+const float kWheelCoggingTorque[4096] = {
+ -0.161690f,
+ -0.114243f,
+ -0.106662f,
+ -0.094305f,
+ -0.064914f,
+ -0.034757f,
+ -0.009276f,
+ 0.010967f,
+ 0.018600f,
+ 0.043138f,
+ 0.048890f,
+ 0.084971f,
+ 0.102648f,
+ 0.114095f,
+ 0.122886f,
+ 0.125129f,
+ 0.122771f,
+ 0.118814f,
+ 0.114886f,
+ 0.110552f,
+ 0.103629f,
+ 0.094795f,
+ 0.082367f,
+ 0.068667f,
+ 0.051762f,
+ 0.033810f,
+ 0.011300f,
+ -0.010986f,
+ -0.034119f,
+ -0.058129f,
+ -0.082548f,
+ -0.106038f,
+ -0.128033f,
+ -0.149010f,
+ -0.167462f,
+ -0.185514f,
+ -0.201052f,
+ -0.211529f,
+ -0.219871f,
+ -0.225371f,
+ -0.229190f,
+ -0.232024f,
+ -0.235014f,
+ -0.237467f,
+ -0.238757f,
+ -0.237262f,
+ -0.236433f,
+ -0.212386f,
+ -0.160690f,
+ -0.149586f,
+ -0.136405f,
+ -0.121952f,
+ -0.106262f,
+ -0.052519f,
+ -0.032238f,
+ -0.023529f,
+ -0.000567f,
+ 0.025505f,
+ 0.048629f,
+ 0.063214f,
+ 0.069181f,
+ 0.109219f,
+ 0.131810f,
+ 0.139257f,
+ 0.138143f,
+ 0.136671f,
+ 0.136186f,
+ 0.137005f,
+ 0.134481f,
+ 0.131638f,
+ 0.127505f,
+ 0.121157f,
+ 0.114652f,
+ 0.106505f,
+ 0.096352f,
+ 0.083719f,
+ 0.069329f,
+ 0.055233f,
+ 0.038224f,
+ 0.019833f,
+ 0.005795f,
+ -0.008938f,
+ -0.020905f,
+ -0.029890f,
+ -0.036833f,
+ -0.043724f,
+ -0.049038f,
+ -0.054338f,
+ -0.057752f,
+ -0.059090f,
+ -0.061419f,
+ -0.063519f,
+ -0.065448f,
+ -0.061957f,
+ -0.055381f,
+ -0.036890f,
+ 0.000819f,
+ 0.028614f,
+ 0.038629f,
+ 0.060614f,
+ 0.083195f,
+ 0.093471f,
+ 0.113538f,
+ 0.167614f,
+ 0.187786f,
+ 0.186662f,
+ 0.206600f,
+ 0.216424f,
+ 0.258176f,
+ 0.286700f,
+ 0.290033f,
+ 0.288048f,
+ 0.286048f,
+ 0.285438f,
+ 0.282600f,
+ 0.279110f,
+ 0.275219f,
+ 0.269876f,
+ 0.265100f,
+ 0.265925f,
+ 0.255825f,
+ 0.243540f,
+ 0.224185f,
+ 0.205470f,
+ 0.183840f,
+ 0.160945f,
+ 0.140590f,
+ 0.120535f,
+ 0.097710f,
+ 0.077745f,
+ 0.055660f,
+ 0.035945f,
+ 0.016470f,
+ 0.002795f,
+ -0.009790f,
+ -0.018015f,
+ -0.025320f,
+ -0.031615f,
+ -0.036055f,
+ -0.040195f,
+ -0.044010f,
+ -0.047135f,
+ -0.049850f,
+ -0.051955f,
+ -0.049250f,
+ -0.040795f,
+ -0.031195f,
+ -0.002290f,
+ 0.033150f,
+ 0.042645f,
+ 0.051995f,
+ 0.050870f,
+ 0.074275f,
+ 0.092585f,
+ 0.127065f,
+ 0.140405f,
+ 0.149595f,
+ 0.151265f,
+ 0.151855f,
+ 0.149955f,
+ 0.148045f,
+ 0.145600f,
+ 0.141665f,
+ 0.139005f,
+ 0.133630f,
+ 0.128790f,
+ 0.120410f,
+ 0.113860f,
+ 0.102245f,
+ 0.085750f,
+ 0.066745f,
+ 0.048765f,
+ 0.028665f,
+ 0.009620f,
+ -0.015955f,
+ -0.034935f,
+ -0.062150f,
+ -0.080825f,
+ -0.100465f,
+ -0.119375f,
+ -0.135110f,
+ -0.144415f,
+ -0.157920f,
+ -0.165860f,
+ -0.170825f,
+ -0.177525f,
+ -0.180885f,
+ -0.182940f,
+ -0.183785f,
+ -0.177330f,
+ -0.174020f,
+ -0.174980f,
+ -0.154140f,
+ -0.133910f,
+ -0.119465f,
+ -0.085645f,
+ -0.078300f,
+ -0.055005f,
+ -0.004490f,
+ 0.003335f,
+ 0.032490f,
+ 0.041960f,
+ 0.074680f,
+ 0.099220f,
+ 0.122725f,
+ 0.143985f,
+ 0.152455f,
+ 0.155215f,
+ 0.155750f,
+ 0.153865f,
+ 0.152395f,
+ 0.149025f,
+ 0.144465f,
+ 0.136965f,
+ 0.131980f,
+ 0.121990f,
+ 0.109325f,
+ 0.092865f,
+ 0.076295f,
+ 0.052365f,
+ 0.025975f,
+ 0.000455f,
+ -0.022820f,
+ -0.048995f,
+ -0.077175f,
+ -0.104570f,
+ -0.130715f,
+ -0.153295f,
+ -0.179195f,
+ -0.199070f,
+ -0.219630f,
+ -0.235055f,
+ -0.246095f,
+ -0.256785f,
+ -0.263350f,
+ -0.268475f,
+ -0.272900f,
+ -0.275370f,
+ -0.276145f,
+ -0.278065f,
+ -0.279995f,
+ -0.278880f,
+ -0.276215f,
+ -0.250900f,
+ -0.225590f,
+ -0.204910f,
+ -0.183375f,
+ -0.163775f,
+ -0.158825f,
+ -0.134700f,
+ -0.101920f,
+ -0.084435f,
+ -0.077705f,
+ -0.056910f,
+ -0.024100f,
+ -0.022545f,
+ -0.017925f,
+ -0.015300f,
+ -0.014320f,
+ -0.015475f,
+ -0.017700f,
+ -0.020125f,
+ -0.023405f,
+ -0.027925f,
+ -0.033500f,
+ -0.040375f,
+ -0.049435f,
+ -0.060735f,
+ -0.073780f,
+ -0.090535f,
+ -0.109335f,
+ -0.127635f,
+ -0.146470f,
+ -0.164470f,
+ -0.184485f,
+ -0.201540f,
+ -0.219745f,
+ -0.233830f,
+ -0.246575f,
+ -0.255500f,
+ -0.261600f,
+ -0.268060f,
+ -0.273135f,
+ -0.276510f,
+ -0.279545f,
+ -0.280050f,
+ -0.282640f,
+ -0.284870f,
+ -0.281530f,
+ -0.269085f,
+ -0.247215f,
+ -0.208655f,
+ -0.193935f,
+ -0.189310f,
+ -0.173375f,
+ -0.152370f,
+ -0.111690f,
+ -0.083280f,
+ -0.064710f,
+ -0.045010f,
+ -0.038390f,
+ -0.024820f,
+ -0.000470f,
+ 0.045605f,
+ 0.049470f,
+ 0.049020f,
+ 0.047000f,
+ 0.050110f,
+ 0.047855f,
+ 0.045440f,
+ 0.041975f,
+ 0.037555f,
+ 0.032220f,
+ 0.023955f,
+ 0.015705f,
+ 0.003005f,
+ -0.013735f,
+ -0.031830f,
+ -0.051050f,
+ -0.071245f,
+ -0.091830f,
+ -0.111305f,
+ -0.131160f,
+ -0.152080f,
+ -0.170000f,
+ -0.188180f,
+ -0.203070f,
+ -0.215710f,
+ -0.225730f,
+ -0.233805f,
+ -0.238745f,
+ -0.243385f,
+ -0.247210f,
+ -0.250245f,
+ -0.250765f,
+ -0.243575f,
+ -0.242185f,
+ -0.235715f,
+ -0.222790f,
+ -0.177250f,
+ -0.176185f,
+ -0.156810f,
+ -0.137785f,
+ -0.121240f,
+ -0.087445f,
+ -0.057715f,
+ -0.042785f,
+ -0.001100f,
+ -0.000310f,
+ 0.023030f,
+ 0.028305f,
+ 0.060015f,
+ 0.098340f,
+ 0.102530f,
+ 0.107000f,
+ 0.106095f,
+ 0.102935f,
+ 0.100020f,
+ 0.095950f,
+ 0.091595f,
+ 0.086150f,
+ 0.078975f,
+ 0.069455f,
+ 0.057685f,
+ 0.037740f,
+ 0.021450f,
+ 0.001010f,
+ -0.020570f,
+ -0.044500f,
+ -0.067835f,
+ -0.090730f,
+ -0.113195f,
+ -0.134415f,
+ -0.156905f,
+ -0.175610f,
+ -0.193585f,
+ -0.208050f,
+ -0.218975f,
+ -0.226725f,
+ -0.233700f,
+ -0.238435f,
+ -0.241835f,
+ -0.243025f,
+ -0.245345f,
+ -0.243140f,
+ -0.245070f,
+ -0.239815f,
+ -0.235060f,
+ -0.188275f,
+ -0.157120f,
+ -0.135190f,
+ -0.136800f,
+ -0.132250f,
+ -0.087350f,
+ -0.047035f,
+ -0.019560f,
+ -0.014280f,
+ -0.007735f,
+ 0.002470f,
+ 0.027835f,
+ 0.070310f,
+ 0.092845f,
+ 0.095520f,
+ 0.093665f,
+ 0.091425f,
+ 0.089220f,
+ 0.086385f,
+ 0.083410f,
+ 0.079145f,
+ 0.073820f,
+ 0.068050f,
+ 0.059990f,
+ 0.049040f,
+ 0.034530f,
+ 0.020050f,
+ 0.002645f,
+ -0.014185f,
+ -0.030685f,
+ -0.049300f,
+ -0.066905f,
+ -0.082670f,
+ -0.097515f,
+ -0.109690f,
+ -0.118895f,
+ -0.126460f,
+ -0.131915f,
+ -0.135785f,
+ -0.138465f,
+ -0.139445f,
+ -0.141725f,
+ -0.143725f,
+ -0.145560f,
+ -0.143735f,
+ -0.142385f,
+ -0.120830f,
+ -0.077690f,
+ -0.061320f,
+ -0.048525f,
+ -0.049425f,
+ -0.010670f,
+ 0.019640f,
+ 0.034160f,
+ 0.068205f,
+ 0.094310f,
+ 0.106605f,
+ 0.127555f,
+ 0.147180f,
+ 0.154780f,
+ 0.202710f,
+ 0.222790f,
+ 0.224470f,
+ 0.221945f,
+ 0.223605f,
+ 0.221530f,
+ 0.219360f,
+ 0.216180f,
+ 0.213070f,
+ 0.207630f,
+ 0.201295f,
+ 0.193160f,
+ 0.183085f,
+ 0.169020f,
+ 0.152800f,
+ 0.134540f,
+ 0.114160f,
+ 0.094685f,
+ 0.075295f,
+ 0.056195f,
+ 0.037555f,
+ 0.019855f,
+ 0.004505f,
+ -0.012150f,
+ -0.022100f,
+ -0.030225f,
+ -0.035955f,
+ -0.041530f,
+ -0.045130f,
+ -0.048035f,
+ -0.048295f,
+ -0.048115f,
+ -0.042695f,
+ -0.041460f,
+ -0.038205f,
+ -0.015325f,
+ 0.016410f,
+ 0.041105f,
+ 0.048055f,
+ 0.057445f,
+ 0.099065f,
+ 0.113350f,
+ 0.143890f,
+ 0.165120f,
+ 0.179440f,
+ 0.204595f,
+ 0.214905f,
+ 0.246815f,
+ 0.264480f,
+ 0.280615f,
+ 0.279395f,
+ 0.282745f,
+ 0.284765f,
+ 0.282645f,
+ 0.279700f,
+ 0.275515f,
+ 0.271185f,
+ 0.264315f,
+ 0.256945f,
+ 0.246300f,
+ 0.234270f,
+ 0.216680f,
+ 0.197270f,
+ 0.176170f,
+ 0.155855f,
+ 0.134845f,
+ 0.112730f,
+ 0.092345f,
+ 0.071780f,
+ 0.051775f,
+ 0.035485f,
+ 0.018435f,
+ 0.007000f,
+ -0.001675f,
+ -0.008560f,
+ -0.012585f,
+ -0.016980f,
+ -0.019755f,
+ -0.022625f,
+ -0.025725f,
+ -0.028270f,
+ -0.024175f,
+ -0.011980f,
+ 0.023655f,
+ 0.063455f,
+ 0.076430f,
+ 0.085270f,
+ 0.091545f,
+ 0.111295f,
+ 0.165320f,
+ 0.186525f,
+ 0.208805f,
+ 0.214880f,
+ 0.223635f,
+ 0.261790f,
+ 0.287290f,
+ 0.304585f,
+ 0.307115f,
+ 0.314380f,
+ 0.316585f,
+ 0.312190f,
+ 0.311815f,
+ 0.306700f,
+ 0.299575f,
+ 0.289810f,
+ 0.278190f,
+ 0.261475f,
+ 0.243555f,
+ 0.222570f,
+ 0.196865f,
+ 0.170880f,
+ 0.143255f,
+ 0.116330f,
+ 0.087710f,
+ 0.058820f,
+ 0.030350f,
+ 0.002720f,
+ -0.024360f,
+ -0.050485f,
+ -0.074875f,
+ -0.096360f,
+ -0.115165f,
+ -0.131750f,
+ -0.142655f,
+ -0.152965f,
+ -0.160030f,
+ -0.164845f,
+ -0.168795f,
+ -0.168595f,
+ -0.171090f,
+ -0.170560f,
+ -0.169110f,
+ -0.163075f,
+ -0.147875f,
+ -0.106410f,
+ -0.085265f,
+ -0.079855f,
+ -0.051605f,
+ -0.043605f,
+ -0.021620f,
+ 0.003540f,
+ 0.031835f,
+ 0.042510f,
+ 0.060810f,
+ 0.066380f,
+ 0.097230f,
+ 0.104695f,
+ 0.106915f,
+ 0.109445f,
+ 0.109720f,
+ 0.113085f,
+ 0.110230f,
+ 0.109855f,
+ 0.105090f,
+ 0.099375f,
+ 0.091460f,
+ 0.082415f,
+ 0.069365f,
+ 0.055495f,
+ 0.040050f,
+ 0.022605f,
+ 0.006240f,
+ -0.011530f,
+ -0.028485f,
+ -0.044750f,
+ -0.059245f,
+ -0.073815f,
+ -0.084595f,
+ -0.093530f,
+ -0.101065f,
+ -0.106710f,
+ -0.111440f,
+ -0.114990f,
+ -0.117595f,
+ -0.114370f,
+ -0.116300f,
+ -0.118740f,
+ -0.118675f,
+ -0.109710f,
+ -0.083340f,
+ -0.052150f,
+ -0.032275f,
+ -0.024800f,
+ -0.011130f,
+ -0.000025f,
+ 0.041245f,
+ 0.083210f,
+ 0.096405f,
+ 0.107030f,
+ 0.121365f,
+ 0.136335f,
+ 0.173510f,
+ 0.197195f,
+ 0.225325f,
+ 0.226845f,
+ 0.231170f,
+ 0.229290f,
+ 0.226855f,
+ 0.226775f,
+ 0.223795f,
+ 0.219540f,
+ 0.214350f,
+ 0.207040f,
+ 0.197025f,
+ 0.185800f,
+ 0.170920f,
+ 0.151360f,
+ 0.131020f,
+ 0.108885f,
+ 0.087220f,
+ 0.063855f,
+ 0.040565f,
+ 0.016735f,
+ -0.003735f,
+ -0.022810f,
+ -0.042660f,
+ -0.061950f,
+ -0.079775f,
+ -0.093480f,
+ -0.104530f,
+ -0.112060f,
+ -0.118530f,
+ -0.123095f,
+ -0.126820f,
+ -0.127770f,
+ -0.125490f,
+ -0.127950f,
+ -0.129820f,
+ -0.113595f,
+ -0.096200f,
+ -0.048355f,
+ -0.049525f,
+ -0.039325f,
+ -0.021305f,
+ -0.006695f,
+ 0.024220f,
+ 0.050140f,
+ 0.055810f,
+ 0.079645f,
+ 0.100310f,
+ 0.121355f,
+ 0.119890f,
+ 0.117030f,
+ 0.115120f,
+ 0.113070f,
+ 0.110655f,
+ 0.107295f,
+ 0.101205f,
+ 0.096050f,
+ 0.087330f,
+ 0.077515f,
+ 0.061100f,
+ 0.041105f,
+ 0.018785f,
+ -0.006035f,
+ -0.033085f,
+ -0.060180f,
+ -0.088620f,
+ -0.114895f,
+ -0.142945f,
+ -0.168875f,
+ -0.195390f,
+ -0.220700f,
+ -0.245685f,
+ -0.267545f,
+ -0.286180f,
+ -0.302375f,
+ -0.313470f,
+ -0.320805f,
+ -0.327160f,
+ -0.331285f,
+ -0.335015f,
+ -0.338355f,
+ -0.342140f,
+ -0.344900f,
+ -0.340800f,
+ -0.331520f,
+ -0.279400f,
+ -0.250280f,
+ -0.237615f,
+ -0.236930f,
+ -0.236625f,
+ -0.209050f,
+ -0.142935f,
+ -0.123825f,
+ -0.119085f,
+ -0.118350f,
+ -0.113565f,
+ -0.076695f,
+ -0.044160f,
+ -0.041435f,
+ -0.034970f,
+ -0.026710f,
+ -0.023750f,
+ -0.027575f,
+ -0.031105f,
+ -0.035495f,
+ -0.041020f,
+ -0.046755f,
+ -0.056595f,
+ -0.068155f,
+ -0.082065f,
+ -0.096785f,
+ -0.113350f,
+ -0.132070f,
+ -0.149280f,
+ -0.166970f,
+ -0.183470f,
+ -0.201250f,
+ -0.215540f,
+ -0.227695f,
+ -0.237240f,
+ -0.245550f,
+ -0.251345f,
+ -0.255975f,
+ -0.259715f,
+ -0.262350f,
+ -0.260645f,
+ -0.262820f,
+ -0.264545f,
+ -0.262520f,
+ -0.245250f,
+ -0.213035f,
+ -0.175700f,
+ -0.158465f,
+ -0.150145f,
+ -0.125895f,
+ -0.091195f,
+ -0.071710f,
+ -0.052160f,
+ -0.024950f,
+ 0.010800f,
+ 0.042900f,
+ 0.063160f,
+ 0.081260f,
+ 0.097740f,
+ 0.105305f,
+ 0.149360f,
+ 0.167775f,
+ 0.172690f,
+ 0.178720f,
+ 0.174985f,
+ 0.171965f,
+ 0.170940f,
+ 0.167510f,
+ 0.162365f,
+ 0.156470f,
+ 0.147915f,
+ 0.136835f,
+ 0.122785f,
+ 0.106655f,
+ 0.088995f,
+ 0.070195f,
+ 0.049760f,
+ 0.029995f,
+ 0.010770f,
+ -0.008895f,
+ -0.027785f,
+ -0.044715f,
+ -0.060240f,
+ -0.074545f,
+ -0.084100f,
+ -0.091050f,
+ -0.096660f,
+ -0.100930f,
+ -0.104850f,
+ -0.107865f,
+ -0.111535f,
+ -0.114125f,
+ -0.111220f,
+ -0.104515f,
+ -0.087315f,
+ -0.057745f,
+ -0.032395f,
+ -0.021535f,
+ -0.009800f,
+ -0.006155f,
+ 0.036965f,
+ 0.069795f,
+ 0.084715f,
+ 0.092970f,
+ 0.094360f,
+ 0.115880f,
+ 0.164110f,
+ 0.175995f,
+ 0.178130f,
+ 0.178750f,
+ 0.184790f,
+ 0.184055f,
+ 0.179155f,
+ 0.174335f,
+ 0.168730f,
+ 0.162270f,
+ 0.153040f,
+ 0.138700f,
+ 0.122365f,
+ 0.104905f,
+ 0.083970f,
+ 0.062975f,
+ 0.038470f,
+ 0.015590f,
+ -0.009290f,
+ -0.032340f,
+ -0.056390f,
+ -0.077025f,
+ -0.099385f,
+ -0.118040f,
+ -0.136340f,
+ -0.150155f,
+ -0.162015f,
+ -0.171330f,
+ -0.177510f,
+ -0.182715f,
+ -0.186465f,
+ -0.188330f,
+ -0.185235f,
+ -0.178890f,
+ -0.181030f,
+ -0.158750f,
+ -0.124930f,
+ -0.106900f,
+ -0.085515f,
+ -0.084385f,
+ -0.059405f,
+ -0.030605f,
+ 0.015570f,
+ 0.029660f,
+ 0.046600f,
+ 0.057920f,
+ 0.066905f,
+ 0.106255f,
+ 0.130620f,
+ 0.151935f,
+ 0.150145f,
+ 0.152640f,
+ 0.152040f,
+ 0.149595f,
+ 0.146395f,
+ 0.141735f,
+ 0.136405f,
+ 0.128725f,
+ 0.119090f,
+ 0.104905f,
+ 0.087775f,
+ 0.068370f,
+ 0.046530f,
+ 0.023550f,
+ 0.000335f,
+ -0.024055f,
+ -0.048540f,
+ -0.074230f,
+ -0.099780f,
+ -0.122385f,
+ -0.144050f,
+ -0.166250f,
+ -0.186800f,
+ -0.203255f,
+ -0.213675f,
+ -0.221780f,
+ -0.229280f,
+ -0.233630f,
+ -0.237140f,
+ -0.239690f,
+ -0.242180f,
+ -0.244365f,
+ -0.246395f,
+ -0.244460f,
+ -0.238350f,
+ -0.197170f,
+ -0.165165f,
+ -0.156470f,
+ -0.145305f,
+ -0.126860f,
+ -0.109040f,
+ -0.067880f,
+ -0.044700f,
+ -0.023520f,
+ -0.019740f,
+ -0.002965f,
+ 0.013445f,
+ 0.043260f,
+ 0.069815f,
+ 0.079825f,
+ 0.085480f,
+ 0.083600f,
+ 0.081305f,
+ 0.078805f,
+ 0.076105f,
+ 0.073215f,
+ 0.068845f,
+ 0.062345f,
+ 0.056570f,
+ 0.047355f,
+ 0.034745f,
+ 0.022280f,
+ 0.006070f,
+ -0.011685f,
+ -0.027850f,
+ -0.045825f,
+ -0.062445f,
+ -0.079345f,
+ -0.093945f,
+ -0.107665f,
+ -0.118285f,
+ -0.127820f,
+ -0.133500f,
+ -0.139505f,
+ -0.144415f,
+ -0.147850f,
+ -0.150515f,
+ -0.153330f,
+ -0.155600f,
+ -0.153475f,
+ -0.152725f,
+ -0.142280f,
+ -0.114250f,
+ -0.096775f,
+ -0.074845f,
+ -0.071225f,
+ -0.064940f,
+ -0.014125f,
+ 0.003875f,
+ 0.029380f,
+ 0.054825f,
+ 0.064745f,
+ 0.068685f,
+ 0.083680f,
+ 0.109000f,
+ 0.145455f,
+ 0.159965f,
+ 0.162125f,
+ 0.158940f,
+ 0.155940f,
+ 0.152930f,
+ 0.149320f,
+ 0.146180f,
+ 0.140100f,
+ 0.133345f,
+ 0.122210f,
+ 0.108600f,
+ 0.091705f,
+ 0.070625f,
+ 0.048665f,
+ 0.026750f,
+ 0.002855f,
+ -0.022215f,
+ -0.047245f,
+ -0.071705f,
+ -0.096455f,
+ -0.120005f,
+ -0.141910f,
+ -0.163525f,
+ -0.182205f,
+ -0.197495f,
+ -0.209240f,
+ -0.217705f,
+ -0.224525f,
+ -0.228530f,
+ -0.230460f,
+ -0.232295f,
+ -0.234515f,
+ -0.233215f,
+ -0.235705f,
+ -0.237205f,
+ -0.216730f,
+ -0.182135f,
+ -0.162485f,
+ -0.157445f,
+ -0.147615f,
+ -0.117750f,
+ -0.089270f,
+ -0.066295f,
+ -0.048840f,
+ -0.035895f,
+ -0.034240f,
+ -0.000085f,
+ 0.021555f,
+ 0.027005f,
+ 0.034465f,
+ 0.032625f,
+ 0.031315f,
+ 0.028610f,
+ 0.024600f,
+ 0.021095f,
+ 0.014395f,
+ 0.005755f,
+ -0.005330f,
+ -0.018345f,
+ -0.035870f,
+ -0.055460f,
+ -0.077585f,
+ -0.102350f,
+ -0.128105f,
+ -0.150300f,
+ -0.174450f,
+ -0.198485f,
+ -0.222815f,
+ -0.245745f,
+ -0.266690f,
+ -0.285065f,
+ -0.299760f,
+ -0.309385f,
+ -0.317410f,
+ -0.323560f,
+ -0.328010f,
+ -0.331160f,
+ -0.331410f,
+ -0.333345f,
+ -0.331875f,
+ -0.333470f,
+ -0.320960f,
+ -0.294220f,
+ -0.283005f,
+ -0.242110f,
+ -0.230000f,
+ -0.216115f,
+ -0.179720f,
+ -0.155820f,
+ -0.120405f,
+ -0.093875f,
+ -0.073800f,
+ -0.044225f,
+ -0.020645f,
+ -0.006630f,
+ 0.008140f,
+ 0.036025f,
+ 0.065815f,
+ 0.079635f,
+ 0.085575f,
+ 0.083305f,
+ 0.083800f,
+ 0.081375f,
+ 0.078230f,
+ 0.074625f,
+ 0.069575f,
+ 0.063520f,
+ 0.056875f,
+ 0.048580f,
+ 0.036960f,
+ 0.024230f,
+ 0.009910f,
+ -0.004245f,
+ -0.019445f,
+ -0.033165f,
+ -0.046115f,
+ -0.059310f,
+ -0.068670f,
+ -0.075745f,
+ -0.083545f,
+ -0.087965f,
+ -0.092040f,
+ -0.095440f,
+ -0.098700f,
+ -0.101505f,
+ -0.100240f,
+ -0.091190f,
+ -0.085630f,
+ -0.086485f,
+ -0.059620f,
+ -0.030065f,
+ -0.011150f,
+ 0.007220f,
+ 0.018015f,
+ 0.064330f,
+ 0.076155f,
+ 0.093085f,
+ 0.121510f,
+ 0.166455f,
+ 0.183135f,
+ 0.199245f,
+ 0.212325f,
+ 0.217605f,
+ 0.263355f,
+ 0.278485f,
+ 0.302175f,
+ 0.305325f,
+ 0.306735f,
+ 0.305085f,
+ 0.302760f,
+ 0.302365f,
+ 0.299720f,
+ 0.295625f,
+ 0.289465f,
+ 0.280595f,
+ 0.270715f,
+ 0.258380f,
+ 0.240975f,
+ 0.221040f,
+ 0.208560f,
+ 0.186650f,
+ 0.163535f,
+ 0.142980f,
+ 0.127555f,
+ 0.103035f,
+ 0.084325f,
+ 0.070040f,
+ 0.058115f,
+ 0.046680f,
+ 0.039275f,
+ 0.032175f,
+ 0.026875f,
+ 0.023925f,
+ 0.020225f,
+ 0.018015f,
+ 0.015715f,
+ 0.013845f,
+ 0.013370f,
+ 0.016255f,
+ 0.028840f,
+ 0.073540f,
+ 0.080680f,
+ 0.098205f,
+ 0.123050f,
+ 0.132570f,
+ 0.144750f,
+ 0.175580f,
+ 0.187760f,
+ 0.224230f,
+ 0.237660f,
+ 0.248855f,
+ 0.272260f,
+ 0.284055f,
+ 0.283685f,
+ 0.287760f,
+ 0.289245f,
+ 0.290965f,
+ 0.291975f,
+ 0.287995f,
+ 0.283300f,
+ 0.276695f,
+ 0.270275f,
+ 0.263725f,
+ 0.250170f,
+ 0.236500f,
+ 0.221260f,
+ 0.202740f,
+ 0.184355f,
+ 0.163430f,
+ 0.142490f,
+ 0.124290f,
+ 0.104825f,
+ 0.084440f,
+ 0.067995f,
+ 0.051180f,
+ 0.041995f,
+ 0.031830f,
+ 0.024200f,
+ 0.015095f,
+ 0.011220f,
+ 0.007065f,
+ 0.004110f,
+ 0.001770f,
+ 0.002690f,
+ 0.002955f,
+ 0.007645f,
+ 0.019435f,
+ 0.053435f,
+ 0.082880f,
+ 0.086875f,
+ 0.094500f,
+ 0.117755f,
+ 0.143130f,
+ 0.184200f,
+ 0.218790f,
+ 0.224215f,
+ 0.234230f,
+ 0.268895f,
+ 0.278770f,
+ 0.312030f,
+ 0.326495f,
+ 0.337885f,
+ 0.340165f,
+ 0.338965f,
+ 0.338435f,
+ 0.335830f,
+ 0.331970f,
+ 0.327205f,
+ 0.321095f,
+ 0.312975f,
+ 0.304480f,
+ 0.292475f,
+ 0.274175f,
+ 0.253450f,
+ 0.230500f,
+ 0.204815f,
+ 0.177920f,
+ 0.152390f,
+ 0.126185f,
+ 0.099215f,
+ 0.072895f,
+ 0.044995f,
+ 0.020935f,
+ -0.003510f,
+ -0.026720f,
+ -0.045910f,
+ -0.063945f,
+ -0.077350f,
+ -0.088150f,
+ -0.097245f,
+ -0.102965f,
+ -0.107715f,
+ -0.111295f,
+ -0.113785f,
+ -0.114320f,
+ -0.116100f,
+ -0.113220f,
+ -0.110125f,
+ -0.095770f,
+ -0.071440f,
+ -0.047050f,
+ -0.032085f,
+ -0.017250f,
+ -0.003090f,
+ 0.014530f,
+ 0.013005f,
+ 0.051095f,
+ 0.050305f,
+ 0.093015f,
+ 0.111935f,
+ 0.112690f,
+ 0.111080f,
+ 0.109070f,
+ 0.107140f,
+ 0.105045f,
+ 0.102045f,
+ 0.098055f,
+ 0.093040f,
+ 0.086475f,
+ 0.079710f,
+ 0.069575f,
+ 0.057240f,
+ 0.041310f,
+ 0.023915f,
+ 0.003510f,
+ -0.017415f,
+ -0.036215f,
+ -0.059400f,
+ -0.079160f,
+ -0.101870f,
+ -0.122595f,
+ -0.142370f,
+ -0.160375f,
+ -0.175555f,
+ -0.188360f,
+ -0.198545f,
+ -0.205615f,
+ -0.211650f,
+ -0.215670f,
+ -0.218520f,
+ -0.221325f,
+ -0.221000f,
+ -0.222975f,
+ -0.219800f,
+ -0.217870f,
+ -0.217390f,
+ -0.196680f,
+ -0.180055f,
+ -0.168845f,
+ -0.138010f,
+ -0.129685f,
+ -0.095315f,
+ -0.072410f,
+ -0.064400f,
+ -0.041920f,
+ -0.032450f,
+ -0.009600f,
+ 0.011850f,
+ 0.033175f,
+ 0.040415f,
+ 0.040860f,
+ 0.041420f,
+ 0.039445f,
+ 0.037155f,
+ 0.034225f,
+ 0.031390f,
+ 0.026535f,
+ 0.020090f,
+ 0.010520f,
+ -0.002045f,
+ -0.018130f,
+ -0.037520f,
+ -0.059430f,
+ -0.082090f,
+ -0.105400f,
+ -0.130330f,
+ -0.156130f,
+ -0.181195f,
+ -0.206070f,
+ -0.230855f,
+ -0.254090f,
+ -0.275865f,
+ -0.295820f,
+ -0.313765f,
+ -0.328455f,
+ -0.338915f,
+ -0.347595f,
+ -0.351380f,
+ -0.356855f,
+ -0.360720f,
+ -0.363105f,
+ -0.355795f,
+ -0.357475f,
+ -0.354170f,
+ -0.350235f,
+ -0.304980f,
+ -0.289430f,
+ -0.279780f,
+ -0.270830f,
+ -0.263235f,
+ -0.216440f,
+ -0.183240f,
+ -0.165085f,
+ -0.155245f,
+ -0.144025f,
+ -0.107555f,
+ -0.093250f,
+ -0.065440f,
+ -0.036865f,
+ -0.018870f,
+ -0.016520f,
+ -0.018810f,
+ -0.021850f,
+ -0.024590f,
+ -0.028505f,
+ -0.033175f,
+ -0.039595f,
+ -0.048830f,
+ -0.060285f,
+ -0.074955f,
+ -0.092635f,
+ -0.113685f,
+ -0.135000f,
+ -0.157570f,
+ -0.181085f,
+ -0.205460f,
+ -0.228700f,
+ -0.250050f,
+ -0.273690f,
+ -0.294165f,
+ -0.314645f,
+ -0.335205f,
+ -0.351705f,
+ -0.368290f,
+ -0.378855f,
+ -0.387240f,
+ -0.394880f,
+ -0.399795f,
+ -0.404605f,
+ -0.407535f,
+ -0.410480f,
+ -0.412200f,
+ -0.410485f,
+ -0.400100f,
+ -0.382070f,
+ -0.352555f,
+ -0.328120f,
+ -0.325295f,
+ -0.299072f,
+ -0.248738f,
+ -0.218213f,
+ -0.183925f,
+ -0.172625f,
+ -0.149487f,
+ -0.138950f,
+ -0.114562f,
+ -0.084131f,
+ -0.071869f,
+ -0.042269f,
+ -0.030062f,
+ -0.021125f,
+ -0.022600f,
+ -0.024250f,
+ -0.026262f,
+ -0.028419f,
+ -0.031088f,
+ -0.034150f,
+ -0.037994f,
+ -0.043700f,
+ -0.050681f,
+ -0.059125f,
+ -0.069837f,
+ -0.083131f,
+ -0.097338f,
+ -0.114425f,
+ -0.130725f,
+ -0.148087f,
+ -0.163863f,
+ -0.179962f,
+ -0.193344f,
+ -0.204819f,
+ -0.214281f,
+ -0.221269f,
+ -0.226500f,
+ -0.231131f,
+ -0.234925f,
+ -0.237581f,
+ -0.240950f,
+ -0.241275f,
+ -0.242731f,
+ -0.231331f,
+ -0.214063f,
+ -0.199656f,
+ -0.188450f,
+ -0.170031f,
+ -0.151688f,
+ -0.136181f,
+ -0.110281f,
+ -0.076400f,
+ -0.060125f,
+ -0.036350f,
+ -0.015887f,
+ 0.005681f,
+ 0.034537f,
+ 0.066194f,
+ 0.071119f,
+ 0.081912f,
+ 0.108575f,
+ 0.138869f,
+ 0.151900f,
+ 0.155388f,
+ 0.161519f,
+ 0.162837f,
+ 0.163606f,
+ 0.161062f,
+ 0.157150f,
+ 0.153625f,
+ 0.148556f,
+ 0.142913f,
+ 0.134550f,
+ 0.124044f,
+ 0.112388f,
+ 0.097581f,
+ 0.083400f,
+ 0.065363f,
+ 0.048694f,
+ 0.032475f,
+ 0.015219f,
+ 0.001025f,
+ -0.014763f,
+ -0.029581f,
+ -0.038237f,
+ -0.046512f,
+ -0.052419f,
+ -0.056431f,
+ -0.060100f,
+ -0.062506f,
+ -0.064669f,
+ -0.066469f,
+ -0.067981f,
+ -0.061675f,
+ -0.057238f,
+ -0.048075f,
+ -0.020612f,
+ -0.002781f,
+ 0.018431f,
+ 0.025913f,
+ 0.041194f,
+ 0.061444f,
+ 0.087263f,
+ 0.123994f,
+ 0.147969f,
+ 0.155694f,
+ 0.180344f,
+ 0.188444f,
+ 0.193325f,
+ 0.235162f,
+ 0.255794f,
+ 0.268313f,
+ 0.275044f,
+ 0.273575f,
+ 0.273094f,
+ 0.271000f,
+ 0.268706f,
+ 0.265906f,
+ 0.262319f,
+ 0.257219f,
+ 0.248694f,
+ 0.242231f,
+ 0.229150f,
+ 0.213700f,
+ 0.195944f,
+ 0.178806f,
+ 0.157719f,
+ 0.139156f,
+ 0.118875f,
+ 0.099169f,
+ 0.079206f,
+ 0.060713f,
+ 0.041794f,
+ 0.026094f,
+ 0.014563f,
+ 0.002675f,
+ -0.004194f,
+ -0.010725f,
+ -0.015850f,
+ -0.018581f,
+ -0.021006f,
+ -0.023181f,
+ -0.023719f,
+ -0.025450f,
+ -0.026900f,
+ -0.015056f,
+ 0.006062f,
+ 0.052012f,
+ 0.065094f,
+ 0.072319f,
+ 0.081969f,
+ 0.106344f,
+ 0.117269f,
+ 0.168737f,
+ 0.189944f,
+ 0.195425f,
+ 0.198425f,
+ 0.228775f,
+ 0.250406f,
+ 0.274844f,
+ 0.277494f,
+ 0.281469f,
+ 0.279537f,
+ 0.277250f,
+ 0.274075f,
+ 0.270750f,
+ 0.266738f,
+ 0.259619f,
+ 0.251556f,
+ 0.238981f,
+ 0.224906f,
+ 0.204831f,
+ 0.184306f,
+ 0.161469f,
+ 0.136181f,
+ 0.109519f,
+ 0.083012f,
+ 0.056306f,
+ 0.028619f,
+ 0.001975f,
+ -0.025394f,
+ -0.050219f,
+ -0.076194f,
+ -0.099562f,
+ -0.122456f,
+ -0.142437f,
+ -0.163188f,
+ -0.178013f,
+ -0.188106f,
+ -0.195288f,
+ -0.199950f,
+ -0.206581f,
+ -0.210144f,
+ -0.212594f,
+ -0.214869f,
+ -0.216831f,
+ -0.215950f,
+ -0.205931f,
+ -0.195706f,
+ -0.159894f,
+ -0.148988f,
+ -0.138688f,
+ -0.134300f,
+ -0.112225f,
+ -0.098881f,
+ -0.071681f,
+ -0.033037f,
+ -0.033881f,
+ -0.032119f,
+ -0.007700f,
+ 0.017750f,
+ 0.026225f,
+ 0.028281f,
+ 0.028500f,
+ 0.026919f,
+ 0.031869f,
+ 0.029769f,
+ 0.027125f,
+ 0.024287f,
+ 0.019906f,
+ 0.014287f,
+ 0.007231f,
+ -0.001138f,
+ -0.011600f,
+ -0.025494f,
+ -0.041431f,
+ -0.057806f,
+ -0.075112f,
+ -0.089975f,
+ -0.107556f,
+ -0.123581f,
+ -0.139137f,
+ -0.153425f,
+ -0.165625f,
+ -0.174638f,
+ -0.182600f,
+ -0.187619f,
+ -0.193288f,
+ -0.196044f,
+ -0.199269f,
+ -0.201769f,
+ -0.204356f,
+ -0.206544f,
+ -0.208075f,
+ -0.202431f,
+ -0.186738f,
+ -0.165769f,
+ -0.145288f,
+ -0.135244f,
+ -0.122119f,
+ -0.086969f,
+ -0.065894f,
+ -0.051675f,
+ -0.035500f,
+ 0.002575f,
+ 0.029875f,
+ 0.059269f,
+ 0.070562f,
+ 0.079931f,
+ 0.095944f,
+ 0.119719f,
+ 0.153069f,
+ 0.173906f,
+ 0.172725f,
+ 0.172769f,
+ 0.170775f,
+ 0.170975f,
+ 0.168556f,
+ 0.165200f,
+ 0.161562f,
+ 0.156975f,
+ 0.148625f,
+ 0.137444f,
+ 0.125013f,
+ 0.110294f,
+ 0.091237f,
+ 0.072062f,
+ 0.052931f,
+ 0.031263f,
+ 0.010256f,
+ -0.010131f,
+ -0.029775f,
+ -0.049300f,
+ -0.066375f,
+ -0.085694f,
+ -0.103406f,
+ -0.117400f,
+ -0.126481f,
+ -0.134138f,
+ -0.139700f,
+ -0.143500f,
+ -0.146719f,
+ -0.149206f,
+ -0.151237f,
+ -0.153119f,
+ -0.154856f,
+ -0.156244f,
+ -0.154506f,
+ -0.144444f,
+ -0.099675f,
+ -0.078119f,
+ -0.064400f,
+ -0.063269f,
+ -0.054844f,
+ -0.018225f,
+ 0.011325f,
+ 0.028625f,
+ 0.055306f,
+ 0.064738f,
+ 0.072306f,
+ 0.100750f,
+ 0.118056f,
+ 0.137806f,
+ 0.137037f,
+ 0.137462f,
+ 0.135856f,
+ 0.134044f,
+ 0.131481f,
+ 0.128000f,
+ 0.123275f,
+ 0.117469f,
+ 0.110619f,
+ 0.100694f,
+ 0.085900f,
+ 0.068831f,
+ 0.047762f,
+ 0.026800f,
+ 0.004981f,
+ -0.019313f,
+ -0.043019f,
+ -0.064906f,
+ -0.088762f,
+ -0.112931f,
+ -0.134231f,
+ -0.156106f,
+ -0.175450f,
+ -0.194256f,
+ -0.208106f,
+ -0.220500f,
+ -0.227913f,
+ -0.233519f,
+ -0.239063f,
+ -0.242294f,
+ -0.244988f,
+ -0.247269f,
+ -0.249156f,
+ -0.245763f,
+ -0.241731f,
+ -0.222406f,
+ -0.188537f,
+ -0.180931f,
+ -0.169181f,
+ -0.151256f,
+ -0.110538f,
+ -0.097419f,
+ -0.070337f,
+ -0.058987f,
+ -0.028131f,
+ 0.003700f,
+ 0.016306f,
+ 0.027306f,
+ 0.036831f,
+ 0.063369f,
+ 0.098944f,
+ 0.101369f,
+ 0.110600f,
+ 0.112206f,
+ 0.110375f,
+ 0.108225f,
+ 0.105769f,
+ 0.102819f,
+ 0.098631f,
+ 0.095213f,
+ 0.089356f,
+ 0.082881f,
+ 0.074556f,
+ 0.065256f,
+ 0.052275f,
+ 0.039194f,
+ 0.025256f,
+ 0.011181f,
+ -0.003587f,
+ -0.016387f,
+ -0.027987f,
+ -0.038113f,
+ -0.046331f,
+ -0.052519f,
+ -0.056950f,
+ -0.060194f,
+ -0.063088f,
+ -0.065263f,
+ -0.067256f,
+ -0.066644f,
+ -0.068006f,
+ -0.064400f,
+ -0.057206f,
+ -0.037881f,
+ -0.025519f,
+ 0.004413f,
+ 0.020500f,
+ 0.031325f,
+ 0.059006f,
+ 0.080212f,
+ 0.092775f,
+ 0.119569f,
+ 0.151444f,
+ 0.176931f,
+ 0.191488f,
+ 0.217394f,
+ 0.235488f,
+ 0.256944f,
+ 0.281500f,
+ 0.302756f,
+ 0.321981f,
+ 0.339206f,
+ 0.351787f,
+ 0.350788f,
+ 0.353737f,
+ 0.351813f,
+ 0.349794f,
+ 0.346869f,
+ 0.343744f,
+ 0.338200f,
+ 0.333119f,
+ 0.325994f,
+ 0.315050f,
+ 0.300856f,
+ 0.284950f,
+ 0.268700f,
+ 0.249669f,
+ 0.232494f,
+ 0.213700f,
+ 0.195394f,
+ 0.177681f,
+ 0.158337f,
+ 0.143369f,
+ 0.127263f,
+ 0.113350f,
+ 0.103200f,
+ 0.095294f,
+ 0.088737f,
+ 0.084725f,
+ 0.081187f,
+ 0.078600f,
+ 0.075831f,
+ 0.073700f,
+ 0.076731f,
+ 0.079313f,
+ 0.084663f,
+ 0.096031f,
+ 0.113656f,
+ 0.148625f,
+ 0.150556f,
+ 0.169844f,
+ 0.184531f,
+ 0.198187f,
+ 0.226919f,
+ 0.228600f,
+ 0.248663f,
+ 0.266519f,
+ 0.290119f,
+ 0.308900f,
+ 0.314188f,
+ 0.323312f,
+ 0.321625f,
+ 0.319594f,
+ 0.317206f,
+ 0.314013f,
+ 0.310256f,
+ 0.305700f,
+ 0.298813f,
+ 0.290319f,
+ 0.278619f,
+ 0.261987f,
+ 0.246194f,
+ 0.225644f,
+ 0.206063f,
+ 0.182894f,
+ 0.158025f,
+ 0.134406f,
+ 0.109450f,
+ 0.085825f,
+ 0.060050f,
+ 0.035675f,
+ 0.014069f,
+ -0.007975f,
+ -0.028906f,
+ -0.045881f,
+ -0.062644f,
+ -0.074856f,
+ -0.083038f,
+ -0.089356f,
+ -0.094431f,
+ -0.097963f,
+ -0.100581f,
+ -0.098813f,
+ -0.100544f,
+ -0.094025f,
+ -0.093981f,
+ -0.087544f,
+ -0.073444f,
+ -0.028175f,
+ -0.022963f,
+ -0.016000f,
+ -0.003769f,
+ 0.028313f,
+ 0.053925f,
+ 0.069681f,
+ 0.081306f,
+ 0.104531f,
+ 0.126412f,
+ 0.149144f,
+ 0.148388f,
+ 0.156244f,
+ 0.154781f,
+ 0.156731f,
+ 0.154550f,
+ 0.151400f,
+ 0.147594f,
+ 0.143494f,
+ 0.136175f,
+ 0.127869f,
+ 0.115700f,
+ 0.100725f,
+ 0.082094f,
+ 0.061131f,
+ 0.037425f,
+ 0.012350f,
+ -0.014094f,
+ -0.041494f,
+ -0.068075f,
+ -0.095350f,
+ -0.122594f,
+ -0.150044f,
+ -0.175881f,
+ -0.201856f,
+ -0.226119f,
+ -0.248681f,
+ -0.267637f,
+ -0.282569f,
+ -0.296031f,
+ -0.306675f,
+ -0.312762f,
+ -0.318656f,
+ -0.322200f,
+ -0.324813f,
+ -0.327237f,
+ -0.329606f,
+ -0.331650f,
+ -0.333363f,
+ -0.323869f,
+ -0.311487f,
+ -0.264863f,
+ -0.239912f,
+ -0.229525f,
+ -0.224350f,
+ -0.209400f,
+ -0.173469f,
+ -0.155775f,
+ -0.138800f,
+ -0.114544f,
+ -0.108450f,
+ -0.088963f,
+ -0.045175f,
+ -0.040219f,
+ -0.025612f,
+ -0.024119f,
+ -0.021388f,
+ -0.023144f,
+ -0.025219f,
+ -0.027556f,
+ -0.030731f,
+ -0.034750f,
+ -0.038400f,
+ -0.043544f,
+ -0.050713f,
+ -0.058819f,
+ -0.071131f,
+ -0.082756f,
+ -0.097812f,
+ -0.113831f,
+ -0.131119f,
+ -0.146131f,
+ -0.163756f,
+ -0.177125f,
+ -0.194069f,
+ -0.209287f,
+ -0.220244f,
+ -0.229600f,
+ -0.236706f,
+ -0.241944f,
+ -0.248306f,
+ -0.251825f,
+ -0.255825f,
+ -0.258081f,
+ -0.261700f,
+ -0.261731f,
+ -0.258400f,
+ -0.256975f,
+ -0.238831f,
+ -0.230556f,
+ -0.202162f,
+ -0.185663f,
+ -0.164600f,
+ -0.146487f,
+ -0.137181f,
+ -0.121606f,
+ -0.088356f,
+ -0.064756f,
+ -0.023987f,
+ -0.017681f,
+ 0.001612f,
+ 0.007787f,
+ 0.025625f,
+ 0.048606f,
+ 0.066875f,
+ 0.076975f,
+ 0.077169f,
+ 0.075244f,
+ 0.072987f,
+ 0.070037f,
+ 0.066756f,
+ 0.061931f,
+ 0.055863f,
+ 0.048281f,
+ 0.038537f,
+ 0.023200f,
+ 0.004862f,
+ -0.013406f,
+ -0.034594f,
+ -0.056919f,
+ -0.078844f,
+ -0.104119f,
+ -0.128613f,
+ -0.151425f,
+ -0.174906f,
+ -0.197669f,
+ -0.216956f,
+ -0.237531f,
+ -0.253944f,
+ -0.266625f,
+ -0.277881f,
+ -0.288769f,
+ -0.295325f,
+ -0.302300f,
+ -0.305950f,
+ -0.308844f,
+ -0.311413f,
+ -0.306375f,
+ -0.303638f,
+ -0.299531f,
+ -0.298438f,
+ -0.279106f,
+ -0.235675f,
+ -0.219356f,
+ -0.213837f,
+ -0.186863f,
+ -0.164744f,
+ -0.148994f,
+ -0.121000f,
+ -0.105237f,
+ -0.090887f,
+ -0.072087f,
+ -0.043281f,
+ -0.035581f,
+ -0.014250f,
+ -0.009869f,
+ -0.005594f,
+ -0.005638f,
+ -0.007931f,
+ -0.011781f,
+ -0.014513f,
+ -0.018606f,
+ -0.024512f,
+ -0.032162f,
+ -0.042831f,
+ -0.055469f,
+ -0.069269f,
+ -0.086594f,
+ -0.108038f,
+ -0.130919f,
+ -0.150737f,
+ -0.170575f,
+ -0.190750f,
+ -0.211388f,
+ -0.227144f,
+ -0.248938f,
+ -0.264488f,
+ -0.277738f,
+ -0.288613f,
+ -0.294700f,
+ -0.300369f,
+ -0.304950f,
+ -0.309194f,
+ -0.312506f,
+ -0.314706f,
+ -0.317056f,
+ -0.318681f,
+ -0.319912f,
+ -0.299212f,
+ -0.277856f,
+ -0.242531f,
+ -0.234369f,
+ -0.213987f,
+ -0.210981f,
+ -0.181950f,
+ -0.136594f,
+ -0.121844f,
+ -0.102781f,
+ -0.092506f,
+ -0.051881f,
+ -0.018138f,
+ -0.010281f,
+ -0.001581f,
+ 0.011531f,
+ 0.047875f,
+ 0.077544f,
+ 0.079544f,
+ 0.081400f,
+ 0.082869f,
+ 0.081256f,
+ 0.079525f,
+ 0.077369f,
+ 0.073206f,
+ 0.069625f,
+ 0.065194f,
+ 0.058981f,
+ 0.053413f,
+ 0.045150f,
+ 0.038363f,
+ 0.026238f,
+ 0.014519f,
+ 0.000181f,
+ -0.013100f,
+ -0.023656f,
+ -0.034125f,
+ -0.041450f,
+ -0.051412f,
+ -0.055738f,
+ -0.062125f,
+ -0.065213f,
+ -0.070100f,
+ -0.073731f,
+ -0.076131f,
+ -0.077962f,
+ -0.075919f,
+ -0.077419f,
+ -0.077094f,
+ -0.067438f,
+ -0.042500f,
+ -0.004475f,
+ 0.015119f,
+ 0.017525f,
+ 0.021412f,
+ 0.044294f,
+ 0.075156f,
+ 0.123125f,
+ 0.149706f,
+ 0.156581f,
+ 0.182481f,
+ 0.186319f,
+ 0.216937f,
+ 0.237706f,
+ 0.263519f,
+ 0.279694f,
+ 0.291881f,
+ 0.292169f,
+ 0.290381f,
+ 0.288194f,
+ 0.286856f,
+ 0.283112f,
+ 0.278631f,
+ 0.274244f,
+ 0.270069f,
+ 0.260556f,
+ 0.254556f,
+ 0.241663f,
+ 0.228950f,
+ 0.214288f,
+ 0.196288f,
+ 0.177900f,
+ 0.152881f,
+ 0.136881f,
+ 0.113600f,
+ 0.094594f,
+ 0.077044f,
+ 0.058681f,
+ 0.044725f,
+ 0.036944f,
+ 0.024044f,
+ 0.017119f,
+ 0.012544f,
+ 0.008650f,
+ 0.005962f,
+ 0.000956f,
+ -0.001625f,
+ -0.004369f,
+ -0.006650f,
+ -0.008194f,
+ -0.003163f,
+ 0.004106f,
+ 0.014362f,
+ 0.028856f,
+ 0.059138f,
+ 0.080756f,
+ 0.098387f,
+ 0.107594f,
+ 0.121762f,
+ 0.130244f,
+ 0.157181f,
+ 0.181844f,
+ 0.203869f,
+ 0.219081f,
+ 0.223675f,
+ 0.226175f,
+ 0.226288f,
+ 0.224881f,
+ 0.223025f,
+ 0.220406f,
+ 0.217181f,
+ 0.213300f,
+ 0.205775f,
+ 0.199819f,
+ 0.193706f,
+ 0.183525f,
+ 0.172550f,
+ 0.157531f,
+ 0.138437f,
+ 0.118250f,
+ 0.100331f,
+ 0.079556f,
+ 0.060306f,
+ 0.035138f,
+ 0.018125f,
+ -0.001837f,
+ -0.018794f,
+ -0.030175f,
+ -0.044681f,
+ -0.048400f,
+ -0.056456f,
+ -0.063112f,
+ -0.066894f,
+ -0.070450f,
+ -0.073031f,
+ -0.075650f,
+ -0.077250f,
+ -0.071387f,
+ -0.068994f,
+ -0.067931f,
+ -0.023950f,
+ 0.012594f,
+ 0.019087f,
+ 0.037812f,
+ 0.048519f,
+ 0.088194f,
+ 0.117113f,
+ 0.135963f,
+ 0.151631f,
+ 0.170800f,
+ 0.197325f,
+ 0.224094f,
+ 0.260850f,
+ 0.265356f,
+ 0.281537f,
+ 0.284294f,
+ 0.282744f,
+ 0.281675f,
+ 0.280419f,
+ 0.277675f,
+ 0.272312f,
+ 0.266569f,
+ 0.260819f,
+ 0.251394f,
+ 0.241231f,
+ 0.226462f,
+ 0.208944f,
+ 0.186900f,
+ 0.162650f,
+ 0.132194f,
+ 0.110637f,
+ 0.084812f,
+ 0.060250f,
+ 0.034219f,
+ 0.008094f,
+ -0.020737f,
+ -0.041719f,
+ -0.059800f,
+ -0.081106f,
+ -0.100294f,
+ -0.113587f,
+ -0.121450f,
+ -0.129975f,
+ -0.134831f,
+ -0.140563f,
+ -0.143738f,
+ -0.145256f,
+ -0.144975f,
+ -0.141819f,
+ -0.139294f,
+ -0.140144f,
+ -0.129519f,
+ -0.107281f,
+ -0.075906f,
+ -0.052600f,
+ -0.052850f,
+ -0.037075f,
+ -0.007519f,
+ 0.020856f,
+ 0.037713f,
+ 0.042819f,
+ 0.057169f,
+ 0.087781f,
+ 0.110875f,
+ 0.125937f,
+ 0.130431f,
+ 0.132225f,
+ 0.130281f,
+ 0.127925f,
+ 0.125406f,
+ 0.122925f,
+ 0.118906f,
+ 0.113306f,
+ 0.107631f,
+ 0.100581f,
+ 0.092675f,
+ 0.081806f,
+ 0.067038f,
+ 0.049563f,
+ 0.031556f,
+ 0.014100f,
+ -0.005013f,
+ -0.022900f,
+ -0.041956f,
+ -0.058981f,
+ -0.074944f,
+ -0.091194f,
+ -0.101656f,
+ -0.109656f,
+ -0.116888f,
+ -0.123500f,
+ -0.127662f,
+ -0.131363f,
+ -0.134844f,
+ -0.137894f,
+ -0.140738f,
+ -0.142963f,
+ -0.137425f,
+ -0.138644f,
+ -0.122881f,
+ -0.079581f,
+ -0.047306f,
+ -0.032056f,
+ -0.028475f,
+ -0.029169f,
+ -0.003638f,
+ 0.072500f,
+ 0.078881f,
+ 0.083525f,
+ 0.094500f,
+ 0.103725f,
+ 0.136275f,
+ 0.163888f,
+ 0.197013f,
+ 0.196119f,
+ 0.196056f,
+ 0.192987f,
+ 0.192194f,
+ 0.191431f,
+ 0.187387f,
+ 0.182431f,
+ 0.177163f,
+ 0.169488f,
+ 0.159231f,
+ 0.146900f,
+ 0.129619f,
+ 0.112669f,
+ 0.092269f,
+ 0.068531f,
+ 0.049263f,
+ 0.027850f,
+ 0.006244f,
+ -0.014869f,
+ -0.037387f,
+ -0.056187f,
+ -0.076088f,
+ -0.095975f,
+ -0.108800f,
+ -0.117669f,
+ -0.125706f,
+ -0.131769f,
+ -0.135531f,
+ -0.139669f,
+ -0.142388f,
+ -0.145775f,
+ -0.146237f,
+ -0.146781f,
+ -0.133969f,
+ -0.126625f,
+ -0.103444f,
+ -0.078944f,
+ -0.056825f,
+ -0.051194f,
+ -0.031113f,
+ 0.001731f,
+ 0.013669f,
+ 0.031762f,
+ 0.067819f,
+ 0.086513f,
+ 0.095744f,
+ 0.098575f,
+ 0.144675f,
+ 0.169887f,
+ 0.181625f,
+ 0.182538f,
+ 0.184000f,
+ 0.181700f,
+ 0.179113f,
+ 0.175650f,
+ 0.171756f,
+ 0.165131f,
+ 0.157331f,
+ 0.148362f,
+ 0.135513f,
+ 0.117400f,
+ 0.096025f,
+ 0.075794f,
+ 0.049750f,
+ 0.027281f,
+ 0.002362f,
+ -0.022263f,
+ -0.047125f,
+ -0.071450f,
+ -0.094606f,
+ -0.116219f,
+ -0.137212f,
+ -0.154700f,
+ -0.169344f,
+ -0.181987f,
+ -0.190906f,
+ -0.199313f,
+ -0.205119f,
+ -0.209444f,
+ -0.210800f,
+ -0.213313f,
+ -0.208550f,
+ -0.198006f,
+ -0.198963f,
+ -0.191019f,
+ -0.155650f,
+ -0.127944f,
+ -0.116944f,
+ -0.093850f,
+ -0.090725f,
+ -0.064675f,
+ -0.044606f,
+ -0.007169f,
+ 0.011031f,
+ 0.032356f,
+ 0.046712f,
+ 0.070844f,
+ 0.078462f,
+ 0.096656f,
+ 0.106638f,
+ 0.108544f,
+ 0.106056f,
+ 0.103575f,
+ 0.100712f,
+ 0.096706f,
+ 0.092119f,
+ 0.085638f,
+ 0.078119f,
+ 0.068769f,
+ 0.057044f,
+ 0.042213f,
+ 0.024731f,
+ 0.007244f,
+ -0.013319f,
+ -0.031987f,
+ -0.050713f,
+ -0.071331f,
+ -0.090000f,
+ -0.107419f,
+ -0.124919f,
+ -0.138956f,
+ -0.148194f,
+ -0.156188f,
+ -0.162256f,
+ -0.166594f,
+ -0.170775f,
+ -0.173956f,
+ -0.176150f,
+ -0.174450f,
+ -0.172625f,
+ -0.166725f,
+ -0.162969f,
+ -0.159125f,
+ -0.101450f,
+ -0.101656f,
+ -0.102894f,
+ -0.072912f,
+ -0.059963f,
+ -0.014494f,
+ 0.014669f,
+ 0.025581f,
+ 0.044788f,
+ 0.054700f,
+ 0.073900f,
+ 0.101162f,
+ 0.129412f,
+ 0.145888f,
+ 0.152450f,
+ 0.154637f,
+ 0.156625f,
+ 0.154500f,
+ 0.152175f,
+ 0.149694f,
+ 0.146238f,
+ 0.141287f,
+ 0.135531f,
+ 0.128325f,
+ 0.116469f,
+ 0.104038f,
+ 0.088019f,
+ 0.070444f,
+ 0.051156f,
+ 0.029863f,
+ 0.009437f,
+ -0.011200f,
+ -0.031337f,
+ -0.051631f,
+ -0.070569f,
+ -0.087644f,
+ -0.102938f,
+ -0.116325f,
+ -0.125856f,
+ -0.131613f,
+ -0.137150f,
+ -0.141738f,
+ -0.145225f,
+ -0.148081f,
+ -0.147069f,
+ -0.149875f,
+ -0.152100f,
+ -0.153906f,
+ -0.132369f,
+ -0.088994f,
+ -0.069119f,
+ -0.061569f,
+ -0.049200f,
+ -0.015256f,
+ 0.004400f,
+ 0.036750f,
+ 0.059444f,
+ 0.071131f,
+ 0.070031f,
+ 0.077531f,
+ 0.142062f,
+ 0.171950f,
+ 0.172831f,
+ 0.176000f,
+ 0.174181f,
+ 0.174181f,
+ 0.171662f,
+ 0.168463f,
+ 0.165631f,
+ 0.159963f,
+ 0.153544f,
+ 0.145281f,
+ 0.136113f,
+ 0.123325f,
+ 0.105850f,
+ 0.085250f,
+ 0.067587f,
+ 0.046300f,
+ 0.023975f,
+ 0.000925f,
+ -0.020594f,
+ -0.041362f,
+ -0.059894f,
+ -0.078794f,
+ -0.097756f,
+ -0.110000f,
+ -0.120506f,
+ -0.128787f,
+ -0.133744f,
+ -0.138325f,
+ -0.140962f,
+ -0.140575f,
+ -0.135106f,
+ -0.136819f,
+ -0.138387f,
+ -0.139819f,
+ -0.129556f,
+ -0.089631f,
+ -0.061606f,
+ -0.038975f,
+ -0.031538f,
+ -0.007769f,
+ -0.004612f,
+ 0.059831f,
+ 0.079394f,
+ 0.095950f,
+ 0.103594f,
+ 0.117988f,
+ 0.139350f,
+ 0.181600f,
+ 0.186719f,
+ 0.198825f,
+ 0.196637f,
+ 0.193200f,
+ 0.188494f,
+ 0.184769f,
+ 0.180225f,
+ 0.172387f,
+ 0.161325f,
+ 0.149438f,
+ 0.132094f,
+ 0.111875f,
+ 0.089531f,
+ 0.065287f,
+ 0.036825f,
+ 0.009931f,
+ -0.018200f,
+ -0.048594f,
+ -0.076800f,
+ -0.105388f,
+ -0.132406f,
+ -0.160794f,
+ -0.186469f,
+ -0.210775f,
+ -0.232338f,
+ -0.252019f,
+ -0.267619f,
+ -0.280381f,
+ -0.289644f,
+ -0.296994f,
+ -0.302706f,
+ -0.306462f,
+ -0.309881f,
+ -0.313525f,
+ -0.315750f,
+ -0.307525f,
+ -0.293944f,
+ -0.282450f,
+ -0.251212f,
+ -0.226325f,
+ -0.210987f,
+ -0.204450f,
+ -0.173231f,
+ -0.143187f,
+ -0.134844f,
+ -0.124781f,
+ -0.099100f,
+ -0.083056f,
+ -0.034987f,
+ -0.030744f,
+ -0.020162f,
+ -0.012987f,
+ -0.009969f,
+ -0.012356f,
+ -0.014894f,
+ -0.017394f,
+ -0.020219f,
+ -0.024469f,
+ -0.029319f,
+ -0.033537f,
+ -0.040644f,
+ -0.049644f,
+ -0.060381f,
+ -0.074750f,
+ -0.086688f,
+ -0.102306f,
+ -0.117188f,
+ -0.131281f,
+ -0.147531f,
+ -0.161425f,
+ -0.173375f,
+ -0.183606f,
+ -0.191769f,
+ -0.198031f,
+ -0.202662f,
+ -0.206525f,
+ -0.209394f,
+ -0.212137f,
+ -0.215525f,
+ -0.218406f,
+ -0.220694f,
+ -0.214412f,
+ -0.213237f,
+ -0.159637f,
+ -0.134744f,
+ -0.116700f,
+ -0.116344f,
+ -0.110912f,
+ -0.092840f,
+ -0.045313f,
+ -0.035827f,
+ 0.000320f,
+ 0.025660f,
+ 0.047920f,
+ 0.061213f,
+ 0.076007f,
+ 0.085193f,
+ 0.121233f,
+ 0.156607f,
+ 0.155233f,
+ 0.153413f,
+ 0.151293f,
+ 0.149040f,
+ 0.149087f,
+ 0.146640f,
+ 0.143507f,
+ 0.137667f,
+ 0.132000f,
+ 0.122420f,
+ 0.113213f,
+ 0.097900f,
+ 0.079080f,
+ 0.059000f,
+ 0.037913f,
+ 0.014693f,
+ -0.007800f,
+ -0.030120f,
+ -0.053140f,
+ -0.075453f,
+ -0.095673f,
+ -0.115587f,
+ -0.132073f,
+ -0.146687f,
+ -0.157707f,
+ -0.166227f,
+ -0.173467f,
+ -0.178547f,
+ -0.182167f,
+ -0.188507f,
+ -0.192153f,
+ -0.189907f,
+ -0.182833f,
+ -0.174747f,
+ -0.170887f,
+ -0.157240f,
+ -0.090387f,
+ -0.061893f,
+ -0.055387f,
+ -0.048353f,
+ -0.045593f,
+ -0.011773f,
+ 0.015747f,
+ 0.023620f,
+ 0.058207f,
+ 0.082733f,
+ 0.099067f,
+ 0.097860f,
+ 0.110813f,
+ 0.108400f,
+ 0.104680f,
+ 0.101513f,
+ 0.100553f,
+ 0.094180f,
+ 0.086253f,
+ 0.076773f,
+ 0.062740f,
+ 0.043800f,
+ 0.023007f,
+ 0.001600f,
+ -0.021553f,
+ -0.046287f,
+ -0.073020f,
+ -0.101780f,
+ -0.128440f,
+ -0.154260f,
+ -0.179153f,
+ -0.203240f,
+ -0.227460f,
+ -0.246773f,
+ -0.265313f,
+ -0.279907f,
+ -0.291047f,
+ -0.300820f,
+ -0.308360f,
+ -0.312393f,
+ -0.316340f,
+ -0.319707f,
+ -0.317220f,
+ -0.319280f,
+ -0.321233f,
+ -0.323653f,
+ -0.291420f,
+ -0.259987f,
+ -0.227320f,
+ -0.203833f,
+ -0.190133f,
+ -0.191640f,
+ -0.156520f,
+ -0.123580f,
+ -0.086180f,
+ -0.071987f,
+ -0.073913f,
+ -0.069533f,
+ -0.042840f,
+ 0.011393f,
+ 0.018500f,
+ 0.023353f,
+ 0.025253f,
+ 0.022607f,
+ 0.019833f,
+ 0.016853f,
+ 0.012693f,
+ 0.009253f,
+ 0.003580f,
+ -0.003193f,
+ -0.010813f,
+ -0.021360f,
+ -0.035427f,
+ -0.050813f,
+ -0.065700f,
+ -0.081780f,
+ -0.097640f,
+ -0.113353f,
+ -0.128007f,
+ -0.138673f,
+ -0.151340f,
+ -0.158993f,
+ -0.167467f,
+ -0.171553f,
+ -0.176120f,
+ -0.180680f,
+ -0.183760f,
+ -0.187340f,
+ -0.184993f,
+ -0.179213f,
+ -0.179020f,
+ -0.151513f,
+ -0.095493f,
+ -0.092187f,
+ -0.093673f,
+ -0.079760f,
+ -0.046880f,
+ -0.000753f,
+ 0.029587f,
+ 0.048640f,
+ 0.065493f,
+ 0.095260f,
+ 0.145393f,
+ 0.164140f,
+ 0.176540f,
+ 0.183180f,
+ 0.201993f,
+ 0.245313f,
+ 0.252920f,
+ 0.265713f,
+ 0.278227f,
+ 0.276040f,
+ 0.272253f,
+ 0.271847f,
+ 0.268340f,
+ 0.262533f,
+ 0.256180f,
+ 0.248640f,
+ 0.236327f,
+ 0.221820f,
+ 0.205627f,
+ 0.190560f,
+ 0.170127f,
+ 0.149533f,
+ 0.131567f,
+ 0.112393f,
+ 0.093427f,
+ 0.076947f,
+ 0.058460f,
+ 0.044907f,
+ 0.030447f,
+ 0.021260f,
+ 0.013327f,
+ 0.007393f,
+ 0.002987f,
+ -0.000933f,
+ -0.004227f,
+ -0.002400f,
+ 0.013607f,
+ 0.022593f,
+ 0.025300f,
+ 0.042367f,
+ 0.059540f,
+ 0.092433f,
+ 0.091513f,
+ 0.110560f,
+ 0.155500f,
+ 0.167433f,
+ 0.196847f,
+ 0.220053f,
+ 0.218833f,
+ 0.228047f,
+ 0.259247f,
+ 0.279740f,
+ 0.307887f,
+ 0.306800f,
+ 0.314480f,
+ 0.310873f,
+ 0.308100f,
+ 0.305133f,
+ 0.303853f,
+ 0.298700f,
+ 0.291567f,
+ 0.282213f,
+ 0.267547f,
+ 0.251440f,
+ 0.231267f,
+ 0.211587f,
+ 0.189413f,
+ 0.166680f,
+ 0.141913f,
+ 0.117687f,
+ 0.093727f,
+ 0.071027f,
+ 0.049280f,
+ 0.028320f,
+ 0.010587f,
+ -0.005347f,
+ -0.016647f,
+ -0.025473f,
+ -0.031907f,
+ -0.036533f,
+ -0.041160f,
+ -0.043893f,
+ -0.046200f,
+ -0.042467f,
+ -0.044267f,
+ -0.037273f,
+ -0.020860f,
+ 0.012353f,
+ 0.038260f,
+ 0.058700f,
+ 0.068460f,
+ 0.099967f,
+ 0.133227f,
+ 0.147800f,
+ 0.185260f,
+ 0.191607f,
+ 0.193707f,
+ 0.221247f,
+ 0.275213f,
+ 0.275947f,
+ 0.292947f,
+ 0.290633f,
+ 0.289480f,
+ 0.285947f,
+ 0.282413f,
+ 0.278607f,
+ 0.272500f,
+ 0.266413f,
+ 0.255333f,
+ 0.240933f,
+ 0.223907f,
+ 0.203180f,
+ 0.179953f,
+ 0.156087f,
+ 0.129807f,
+ 0.103673f,
+ 0.077320f,
+ 0.049760f,
+ 0.022953f,
+ -0.002580f,
+ -0.027133f,
+ -0.050447f,
+ -0.071287f,
+ -0.089493f,
+ -0.105093f,
+ -0.116627f,
+ -0.125640f,
+ -0.133440f,
+ -0.138480f,
+ -0.142093f,
+ -0.144633f,
+ -0.146933f,
+ -0.148987f,
+ -0.146147f,
+ -0.147540f,
+ -0.135653f,
+ -0.079047f,
+ -0.067220f,
+ -0.058640f,
+ -0.051533f,
+ -0.027127f,
+ 0.012513f,
+ 0.032360f,
+ 0.042440f,
+ 0.043020f,
+ 0.064047f,
+ 0.113027f,
+ 0.140860f,
+ 0.158727f,
+ 0.159040f,
+ 0.157140f,
+ 0.155280f,
+ 0.157900f,
+ 0.155840f,
+ 0.153720f,
+ 0.149860f,
+ 0.144207f,
+ 0.139720f,
+ 0.131593f,
+ 0.123460f,
+ 0.111593f,
+ 0.096113f,
+ 0.079907f,
+ 0.063173f,
+ 0.044673f,
+ 0.027680f,
+ 0.010220f,
+ -0.009580f,
+ -0.026127f,
+ -0.042293f,
+ -0.055753f,
+ -0.066860f,
+ -0.075933f,
+ -0.082253f,
+ -0.088033f,
+ -0.093560f,
+ -0.096840f,
+ -0.099353f,
+ -0.096307f,
+ -0.098487f,
+ -0.100287f,
+ -0.098413f,
+ -0.088347f,
+ -0.078973f,
+ -0.037447f,
+ -0.010080f,
+ 0.001927f,
+ 0.017447f,
+ 0.029467f,
+ 0.053253f,
+ 0.074053f,
+ 0.081007f,
+ 0.114420f,
+ 0.141207f,
+ 0.162287f,
+ 0.170207f,
+ 0.172407f,
+ 0.170567f,
+ 0.170627f,
+ 0.168033f,
+ 0.165447f,
+ 0.160720f,
+ 0.155113f,
+ 0.147587f,
+ 0.138213f,
+ 0.124153f,
+ 0.106333f,
+ 0.085567f,
+ 0.062967f,
+ 0.037273f,
+ 0.012260f,
+ -0.013380f,
+ -0.041047f,
+ -0.069860f,
+ -0.097547f,
+ -0.124000f,
+ -0.150240f,
+ -0.174833f,
+ -0.197087f,
+ -0.218000f,
+ -0.235453f,
+ -0.248440f,
+ -0.258673f,
+ -0.264760f,
+ -0.271320f,
+ -0.276280f,
+ -0.279367f,
+ -0.274680f,
+ -0.270667f,
+ -0.273207f,
+ -0.274553f,
+ -0.271693f,
+ -0.226300f,
+ -0.210113f,
+ -0.193393f,
+ -0.181180f,
+ -0.156633f,
+ -0.143587f,
+ -0.128567f,
+ -0.111420f,
+ -0.091187f,
+ -0.052360f,
+ -0.034733f,
+ -0.036067f,
+ -0.038333f,
+ -0.040440f,
+ -0.042847f,
+ -0.045433f,
+ -0.048453f,
+ -0.053107f,
+ -0.059967f,
+ -0.067973f,
+ -0.079180f,
+ -0.094307f,
+ -0.112673f,
+ -0.135753f,
+ -0.158647f,
+ -0.183827f,
+ -0.211420f,
+ -0.236313f,
+ -0.262840f,
+ -0.289547f,
+ -0.314553f,
+ -0.340173f,
+ -0.359847f,
+ -0.380520f,
+ -0.395733f,
+ -0.409893f,
+ -0.417667f,
+ -0.424287f,
+ -0.428800f,
+ -0.432833f,
+ -0.435807f,
+ -0.434580f,
+ -0.434220f,
+ -0.431100f,
+ -0.428800f,
+ -0.396040f,
+ -0.361600f,
+ -0.362513f,
+ -0.327600f,
+ -0.314000f,
+ -0.272767f,
+ -0.266013f,
+ -0.225947f,
+ -0.189107f,
+ -0.165453f,
+ -0.141173f,
+ -0.133073f,
+ -0.131367f,
+ -0.100873f,
+ -0.052280f,
+ -0.032673f,
+ -0.034180f,
+ -0.036107f,
+ -0.038513f,
+ -0.041073f,
+ -0.043693f,
+ -0.047387f,
+ -0.051867f,
+ -0.056727f,
+ -0.063047f,
+ -0.070927f,
+ -0.081447f,
+ -0.092607f,
+ -0.109347f,
+ -0.124487f,
+ -0.140687f,
+ -0.155580f,
+ -0.168200f,
+ -0.184247f,
+ -0.194213f,
+ -0.207073f,
+ -0.215840f,
+ -0.221867f,
+ -0.226933f,
+ -0.229540f,
+ -0.233153f,
+ -0.235807f,
+ -0.233533f,
+ -0.235387f,
+ -0.229447f,
+ -0.220833f,
+ -0.191613f,
+ -0.162353f,
+ -0.151047f,
+ -0.151653f,
+ -0.110413f,
+ -0.063580f,
+ -0.056393f,
+ -0.043967f,
+ -0.033413f,
+ 0.007420f,
+ 0.049800f,
+ 0.059087f,
+ 0.063980f,
+ 0.073287f,
+ 0.105720f,
+ 0.156160f,
+ 0.157447f,
+ 0.161753f,
+ 0.159820f,
+ 0.160320f,
+ 0.160253f,
+ 0.157140f,
+ 0.153000f,
+ 0.148947f,
+ 0.140733f,
+ 0.132760f,
+ 0.122433f,
+ 0.108840f,
+ 0.094167f,
+ 0.078287f,
+ 0.055887f,
+ 0.041520f,
+ 0.015033f,
+ 0.001847f,
+ -0.023147f,
+ -0.042513f,
+ -0.058040f,
+ -0.074307f,
+ -0.085493f,
+ -0.092540f,
+ -0.100493f,
+ -0.107213f,
+ -0.112980f,
+ -0.118227f,
+ -0.120900f,
+ -0.122887f,
+ -0.122847f,
+ -0.124547f,
+ -0.120353f,
+ -0.114727f,
+ -0.101233f,
+ -0.055893f,
+ -0.050460f,
+ -0.038727f,
+ -0.018993f,
+ 0.003293f,
+ 0.025453f,
+ 0.048540f,
+ 0.058213f,
+ 0.077493f,
+ 0.083693f,
+ 0.121247f,
+ 0.138167f,
+ 0.139133f,
+ 0.145527f,
+ 0.146467f,
+ 0.144427f,
+ 0.142167f,
+ 0.138980f,
+ 0.134993f,
+ 0.129600f,
+ 0.125353f,
+ 0.116940f,
+ 0.109640f,
+ 0.097773f,
+ 0.083960f,
+ 0.067980f,
+ 0.051040f,
+ 0.026920f,
+ 0.011553f,
+ -0.007313f,
+ -0.025193f,
+ -0.045460f,
+ -0.057247f,
+ -0.071767f,
+ -0.081260f,
+ -0.091507f,
+ -0.098067f,
+ -0.103980f,
+ -0.107213f,
+ -0.111107f,
+ -0.113533f,
+ -0.113687f,
+ -0.115653f,
+ -0.115067f,
+ -0.102853f,
+ -0.086333f,
+ -0.046353f,
+ -0.022133f,
+ 0.003067f,
+ 0.015407f,
+ 0.036820f,
+ 0.048000f,
+ 0.071607f,
+ 0.115453f,
+ 0.131153f,
+ 0.143773f,
+ 0.155687f,
+ 0.166020f,
+ 0.228533f,
+ 0.248747f,
+ 0.253667f,
+ 0.251233f,
+ 0.248387f,
+ 0.245627f,
+ 0.242720f,
+ 0.240140f,
+ 0.234580f,
+ 0.228613f,
+ 0.219353f,
+ 0.209100f,
+ 0.193853f,
+ 0.172753f,
+ 0.147620f,
+ 0.126787f,
+ 0.103373f,
+ 0.079393f,
+ 0.054967f,
+ 0.025467f,
+ -0.000507f,
+ -0.024840f,
+ -0.048520f,
+ -0.073227f,
+ -0.094320f,
+ -0.111860f,
+ -0.125100f,
+ -0.135320f,
+ -0.145153f,
+ -0.152867f,
+ -0.158887f,
+ -0.162020f,
+ -0.165440f,
+ -0.167527f,
+ -0.169360f,
+ -0.171107f,
+ -0.170060f,
+ -0.168167f,
+ -0.142867f,
+ -0.093533f,
+ -0.084373f,
+ -0.068600f,
+ -0.062307f,
+ -0.047053f,
+ -0.020833f,
+ -0.021527f,
+ 0.034467f,
+ 0.050153f,
+ 0.067513f,
+ 0.072833f,
+ 0.073147f,
+ 0.075527f,
+ 0.087020f,
+ 0.085180f,
+ 0.082913f,
+ 0.080027f,
+ 0.076273f,
+ 0.071513f,
+ 0.064420f,
+ 0.057420f,
+ 0.047687f,
+ 0.035133f,
+ 0.019367f,
+ 0.001013f,
+ -0.017287f,
+ -0.036420f,
+ -0.056093f,
+ -0.075187f,
+ -0.095800f,
+ -0.114540f,
+ -0.132547f,
+ -0.149867f,
+ -0.163467f,
+ -0.175080f,
+ -0.183287f,
+ -0.190447f,
+ -0.195500f,
+ -0.200033f,
+ -0.203220f,
+ -0.207560f,
+ -0.211053f,
+ -0.207627f,
+ -0.187793f,
+ -0.188613f,
+ -0.167140f,
+ -0.142267f,
+ -0.134247f,
+ -0.135553f,
+ -0.108660f,
+ -0.074313f,
+ -0.048847f,
+ -0.040147f,
+ -0.029900f,
+ -0.001000f,
+ 0.013620f,
+ 0.041633f,
+ 0.062113f,
+ 0.072120f,
+ 0.075407f,
+ 0.074973f,
+ 0.077113f,
+ 0.078833f,
+ 0.076287f,
+ 0.073147f,
+ 0.069273f,
+ 0.064167f,
+ 0.057400f,
+ 0.048267f,
+ 0.035967f,
+ 0.018540f,
+ -0.002227f,
+ -0.021513f,
+ -0.043320f,
+ -0.066173f,
+ -0.090113f,
+ -0.114393f,
+ -0.133400f,
+ -0.157180f,
+ -0.179420f,
+ -0.198513f,
+ -0.216953f,
+ -0.231513f,
+ -0.242713f,
+ -0.252033f,
+ -0.258627f,
+ -0.263847f,
+ -0.267340f,
+ -0.270247f,
+ -0.273227f,
+ -0.275900f,
+ -0.275167f,
+ -0.263053f,
+ -0.232147f,
+ -0.208087f,
+ -0.206867f,
+ -0.188100f,
+ -0.145373f,
+ -0.132407f,
+ -0.089840f,
+ -0.075447f,
+ -0.059080f,
+ -0.026333f,
+ -0.009720f,
+ 0.018020f,
+ 0.026573f,
+ 0.059567f,
+ 0.079507f,
+ 0.083140f,
+ 0.081633f,
+ 0.079833f,
+ 0.077993f,
+ 0.075473f,
+ 0.072307f,
+ 0.066933f,
+ 0.062400f,
+ 0.056573f,
+ 0.046933f,
+ 0.033407f,
+ 0.016000f,
+ -0.004667f,
+ -0.025400f,
+ -0.048027f,
+ -0.071380f,
+ -0.093280f,
+ -0.117160f,
+ -0.138900f,
+ -0.160147f,
+ -0.181120f,
+ -0.198660f,
+ -0.215047f,
+ -0.228240f,
+ -0.236860f,
+ -0.243033f,
+ -0.247873f,
+ -0.251580f,
+ -0.250393f,
+ -0.250420f,
+ -0.252320f,
+ -0.254013f,
+ -0.255873f,
+ -0.252347f,
+ -0.220627f,
+ -0.188220f,
+ -0.184320f,
+ -0.143407f,
+ -0.142947f,
+ -0.095653f,
+ -0.073007f,
+ -0.048027f,
+ -0.019107f,
+ -0.007513f,
+ 0.001107f,
+ 0.014880f,
+ 0.038327f,
+ 0.090433f,
+ 0.091233f,
+ 0.098060f,
+ 0.098153f,
+ 0.102127f,
+ 0.109247f,
+ 0.106640f,
+ 0.103667f,
+ 0.103633f,
+ 0.100067f,
+ 0.094353f,
+ 0.086087f,
+ 0.076967f,
+ 0.065267f,
+ 0.050200f,
+ 0.035547f,
+ 0.018473f,
+ 0.001200f,
+ -0.014467f,
+ -0.031253f,
+ -0.046953f,
+ -0.061427f,
+ -0.073320f,
+ -0.082127f,
+ -0.089027f,
+ -0.094387f,
+ -0.098600f,
+ -0.102120f,
+ -0.104493f,
+ -0.106860f,
+ -0.105887f,
+ -0.107520f,
+ -0.104380f,
+ -0.094893f,
+ -0.059420f,
+ -0.024007f,
+ -0.015293f,
+ -0.011933f,
+ 0.004033f,
+ 0.043580f,
+ 0.069773f,
+ 0.085333f,
+ 0.114927f,
+ 0.159247f,
+ 0.183380f,
+ 0.197993f,
+ 0.199200f,
+ 0.223687f,
+ 0.236573f,
+ 0.271133f,
+ 0.296067f,
+ 0.299927f,
+ 0.300393f,
+ 0.297800f,
+ 0.295287f,
+ 0.292800f,
+ 0.289680f,
+ 0.284793f,
+ 0.279980f,
+ 0.272860f,
+ 0.262347f,
+ 0.251207f,
+ 0.235400f,
+ 0.218660f,
+ 0.200967f,
+ 0.183627f,
+ 0.163407f,
+ 0.146053f,
+ 0.127953f,
+ 0.110260f,
+ 0.095333f,
+ 0.081320f,
+ 0.071233f,
+ 0.062887f,
+ 0.056067f,
+ 0.050820f,
+ 0.046487f,
+ 0.043200f,
+ 0.041040f,
+ 0.042827f,
+ 0.041273f,
+ 0.045500f,
+ 0.044673f,
+ 0.050520f,
+ 0.088900f,
+ 0.118100f,
+ 0.128293f,
+ 0.146340f,
+ 0.155087f,
+ 0.170807f,
+ 0.199073f,
+ 0.232267f,
+ 0.245100f,
+ 0.258547f,
+ 0.275147f,
+ 0.278620f,
+ 0.325347f,
+ 0.344673f,
+ 0.346880f,
+ 0.344987f,
+ 0.342747f,
+ 0.340707f,
+ 0.340607f,
+ 0.338307f,
+ 0.334207f,
+ 0.328827f,
+ 0.322473f,
+ 0.314373f,
+ 0.302447f,
+ 0.288580f,
+ 0.270267f,
+ 0.250553f,
+ 0.230267f,
+ 0.209440f,
+ 0.187673f,
+ 0.166347f,
+ 0.144033f,
+ 0.124367f,
+ 0.103947f,
+ 0.085760f,
+ 0.068260f,
+ 0.055147f,
+ 0.044007f,
+ 0.036233f,
+ 0.029773f,
+ 0.025480f,
+ 0.024687f,
+ 0.021273f,
+ 0.028273f,
+ 0.026220f,
+ 0.033093f,
+ 0.032007f,
+ 0.051980f,
+ 0.077627f,
+ 0.106033f,
+ 0.112640f,
+ 0.111373f,
+ 0.159440f,
+ 0.186233f,
+ 0.210953f,
+ 0.232933f,
+ 0.236173f,
+ 0.245080f,
+ 0.277173f,
+ 0.287353f,
+ 0.301040f,
+ 0.311240f,
+ 0.308833f,
+ 0.314900f,
+ 0.311773f,
+ 0.307420f,
+ 0.300940f,
+ 0.291940f,
+ 0.279813f,
+ 0.263680f,
+ 0.244493f,
+ 0.222947f,
+ 0.199013f,
+ 0.173380f,
+ 0.145793f,
+ 0.116867f,
+ 0.087507f,
+ 0.057773f,
+ 0.025547f,
+ -0.005920f,
+ -0.036727f,
+ -0.065480f,
+ -0.092827f,
+ -0.119873f,
+ -0.146487f,
+ -0.171080f,
+ -0.193427f,
+ -0.210707f,
+ -0.225573f,
+ -0.236493f,
+ -0.243500f,
+ -0.249173f,
+ -0.253087f,
+ -0.256627f,
+ -0.259227f,
+ -0.258120f,
+ -0.256773f,
+ -0.254547f,
+ -0.247673f,
+ -0.220340f,
+ -0.212100f,
+ -0.181540f,
+ -0.169607f,
+ -0.161507f,
+ -0.126847f,
+ -0.118647f,
+ -0.106473f,
+ -0.095880f,
+ -0.063627f,
+ -0.054907f,
+ -0.044480f,
+ -0.041187f,
+ -0.036360f,
+ -0.022120f,
+ -0.017373f,
+ -0.020980f,
+ -0.026427f,
+ -0.031240f,
+ -0.039233f,
+ -0.047660f,
+ -0.058060f,
+ -0.071407f,
+ -0.086780f,
+ -0.102533f,
+ -0.120720f,
+ -0.139867f,
+ -0.158860f,
+ -0.179820f,
+ -0.197313f,
+ -0.217060f,
+ -0.234560f,
+ -0.251260f,
+ -0.266127f,
+ -0.278480f,
+ -0.287327f,
+ -0.294773f,
+ -0.301353f,
+ -0.305860f,
+ -0.308947f,
+ -0.308320f,
+ -0.310520f,
+ -0.313133f,
+ -0.314693f,
+ -0.303220f,
+ -0.298807f,
+ -0.252487f,
+ -0.243347f,
+ -0.221793f,
+ -0.214260f,
+ -0.204807f,
+ -0.149567f,
+ -0.128987f,
+ -0.096120f,
+ -0.089567f,
+ -0.065927f,
+ -0.057433f,
+ -0.042340f,
+ -0.010727f,
+ 0.017607f,
+ 0.038953f,
+ 0.041893f,
+ 0.044120f,
+ 0.041740f,
+ 0.047720f,
+ 0.045267f,
+ 0.041820f,
+ 0.035653f,
+ 0.030220f,
+ 0.023013f,
+ 0.012140f,
+ -0.001987f,
+ -0.019420f,
+ -0.036947f,
+ -0.057473f,
+ -0.078180f,
+ -0.099773f,
+ -0.119260f,
+ -0.141113f,
+ -0.164080f,
+ -0.186033f,
+ -0.208040f,
+ -0.228160f,
+ -0.247347f,
+ -0.264167f,
+ -0.276847f,
+ -0.287113f,
+ -0.293933f,
+ -0.300533f,
+ -0.305800f,
+ -0.309353f,
+ -0.312233f,
+ -0.315160f,
+ -0.314653f,
+ -0.311393f,
+ -0.297193f,
+ -0.260727f,
+ -0.232020f,
+ -0.231953f,
+ -0.231127f,
+ -0.216893f,
+ -0.175380f,
+ -0.129633f,
+ -0.109093f,
+ -0.104500f,
+ -0.103160f,
+ -0.103973f,
+ -0.038213f,
+ -0.007180f,
+ -0.008213f,
+ -0.008013f,
+ -0.006260f,
+ -0.004307f,
+ -0.006827f,
+ -0.009800f,
+ -0.014107f,
+ -0.018833f,
+ -0.026167f,
+ -0.035873f,
+ -0.051127f,
+ -0.069353f,
+ -0.088473f,
+ -0.112273f,
+ -0.134147f,
+ -0.158020f,
+ -0.184247f,
+ -0.209440f,
+ -0.234980f,
+ -0.260947f,
+ -0.284300f,
+ -0.307153f,
+ -0.329767f,
+ -0.349920f,
+ -0.366093f,
+ -0.377840f,
+ -0.388973f,
+ -0.395893f,
+ -0.401793f,
+ -0.405173f,
+ -0.408153f,
+ -0.411020f,
+ -0.415200f,
+ -0.418053f,
+ -0.415653f,
+ -0.393747f,
+ -0.369660f,
+ -0.327613f,
+ -0.317300f,
+ -0.293253f,
+ -0.274180f,
+ -0.268553f,
+ -0.236080f,
+ -0.208333f,
+ -0.188460f,
+ -0.145420f,
+ -0.128287f,
+ -0.112907f,
+ -0.114267f,
+ -0.091700f,
+ -0.054933f,
+ -0.047307f,
+ -0.041120f,
+ -0.042453f,
+ -0.045180f,
+ -0.046920f,
+ -0.049233f,
+ -0.052060f,
+ -0.055020f,
+ -0.059300f,
+ -0.064307f,
+ -0.070193f,
+ -0.076653f,
+ -0.084427f,
+ -0.094900f,
+ -0.105993f,
+ -0.119367f,
+ -0.131273f,
+ -0.142960f,
+ -0.153653f,
+ -0.160940f,
+ -0.168887f,
+ -0.174787f,
+ -0.178840f,
+ -0.182447f,
+ -0.185580f,
+ -0.188080f,
+ -0.186680f,
+ -0.188560f,
+ -0.191380f,
+ -0.193460f,
+ -0.172380f,
+ -0.118407f,
+ -0.103300f,
+ -0.104347f,
+ -0.088953f,
+ -0.074933f,
+ -0.025607f,
+ 0.003127f,
+ 0.023100f,
+ 0.058207f,
+ 0.074360f,
+ 0.096080f,
+ 0.117273f,
+ 0.146020f,
+ 0.181613f,
+ 0.199647f,
+ 0.199313f,
+ 0.222767f,
+ 0.255747f,
+ 0.269133f,
+ 0.270593f,
+ 0.272940f,
+ 0.271407f,
+ 0.269660f,
+ 0.267540f,
+ 0.264280f,
+ 0.259820f,
+ 0.254127f,
+ 0.247413f,
+ 0.239587f,
+ 0.226200f,
+ 0.212133f,
+ 0.196740f,
+ 0.179033f,
+ 0.157073f,
+ 0.139913f,
+ 0.119480f,
+ 0.099933f,
+ 0.085580f,
+ 0.071080f,
+ 0.055867f,
+ 0.044053f,
+ 0.036607f,
+ 0.030387f,
+ 0.024367f,
+ 0.019560f,
+ 0.016467f,
+ 0.013553f,
+ 0.015887f,
+ 0.018360f,
+ 0.018447f,
+ 0.023480f,
+ 0.035227f,
+ 0.061040f,
+ 0.097940f,
+ 0.109687f,
+ 0.111180f,
+ 0.134293f,
+ 0.136247f,
+ 0.147247f,
+ 0.216740f,
+ 0.215860f,
+ 0.216180f,
+ 0.260087f,
+ 0.265967f,
+ 0.274073f,
+ 0.279000f,
+ 0.277213f,
+ 0.278847f,
+ 0.276673f,
+ 0.274073f,
+ 0.270333f,
+ 0.266080f,
+ 0.260260f,
+ 0.252387f,
+ 0.240667f,
+ 0.228560f,
+ 0.211440f,
+ 0.191000f,
+ 0.170587f,
+ 0.147160f,
+ 0.124493f,
+ 0.099720f,
+ 0.077187f,
+ 0.054040f,
+ 0.031107f,
+ 0.009300f,
+ -0.010193f,
+ -0.029173f,
+ -0.042947f,
+ -0.057133f,
+ -0.067873f,
+ -0.075620f,
+ -0.081007f,
+ -0.085360f,
+ -0.089827f,
+ -0.092273f,
+ -0.083667f,
+ -0.077987f,
+ -0.080440f,
+ -0.075873f,
+ -0.038447f,
+ -0.010580f,
+ 0.007020f,
+ 0.027313f,
+ 0.033767f,
+ 0.061613f,
+ 0.086940f,
+ 0.099593f,
+ 0.115200f,
+ 0.132713f,
+ 0.158093f,
+ 0.177113f,
+ 0.201740f,
+ 0.202940f,
+ 0.205113f,
+ 0.207053f,
+ 0.204953f,
+ 0.202573f,
+ 0.199687f,
+ 0.195220f,
+ 0.189833f,
+ 0.181120f,
+ 0.172000f,
+ 0.156713f,
+ 0.137680f,
+ 0.114867f,
+ 0.093907f,
+ 0.068713f,
+ 0.042853f,
+ 0.015347f,
+ -0.010947f,
+ -0.037840f,
+ -0.063827f,
+ -0.089840f,
+ -0.116347f,
+ -0.141087f,
+ -0.160813f,
+ -0.180907f,
+ -0.199767f,
+ -0.214147f,
+ -0.224840f,
+ -0.200772f,
+ -0.181829f,
+ -0.192738f,
+ -0.201057f,
+ -0.208290f,
+ -0.214395f,
+ -0.219571f,
+ -0.213576f,
+ -0.201148f,
+ -0.174829f,
+ -0.141224f,
+ -0.131505f,
+ -0.123110f,
+ -0.124462f,
+ -0.093995f,
+ -0.060324f,
+ -0.043571f,
+ -0.004562f,
+ 0.016833f,
+ 0.028629f,
+ 0.028929f,
+ 0.048100f,
+ 0.074310f,
+ 0.089276f,
+ 0.092252f,
+ 0.093457f,
+ 0.097771f,
+ 0.095529f,
+ 0.092833f,
+ 0.089452f,
+ 0.086076f,
+ 0.080648f,
+ 0.074586f,
+ 0.064967f,
+ 0.055629f,
+ 0.041362f,
+ 0.027186f,
+ 0.011248f,
+ -0.005348f,
+ -0.022533f,
+ -0.039757f,
+ -0.057181f,
+ -0.073738f,
+ -0.088533f,
+ -0.101305f,
+ -0.110681f,
+ -0.119095f,
+ -0.124067f,
+ -0.129243f,
+ -0.133676f,
+ -0.137000f,
+ -0.139419f,
+ -0.137543f,
+ -0.139748f,
+ -0.135795f,
+ -0.136829f,
+ -0.127386f,
+ -0.077514f,
+ -0.055914f,
+ -0.054995f,
+ -0.046371f,
+ -0.024052f,
+ 0.016171f,
+ 0.043138f,
+ 0.046600f,
+ 0.064605f,
+ 0.096029f,
+ 0.135214f,
+ 0.139433f,
+ 0.156105f,
+ 0.183829f,
+ 0.190548f,
+ 0.197224f,
+ 0.195686f,
+ 0.197957f,
+ 0.195867f,
+ 0.193690f,
+ 0.190324f,
+ 0.185171f,
+ 0.177748f,
+ 0.169248f,
+ 0.158724f,
+ 0.143314f,
+ 0.125000f,
+ 0.102610f,
+ 0.080119f,
+ 0.056790f,
+ 0.033062f,
+ 0.010057f,
+ -0.015381f,
+ -0.038657f,
+ -0.063052f,
+ -0.084848f,
+ -0.106986f,
+ -0.125681f,
+ -0.142557f,
+ -0.154248f,
+ -0.162929f,
+ -0.170038f,
+ -0.176124f,
+ -0.180557f,
+ -0.183514f,
+ -0.184038f,
+ -0.186033f,
+ -0.182433f,
+ -0.178810f,
+ -0.177962f,
+};
\ No newline at end of file
diff --git a/motors/python/BUILD b/motors/python/BUILD
index 20dd786..458399f 100644
--- a/motors/python/BUILD
+++ b/motors/python/BUILD
@@ -10,3 +10,16 @@
],
restricted_to = ["//tools:k8"],
)
+
+py_binary(
+ name = "haptic_phase_current",
+ srcs = [
+ "haptic_phase_current.py",
+ ],
+ deps = [
+ "//external:python-gflags",
+ "//external:python-glog",
+ "//frc971/control_loops/python:controls",
+ ],
+ restricted_to = ["//tools:k8"],
+)
diff --git a/motors/python/haptic_phase_current.py b/motors/python/haptic_phase_current.py
new file mode 100755
index 0000000..b17c514
--- /dev/null
+++ b/motors/python/haptic_phase_current.py
@@ -0,0 +1,576 @@
+#!/usr/bin/python3
+
+import numpy
+from matplotlib import pylab
+import scipy.integrate
+from frc971.control_loops.python import controls
+import time
+import operator
+
+K1 = 1.81e04
+K2 = 0.0
+
+# Make the amplitude of the fundamental 1 for ease of playing with.
+K2 /= K1
+K1 = 1
+
+vcc = 14.0 # volts
+R_motor = 0.1073926073926074 # ohms for the motor
+R = R_motor + 0.080 + 0.02 # motor + fets + wires ohms for system
+
+L = 80.0 * 1e-6 # Henries
+M = L / 10.0
+
+Kv = 37.6 # rad/s/volt, where the voltage is measured from the neutral to the phase.
+J = 0.0000007
+
+R_shunt = 0.0003
+
+# RC circuit for current sense filtering.
+R_sense1 = 768.0
+R_sense2 = 1470.0
+C_sense = 10.0 * 1e-9
+
+# So, we measured the inductance by switching between ~5 and ~20 amps through
+# the motor.
+# We then looked at the change in voltage that should give us (assuming duty
+# cycle * vin), and divided it by the corresponding change in current.
+
+# We then looked at the amount of time it took to decay the current to 1/e
+# That gave us the inductance.
+
+# Overrides for experiments
+J = J * 10.0
+
+# Firing phase A -> 0.0
+# Firing phase B -> - numpy.pi * 2.0 / 3.0
+# Firing phase C -> + numpy.pi * 2.0 / 3.0
+
+hz = 20000.0
+
+#switching_pattern = 'front'
+switching_pattern = 'centered'
+#switching_pattern = 'rear'
+#switching_pattern = 'centered front shifted'
+#switching_pattern = 'anticentered'
+
+Vconv = numpy.matrix([[2.0, -1.0, -1.0],
+ [-1.0, 2.0, -1.0],
+ [-1.0, -1.0, 2.0]]) / 3.0
+
+def f_single(theta):
+ return K1 * numpy.sin(theta) + K2 * numpy.sin(theta * 5)
+
+def g_single(theta):
+ return K1 * numpy.sin(theta) - K2 * numpy.sin(theta * 5)
+
+def gdot_single(theta):
+ """Derivitive of the current.
+
+ Must be multiplied by omega externally.
+ """
+ return K1 * numpy.cos(theta) - 5.0 * K2 * numpy.cos(theta * 5.0)
+
+f = numpy.vectorize(f_single, otypes=(numpy.float,))
+g = numpy.vectorize(g_single, otypes=(numpy.float,))
+gdot = numpy.vectorize(gdot_single, otypes=(numpy.float,))
+
+def torque(theta):
+ return f(theta) * g(theta)
+
+def phase_a(function, theta):
+ return function(theta)
+
+def phase_b(function, theta):
+ return function(theta + 2 * numpy.pi / 3)
+
+def phase_c(function, theta):
+ return function(theta + 4 * numpy.pi / 3)
+
+def phases(function, theta):
+ return numpy.matrix([[phase_a(function, theta)],
+ [phase_b(function, theta)],
+ [phase_c(function, theta)]])
+
+def all_phases(function, theta_range):
+ return (phase_a(function, theta_range) +
+ phase_b(function, theta_range) +
+ phase_c(function, theta_range))
+
+theta_range = numpy.linspace(start=0, stop=4 * numpy.pi, num=10000)
+one_amp_driving_voltage = R * g(theta_range) + (L * gdot(theta_range) + M * gdot(theta_range + 2.0 / 3.0 * numpy.pi) + M * gdot(theta_range - 2.0 / 3.0 * numpy.pi)) * Kv * vcc / 2.0
+
+max_one_amp_driving_voltage = max(one_amp_driving_voltage)
+
+# The number to divide the product of the unit BEMF and the per phase current
+# by to get motor current.
+one_amp_scalar = (phases(f_single, 0.0).T * phases(g_single, 0.0))[0, 0]
+
+print 'Max BEMF', max(f(theta_range))
+print 'Max current', max(g(theta_range))
+print 'Max drive voltage (one_amp_driving_voltage)', max(one_amp_driving_voltage)
+print 'one_amp_scalar', one_amp_scalar
+
+pylab.figure()
+pylab.subplot(1, 1, 1)
+pylab.plot(theta_range, f(theta_range), label='bemf')
+pylab.plot(theta_range, g(theta_range), label='phase_current')
+pylab.plot(theta_range, torque(theta_range), label='phase_torque')
+pylab.plot(theta_range, all_phases(torque, theta_range), label='sum_torque/current')
+pylab.legend()
+
+
+def full_sample_times(Ton, Toff, dt, n, start_time):
+ """Returns n + 4 samples for the provided switching times.
+
+ We need the timesteps and Us to integrate.
+
+ Args:
+ Ton: On times for each phase.
+ Toff: Off times for each phase.
+ dt: The cycle time.
+ n: Number of intermediate points to include in the result.
+ start_time: Starting value for the t values in the result.
+
+ Returns:
+ array of [t, U matrix]
+ """
+
+ assert((Toff <= 1.0).all())
+ assert((Ton <= 1.0).all())
+ assert((Toff >= 0.0).all())
+ assert((Ton >= 0.0).all())
+
+ if (Ton <= Toff).all():
+ on_before_off = True
+ else:
+ # Verify that they are all ordered correctly.
+ assert(not (Ton <= Toff).any())
+ on_before_off = False
+
+ Toff = Toff.copy() * dt
+ Toff[Toff < 100e-9] = -1.0
+ Toff[Toff > dt] = dt
+
+ Ton = Ton.copy() * dt
+ Ton[Ton < 100e-9] = -1.0
+ Ton[Ton > dt - 100e-9] = dt + 1.0
+
+ result = []
+ t = 0
+
+ result_times = numpy.concatenate(
+ (numpy.linspace(0, dt, num=n),
+ numpy.reshape(numpy.asarray(Ton[numpy.logical_and(Ton < dt, Ton > 0.0)]), (-1,)),
+ numpy.reshape(numpy.asarray(Toff[numpy.logical_and(Toff < dt, Toff > 0.0)]), (-1,))
+ ))
+ result_times.sort()
+ assert((result_times >= 0).all())
+ assert((result_times <= dt).all())
+
+ for t in result_times:
+ if on_before_off:
+ U = numpy.matrix([[vcc], [vcc], [vcc]])
+ U[t <= Ton] = 0.0
+ U[Toff < t] = 0.0
+ else:
+ U = numpy.matrix([[0.0], [0.0], [0.0]])
+ U[t > Ton] = vcc
+ U[t <= Toff] = vcc
+ result.append((float(t + start_time), U.copy()))
+
+ return result
+
+def sample_times(T, dt, n, start_time):
+ if switching_pattern == 'rear':
+ T = 1.0 - T
+ ans = full_sample_times(T, numpy.matrix(numpy.ones((3, 1))) * 1.0, dt, n, start_time)
+ elif switching_pattern == 'centered front shifted':
+ # Centered, but shifted to the beginning of the cycle.
+ Ton = 0.5 - T / 2.0
+ Toff = 0.5 + T / 2.0
+
+ tn = min(Ton)[0, 0]
+ Ton -= tn
+ Toff -= tn
+
+ ans = full_sample_times(Ton, Toff, dt, n, start_time)
+ elif switching_pattern == 'centered':
+ # Centered, looks waaay better.
+ Ton = 0.5 - T / 2.0
+ Toff = 0.5 + T / 2.0
+
+ ans = full_sample_times(Ton, Toff, dt, n, start_time)
+ elif switching_pattern == 'anticentered':
+ # Centered, looks waaay better.
+ Toff = T / 2.0
+ Ton = 1.0 - T / 2.0
+
+ ans = full_sample_times(Ton, Toff, dt, n, start_time)
+ elif switching_pattern == 'front':
+ ans = full_sample_times(numpy.matrix(numpy.zeros((3, 1))), T, dt, n, start_time)
+ else:
+ assert(False)
+
+ return ans
+
+class DataLogger(object):
+ def __init__(self, title=None):
+ self.title = title
+ self.ia = []
+ self.ib = []
+ self.ic = []
+ self.ia_goal = []
+ self.ib_goal = []
+ self.ic_goal = []
+ self.ia_controls = []
+ self.ib_controls = []
+ self.ic_controls = []
+ self.isensea = []
+ self.isenseb = []
+ self.isensec = []
+
+ self.va = []
+ self.vb = []
+ self.vc = []
+ self.van = []
+ self.vbn = []
+ self.vcn = []
+
+ self.ea = []
+ self.eb = []
+ self.ec = []
+
+ self.theta = []
+ self.omega = []
+
+ self.i_goal = []
+
+ self.time = []
+ self.controls_time = []
+ self.predicted_time = []
+
+ self.ia_pred = []
+ self.ib_pred = []
+ self.ic_pred = []
+
+ self.voltage_time = []
+ self.estimated_velocity = []
+ self.U_last = numpy.matrix(numpy.zeros((3, 1)))
+
+ def log_predicted(self, current_time, p):
+ self.predicted_time.append(current_time)
+ self.ia_pred.append(p[0, 0])
+ self.ib_pred.append(p[1, 0])
+ self.ic_pred.append(p[2, 0])
+
+ def log_controls(self, current_time, measured_current, In, E, estimated_velocity):
+ self.controls_time.append(current_time)
+ self.ia_controls.append(measured_current[0, 0])
+ self.ib_controls.append(measured_current[1, 0])
+ self.ic_controls.append(measured_current[2, 0])
+
+ self.ea.append(E[0, 0])
+ self.eb.append(E[1, 0])
+ self.ec.append(E[2, 0])
+
+ self.ia_goal.append(In[0, 0])
+ self.ib_goal.append(In[1, 0])
+ self.ic_goal.append(In[2, 0])
+ self.estimated_velocity.append(estimated_velocity)
+
+ def log_data(self, X, U, current_time, Vn, i_goal):
+ self.ia.append(X[0, 0])
+ self.ib.append(X[1, 0])
+ self.ic.append(X[2, 0])
+
+ self.i_goal.append(i_goal)
+
+ self.isensea.append(X[5, 0])
+ self.isenseb.append(X[6, 0])
+ self.isensec.append(X[7, 0])
+
+ self.theta.append(X[3, 0])
+ self.omega.append(X[4, 0])
+
+ self.time.append(current_time)
+
+ self.van.append(Vn[0, 0])
+ self.vbn.append(Vn[1, 0])
+ self.vcn.append(Vn[2, 0])
+
+ if (self.U_last != U).any():
+ self.va.append(self.U_last[0, 0])
+ self.vb.append(self.U_last[1, 0])
+ self.vc.append(self.U_last[2, 0])
+ self.voltage_time.append(current_time)
+
+ self.va.append(U[0, 0])
+ self.vb.append(U[1, 0])
+ self.vc.append(U[2, 0])
+ self.voltage_time.append(current_time)
+ self.U_last = U.copy()
+
+ def plot(self):
+ fig = pylab.figure()
+ pylab.subplot(3, 1, 1)
+ pylab.plot(self.controls_time, self.ia_controls, 'ro', label='ia_controls')
+ pylab.plot(self.controls_time, self.ib_controls, 'go', label='ib_controls')
+ pylab.plot(self.controls_time, self.ic_controls, 'bo', label='ic_controls')
+ pylab.plot(self.controls_time, self.ia_goal, 'r--', label='ia_goal')
+ pylab.plot(self.controls_time, self.ib_goal, 'g--', label='ib_goal')
+ pylab.plot(self.controls_time, self.ic_goal, 'b--', label='ic_goal')
+
+ #pylab.plot(self.controls_time, self.ia_pred, 'r*', label='ia_pred')
+ #pylab.plot(self.controls_time, self.ib_pred, 'g*', label='ib_pred')
+ #pylab.plot(self.controls_time, self.ic_pred, 'b*', label='ic_pred')
+ pylab.plot(self.time, self.isensea, 'r:', label='ia_sense')
+ pylab.plot(self.time, self.isenseb, 'g:', label='ib_sense')
+ pylab.plot(self.time, self.isensec, 'b:', label='ic_sense')
+ pylab.plot(self.time, self.ia, 'r', label='ia')
+ pylab.plot(self.time, self.ib, 'g', label='ib')
+ pylab.plot(self.time, self.ic, 'b', label='ic')
+ pylab.plot(self.time, self.i_goal, label='i_goal')
+ if self.title is not None:
+ fig.canvas.set_window_title(self.title)
+ pylab.legend()
+
+ pylab.subplot(3, 1, 2)
+ pylab.plot(self.voltage_time, self.va, label='va')
+ pylab.plot(self.voltage_time, self.vb, label='vb')
+ pylab.plot(self.voltage_time, self.vc, label='vc')
+ pylab.plot(self.time, self.van, label='van')
+ pylab.plot(self.time, self.vbn, label='vbn')
+ pylab.plot(self.time, self.vcn, label='vcn')
+ pylab.plot(self.controls_time, self.ea, label='ea')
+ pylab.plot(self.controls_time, self.eb, label='eb')
+ pylab.plot(self.controls_time, self.ec, label='ec')
+ pylab.legend()
+
+ pylab.subplot(3, 1, 3)
+ pylab.plot(self.time, self.theta, label='theta')
+ pylab.plot(self.time, self.omega, label='omega')
+ #pylab.plot(self.controls_time, self.estimated_velocity, label='estimated omega')
+
+ pylab.legend()
+
+ fig = pylab.figure()
+ pylab.plot(self.controls_time,
+ map(operator.sub, self.ia_goal, self.ia_controls), 'r', label='ia_error')
+ pylab.plot(self.controls_time,
+ map(operator.sub, self.ib_goal, self.ib_controls), 'g', label='ib_error')
+ pylab.plot(self.controls_time,
+ map(operator.sub, self.ic_goal, self.ic_controls), 'b', label='ic_error')
+ if self.title is not None:
+ fig.canvas.set_window_title(self.title)
+ pylab.legend()
+ pylab.show()
+
+
+# So, from running a bunch of math, we know the following:
+# Van + Vbn + Vcn = 0
+# ia + ib + ic = 0
+# ea + eb + ec = 0
+# d ia/dt + d ib/dt + d ic/dt = 0
+#
+# We also have:
+# [ Van ] [ 2/3 -1/3 -1/3] [Va]
+# [ Vbn ] = [ -1/3 2/3 -1/3] [Vb]
+# [ Vcn ] [ -1/3 -1/3 2/3] [Vc]
+#
+# or,
+#
+# Vabcn = Vconv * V
+#
+# The base equation is:
+#
+# [ Van ] [ R 0 0 ] [ ia ] [ L M M ] [ dia/dt ] [ ea ]
+# [ Vbn ] = [ 0 R 0 ] [ ib ] + [ M L M ] [ dib/dt ] + [ eb ]
+# [ Vbn ] [ 0 0 R ] [ ic ] [ M M L ] [ dic/dt ] [ ec ]
+#
+# or
+#
+# Vabcn = R_matrix * I + L_matrix * I_dot + E
+#
+# We can re-arrange this as:
+#
+# inv(L_matrix) * (Vconv * V - E - R_matrix * I) = I_dot
+# B * V - inv(L_matrix) * E - A * I = I_dot
+class Simulation(object):
+ def __init__(self):
+ self.R_matrix = numpy.matrix(numpy.eye(3)) * R
+ self.L_matrix = numpy.matrix([[L, M, M], [M, L, M], [M, M, L]])
+ self.L_matrix_inv = numpy.linalg.inv(self.L_matrix)
+ self.A = self.L_matrix_inv * self.R_matrix
+ self.B = self.L_matrix_inv * Vconv
+ self.A_discrete, self.B_discrete = controls.c2d(-self.A, self.B, 1.0 / hz)
+ self.B_discrete_inverse = numpy.matrix(numpy.eye(3)) / (self.B_discrete[0, 0] - self.B_discrete[1, 0])
+
+ self.R_model = R * 1.0
+ self.L_model = L * 1.0
+ self.M_model = M * 1.0
+ self.R_matrix_model = numpy.matrix(numpy.eye(3)) * self.R_model
+ self.L_matrix_model = numpy.matrix([[self.L_model, self.M_model, self.M_model],
+ [self.M_model, self.L_model, self.M_model],
+ [self.M_model, self.M_model, self.L_model]])
+ self.L_matrix_inv_model = numpy.linalg.inv(self.L_matrix_model)
+ self.A_model = self.L_matrix_inv_model * self.R_matrix_model
+ self.B_model = self.L_matrix_inv_model * Vconv
+ self.A_discrete_model, self.B_discrete_model = \
+ controls.c2d(-self.A_model, self.B_model, 1.0 / hz)
+ self.B_discrete_inverse_model = numpy.matrix(numpy.eye(3)) / (self.B_discrete_model[0, 0] - self.B_discrete_model[1, 0])
+
+ print 'constexpr double kL = %g;' % self.L_model
+ print 'constexpr double kM = %g;' % self.M_model
+ print 'constexpr double kR = %g;' % self.R_model
+ print 'constexpr float kAdiscrete_diagonal = %gf;' % self.A_discrete_model[0, 0]
+ print 'constexpr float kAdiscrete_offdiagonal = %gf;' % self.A_discrete_model[1, 0]
+ print 'constexpr float kBdiscrete_inv_diagonal = %gf;' % self.B_discrete_inverse_model[0, 0]
+ print 'constexpr float kBdiscrete_inv_offdiagonal = %gf;' % self.B_discrete_inverse_model[1, 0]
+ print 'constexpr double kOneAmpScalar = %g;' % one_amp_scalar
+ print 'constexpr double kMaxOneAmpDrivingVoltage = %g;' % max_one_amp_driving_voltage
+ print('A_discrete', self.A_discrete)
+ print('B_discrete', self.B_discrete)
+ print('B_discrete_sub', numpy.linalg.inv(self.B_discrete[0:2, 0:2]))
+ print('B_discrete_inv', self.B_discrete_inverse)
+
+ # Xdot[5:, :] = (R_sense2 + R_sense1) / R_sense2 * (
+ # (1.0 / (R_sense1 * C_sense)) * (-Isense * R_sense2 / (R_sense1 + R_sense2) * (R_sense1 / R_sense2 + 1.0) + I))
+ self.mk1 = (R_sense2 + R_sense1) / R_sense2 * (1.0 / (R_sense1 * C_sense))
+ self.mk2 = -self.mk1 * R_sense2 / (R_sense1 + R_sense2) * (R_sense1 / R_sense2 + 1.0)
+
+ # ia, ib, ic, theta, omega, isensea, isenseb, isensec
+ self.X = numpy.matrix([[0.0], [0.0], [0.0], [-2.0 * numpy.pi / 3.0], [0.0], [0.0], [0.0], [0.0]])
+
+ self.K = 0.05 * Vconv
+ print('A %s' % repr(self.A))
+ print('B %s' % repr(self.B))
+ print('K %s' % repr(self.K))
+
+ print('System poles are %s' % repr(numpy.linalg.eig(self.A)[0]))
+ print('Poles are %s' % repr(numpy.linalg.eig(self.A - self.B * self.K)[0]))
+
+ controllability = controls.ctrb(self.A, self.B)
+ print('Rank of augmented controlability matrix. %d' % numpy.linalg.matrix_rank(
+ controllability))
+
+ self.data_logger = DataLogger(switching_pattern)
+ self.current_time = 0.0
+
+ self.estimated_velocity = self.X[4, 0]
+
+ def motor_diffeq(self, x, t, U):
+ I = numpy.matrix(x[0:3]).T
+ theta = x[3]
+ omega = x[4]
+ Isense = numpy.matrix(x[5:]).T
+
+ dflux = phases(f_single, theta) / Kv
+
+ Xdot = numpy.matrix(numpy.zeros((8, 1)))
+ di_dt = -self.A_model * I + self.B_model * U - self.L_matrix_inv_model * dflux * omega
+ torque = I.T * dflux
+ Xdot[0:3, :] = di_dt
+ Xdot[3, :] = omega
+ Xdot[4, :] = torque / J
+
+ Xdot[5:, :] = self.mk1 * I + self.mk2 * Isense
+ return numpy.squeeze(numpy.asarray(Xdot))
+
+ def DoControls(self, goal_current):
+ theta = self.X[3, 0]
+ # Use the actual angular velocity.
+ omega = self.X[4, 0]
+
+ measured_current = self.X[5:, :].copy()
+
+ # Ok, lets now fake it.
+ E_imag1 = numpy.exp(1j * theta) * K1 * numpy.matrix(
+ [[-1j],
+ [-1j * numpy.exp(1j * numpy.pi * 2.0 / 3.0)],
+ [-1j * numpy.exp(-1j * numpy.pi * 2.0 / 3.0)]])
+ E_imag2 = numpy.exp(1j * 5.0 * theta) * K2 * numpy.matrix(
+ [[-1j],
+ [-1j * numpy.exp(-1j * numpy.pi * 2.0 / 3.0)],
+ [-1j * numpy.exp(1j * numpy.pi * 2.0 / 3.0)]])
+
+ overall_measured_current = ((E_imag1 + E_imag2).real.T * measured_current / one_amp_scalar)[0, 0]
+
+ current_error = goal_current - overall_measured_current
+ #print(current_error)
+ self.estimated_velocity += current_error * 1.0
+ omega = self.estimated_velocity
+
+ # Now, apply the transfer function of the inductor.
+ # Use that to difference the current across the cycle.
+ Icurrent = self.Ilast
+ # No history:
+ #Icurrent = phases(g_single, theta) * goal_current
+ Inext = phases(g_single, theta + omega * 1.0 / hz) * goal_current
+
+ deltaI = Inext - Icurrent
+
+ H1 = -numpy.linalg.inv(1j * omega * self.L_matrix + self.R_matrix) * omega / Kv
+ H2 = -numpy.linalg.inv(1j * omega * 5.0 * self.L_matrix + self.R_matrix) * omega / Kv
+ p_imag = H1 * E_imag1 + H2 * E_imag2
+ p_next_imag = numpy.exp(1j * omega * 1.0 / hz) * H1 * E_imag1 + \
+ numpy.exp(1j * omega * 5.0 * 1.0 / hz) * H2 * E_imag2
+ p = p_imag.real
+
+ # So, we now know how much the change in current is due to changes in BEMF.
+ # Subtract that, and then run the stock statespace equation.
+ Vn_ff = self.B_discrete_inverse * (Inext - self.A_discrete * (Icurrent - p) - p_next_imag.real)
+ print 'Vn_ff', Vn_ff
+ print 'Inext', Inext
+ Vn = Vn_ff + self.K * (Icurrent - measured_current)
+
+ E = phases(f_single, self.X[3, 0]) / Kv * self.X[4, 0]
+ self.data_logger.log_controls(self.current_time, measured_current, Icurrent, E, self.estimated_velocity)
+
+ self.Ilast = Inext
+
+ return Vn
+
+ def Simulate(self):
+ start_wall_time = time.time()
+ self.Ilast = numpy.matrix(numpy.zeros((3, 1)))
+ for n in range(200):
+ goal_current = 1.0
+ max_current = (vcc - (self.X[4, 0] / Kv * 2.0)) / max_one_amp_driving_voltage
+ min_current = (-vcc - (self.X[4, 0] / Kv * 2.0)) / max_one_amp_driving_voltage
+ goal_current = max(min_current, min(max_current, goal_current))
+
+ Vn = self.DoControls(goal_current)
+
+ #Vn = numpy.matrix([[1.00], [0.0], [0.0]])
+ Vn = numpy.matrix([[0.00], [1.00], [0.0]])
+ #Vn = numpy.matrix([[0.00], [0.0], [1.00]])
+
+ # T is the fractional rate.
+ T = Vn / vcc
+ tn = -numpy.min(T)
+ T += tn
+ if (T > 1.0).any():
+ T = T / numpy.max(T)
+
+ for t, U in sample_times(T = T,
+ dt = 1.0 / hz, n = 10,
+ start_time = self.current_time):
+ # Analog amplifier mode!
+ #U = Vn
+
+ self.data_logger.log_data(self.X, (U - min(U)), self.current_time, Vn, goal_current)
+ t_array = numpy.array([self.current_time, t])
+ self.X = numpy.matrix(scipy.integrate.odeint(
+ self.motor_diffeq,
+ numpy.squeeze(numpy.asarray(self.X)),
+ t_array, args=(U,)))[1, :].T
+
+ self.current_time = t
+
+ print 'Took %f to simulate' % (time.time() - start_wall_time)
+
+ self.data_logger.plot()
+
+simulation = Simulation()
+simulation.Simulate()
diff --git a/tools/cpp/CROSSTOOL b/tools/cpp/CROSSTOOL
index 6e0c158..bda3e1b 100644
--- a/tools/cpp/CROSSTOOL
+++ b/tools/cpp/CROSSTOOL
@@ -1013,7 +1013,7 @@
}
flag_group {
iterate_over: 'system_include_paths'
- flag: '-iquote'
+ flag: '-I'
flag: '%{system_include_paths}'
}
}
diff --git a/y2017/control_loops/drivetrain/BUILD b/y2017/control_loops/drivetrain/BUILD
index c44da31..8651320 100644
--- a/y2017/control_loops/drivetrain/BUILD
+++ b/y2017/control_loops/drivetrain/BUILD
@@ -1,6 +1,6 @@
package(default_visibility = ['//visibility:public'])
-load('/aos/build/queues', 'queue_library')
+load('//aos/build:queues.bzl', 'queue_library')
genrule(
name = 'genrule_drivetrain',