コード例 #1
0
ファイル: wheels.py プロジェクト: prophile/tudor-software
class WheelSystem(object):
    def __init__(self, robot):
        self.robot = robot
        self.ramp_time = 0.3
        self._wheels = {}
        self._forward_matrix = None
        self._back_matrix = None
        self._wheel_indices = None
        self._wheel_speeds = {}
        self._turn_factor_correction = None
        self._last_ramp_update = time.time()
        self.target = (0.0, 0.0, 0.0)
        self.ramp = True

    def add_wheel(self, n, distance, angle, radius, calibration = 1.0):
        overall_adjust = calibration / radius
        factor_advance = -sin(angle)
        factor_lateral = cos(angle)
        factor_turn = distance
        row = (factor_advance * overall_adjust,
               factor_lateral * overall_adjust,
               factor_turn * overall_adjust)
        self._wheels[n] = row
        self._forward_matrix = None
        self._back_matrix = None
        interpolator = Interpolator()
        interpolator.smooth_function = smooth_ease
        self._wheel_speeds[n] = interpolator

    def _calculate_forward_matrix(self):
        if self._forward_matrix is not None:
            return
        self._wheel_indices = {}
        rows = []
        for i, (n, row) in enumerate(self._wheels.iteritems()):
            rows.append(row)
            self._wheel_indices[i] = n
        self._forward_matrix = Matrix(rows)

    def _calculate_back_matrix(self):
        if self._back_matrix is not None:
            return
        self._calculate_forward_matrix()
        try:
            self._back_matrix = self._forward_matrix.pseudo_inverse()
        except NotImplemented:
            self._back_matrix = _NoInverse

    def _drive(self):
        self._calculate_forward_matrix()
        advancing, lateral, angular = self.target
        speeds = self._forward_matrix * (advancing, lateral, angular)
        for i, speed in enumerate(speeds):
            n = self._wheel_indices[i]
            speed_difference = abs(speed - self._wheel_speeds[n].value)
            if not self.ramp or speed_difference < 0.02:
                self._wheel_speeds[n].set(speed)
            else:
                ramp_time = self.ramp_time * speed_difference
                self._wheel_speeds[n].smooth(speed, self.ramp_time)

    def _adjust_turn(self, dt):
        if self._turn_factor_correction is not None:
            advancing, lateral, angular = self.target
            advancing_true, lateral_true, angular_true = self.current_motion
            self._turn_factor_correction += angular_true * dt
            sine, cosine = sin(self._turn_factor_correction), cos(self._turn_factor_correction)
            self.target = (advancing * cosine,
                           lateral * sine,
                           angular)

    def ramp_update(self):
        current_time = time.time()
        dt = current_time - self._last_ramp_update
        self._last_ramp_update = current_time
        self._adjust_turn(dt)
        self._drive()
        for n in self._wheels:
            speed = self._wheel_speeds[n].value
            output = speed * 100
            if output > 100:
                output = 100
            elif output < -100:
                output = -100
            self.robot.motors[n].target = output

    def _current_motion(self):
        self._calculate_back_matrix()
        if self._back_matrix is _NoInverse:
            return self.target # approximate it with the current target
        speeds = [0.0] * len(self._wheels)
        for i, n in self._wheel_indices.iteritems():
            speeds[i] = self._wheel_speeds[n].value
        calculated_components = self._back_matrix * speeds
        return (float(calculated_components[0]),
                float(calculated_components[1]),
                float(calculated_components[2]))

    current_motion = property(fget = _current_motion)

    def start_turn_adjust(self):
        self._turn_factor_correction = 0.0

    def stop_turn_adjust(self):
        self._turn_factor_correction = None
コード例 #2
0
A5c = Matrix([[1, 0], [-1, 0], [-15, 0], [2, 1]])
dec(A5c, "5c")

A6 = Matrix([[1, 1, 1], [2, 2, 2], [3, 3, 3], [1, 2, 15]])
dec(A6, "6")

f = open("in.txt", "r")
temp = Matrix([list(map(int, f.read().split()))])
temp = temp.transposed()

years = list(range(1949, 2013))
years = [1937] + years
years.pop(years.index(1956))
years.pop(years.index(1971))
years.pop(years.index(1972))
years.pop(years.index(1987))
years.pop(years.index(1958))

print("задача 4:")

const = Matrix([[1] for i in range(60)])
print("если климат стабилен:", const.pseudo_inverse() * temp)

lin = Matrix([years]).transposed().matrix
for i in range(len(lin)):
    lin[i] += [1]
lin = Matrix(lin)
res = lin.pseudo_inverse() * temp
print("если учитывать потепление:", "{:0.4f}".format(res[0][0] * 2015 + res[1][0]))