frc971/control_loops/python/flywheel.py

from frc971.control_loops.python import control_loop
from frc971.control_loops.python import controls
import numpy

import matplotlib
import matplotlib.pyplot as plt

import glog


class FlywheelParams(object):

    def __init__(self,
                 name,
                 motor,
                 G,
                 J,
                 q_pos,
                 q_vel,
                 q_voltage,
                 r_pos,
                 controller_poles,
                 dt=0.00505):
        self.name = name
        self.motor = motor
        self.G = G
        self.J = J
        self.q_pos = q_pos
        self.q_vel = q_vel
        self.q_voltage = q_voltage
        self.r_pos = r_pos
        self.dt = dt
        self.controller_poles = controller_poles


class VelocityFlywheel(control_loop.HybridControlLoop):

    def __init__(self, params, name="Flywheel"):
        super(VelocityFlywheel, self).__init__(name=name)
        self.params = params
        # Set Motor
        self.motor = self.params.motor
        # Moment of inertia of the flywheel wheel in kg m^2
        self.J = self.params.J
        # Gear ratio
        self.G = self.params.G
        # Control loop time step
        self.dt = self.params.dt

        # State feedback matrices
        # [angular velocity]
        self.A_continuous = numpy.matrix([[
            -self.motor.Kt / self.motor.Kv /
            (self.J * self.G * self.G * self.motor.resistance)
        ]])
        self.B_continuous = numpy.matrix(
            [[self.motor.Kt / (self.J * self.G * self.motor.resistance)]])
        self.C = numpy.matrix([[1]])
        self.D = numpy.matrix([[0]])

        self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
                                                   self.B_continuous, self.dt)

        self.PlaceControllerPoles(self.params.controller_poles)

        # Generated controller not used.
        self.PlaceObserverPoles([0.3])

        self.U_max = numpy.matrix([[12.0]])
        self.U_min = numpy.matrix([[-12.0]])

        qff_vel = 8.0
        self.Qff = numpy.matrix([[1.0 / (qff_vel**2.0)]])

        self.Kff = controls.TwoStateFeedForwards(self.B, self.Qff)

        glog.debug('K: %s', str(self.K))
        glog.debug('Poles: %s',
                   str(numpy.linalg.eig(self.A - self.B * self.K)[0]))


class Flywheel(VelocityFlywheel):

    def __init__(self, params, name="Flywheel"):
        super(Flywheel, self).__init__(params, name=name)

        self.A_continuous_unaugmented = self.A_continuous
        self.B_continuous_unaugmented = self.B_continuous

        self.A_continuous = numpy.matrix(numpy.zeros((2, 2)))
        self.A_continuous[1:2, 1:2] = self.A_continuous_unaugmented
        self.A_continuous[0, 1] = 1

        self.B_continuous = numpy.matrix(numpy.zeros((2, 1)))
        self.B_continuous[1:2, 0] = self.B_continuous_unaugmented

        # State feedback matrices
        # [position, angular velocity]
        self.C = numpy.matrix([[1, 0]])
        self.D = numpy.matrix([[0]])

        self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
                                                   self.B_continuous, self.dt)

        rpl = 0.45
        ipl = 0.07
        self.PlaceObserverPoles([rpl + 1j * ipl, rpl - 1j * ipl])

        self.K_unaugmented = self.K
        self.K = numpy.matrix(numpy.zeros((1, 2)))
        self.K[0, 1:2] = self.K_unaugmented
        self.Kff_unaugmented = self.Kff
        self.Kff = numpy.matrix(numpy.zeros((1, 2)))
        self.Kff[0, 1:2] = self.Kff_unaugmented

        self.InitializeState()


class IntegralFlywheel(Flywheel):

    def __init__(self, params, name="IntegralFlywheel"):
        super(IntegralFlywheel, self).__init__(params, name=name)

        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[0:2, 2] = self.B_continuous_unaugmented

        self.B_continuous = numpy.matrix(numpy.zeros((3, 1)))
        self.B_continuous[0:2, 0] = self.B_continuous_unaugmented

        # states
        # [position, velocity, voltage_error]
        self.C_unaugmented = self.C
        self.C = numpy.matrix(numpy.zeros((1, 3)))
        self.C[0:1, 0:2] = self.C_unaugmented

        glog.debug('A_continuous %s' % str(self.A_continuous))
        glog.debug('B_continuous %s' % str(self.B_continuous))
        glog.debug('C %s' % str(self.C))

        self.A, self.B = self.ContinuousToDiscrete(self.A_continuous,
                                                   self.B_continuous, self.dt)

        glog.debug('A %s' % str(self.A))
        glog.debug('B %s' % str(self.B))

        q_pos = self.params.q_pos
        q_vel = self.params.q_vel
        q_voltage = self.params.q_voltage
        self.Q_continuous = numpy.matrix([[(q_pos**2.0), 0.0, 0.0],
                                          [0.0, (q_vel**2.0), 0.0],
                                          [0.0, 0.0, (q_voltage**2.0)]])

        r_pos = self.params.r_pos
        self.R_continuous = numpy.matrix([[(r_pos**2.0)]])

        _, _, self.Q, self.R = controls.kalmd(A_continuous=self.A_continuous,
                                              B_continuous=self.B_continuous,
                                              Q_continuous=self.Q_continuous,
                                              R_continuous=self.R_continuous,
                                              dt=self.dt)

        glog.debug('Q_discrete %s' % (str(self.Q)))
        glog.debug('R_discrete %s' % (str(self.R)))

        self.KalmanGain, self.P_steady_state = 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
        self.Kff_unaugmented = self.Kff
        self.Kff = numpy.matrix(numpy.zeros((1, 3)))
        self.Kff[0, 0:2] = self.Kff_unaugmented

        self.InitializeState()


def PlotSpinup(params, goal, iterations=400):
    """Runs the flywheel plant with an initial condition and goal.

    Args:
        flywheel: Flywheel object to use.
        goal: goal state.
        iterations: Number of timesteps to run the model for.
        controller_flywheel: Flywheel object to get K from, or None if we should
             use flywheel.
        observer_flywheel: Flywheel object to use for the observer, or None if we
            should use the actual state.
    """

    # Various lists for graphing things.
    t = []
    x = []
    v = []
    a = []
    x_hat = []
    u = []
    offset = []

    flywheel = Flywheel(params, params.name)
    controller_flywheel = IntegralFlywheel(params, params.name)
    observer_flywheel = IntegralFlywheel(params, params.name)
    vbat = 12.0

    if t:
        initial_t = t[-1] + flywheel.dt
    else:
        initial_t = 0

    for i in range(iterations):
        X_hat = flywheel.X

        if observer_flywheel is not None:
            X_hat = observer_flywheel.X_hat
            x_hat.append(observer_flywheel.X_hat[1, 0])

        ff_U = controller_flywheel.Kff * (goal - observer_flywheel.A * goal)

        U = controller_flywheel.K * (goal - X_hat) + ff_U
        U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
        x.append(flywheel.X[0, 0])

        if v:
            last_v = v[-1]
        else:
            last_v = 0

        v.append(flywheel.X[1, 0])
        a.append((v[-1] - last_v) / flywheel.dt)

        if observer_flywheel is not None:
            observer_flywheel.Y = flywheel.Y
            observer_flywheel.CorrectHybridObserver(U)
            offset.append(observer_flywheel.X_hat[2, 0])

        applied_U = U.copy()
        if i > 200:
            applied_U += 2
        flywheel.Update(applied_U)

        if observer_flywheel is not None:
            observer_flywheel.PredictHybridObserver(U, flywheel.dt)

        t.append(initial_t + i * flywheel.dt)
        u.append(U[0, 0])

    matplotlib.use("GTK3Agg")

    plt.subplot(3, 1, 1)
    plt.plot(t, v, label='x')
    plt.plot(t, x_hat, label='x_hat')
    plt.legend()

    plt.subplot(3, 1, 2)
    plt.plot(t, u, label='u')
    plt.plot(t, offset, label='voltage_offset')
    plt.legend()

    plt.subplot(3, 1, 3)
    plt.plot(t, a, label='a')
    plt.legend()

    plt.show()


def WriteFlywheel(params, plant_files, controller_files, namespace):
    """Writes out the constants for a flywheel to a file.

    Args:
      params: list of Flywheel Params, the
        parameters defining the system.
      plant_files: list of strings, the cc and h files for the plant.
      controller_files: list of strings, the cc and h files for the integral
        controller.
      namespaces: list of strings, the namespace list to use.
    """
    # Write the generated constants out to a file.
    flywheels = []
    integral_flywheels = []

    if type(params) is list:
        name = params[0].name
        for index, param in enumerate(params):
            flywheels.append(Flywheel(param, name=param.name + str(index)))
            integral_flywheels.append(
                IntegralFlywheel(param,
                                 name='Integral' + param.name + str(index)))
    else:
        name = params.name
        flywheels.append(Flywheel(params, params.name))
        integral_flywheels.append(
            IntegralFlywheel(params, name='Integral' + params.name))

    loop_writer = control_loop.ControlLoopWriter(name,
                                                 flywheels,
                                                 namespaces=namespace)
    loop_writer.AddConstant(
        control_loop.Constant('kOutputRatio', '%f', flywheels[0].G))
    loop_writer.AddConstant(
        control_loop.Constant('kFreeSpeed', '%f',
                              flywheels[0].motor.free_speed))
    loop_writer.AddConstant(
        control_loop.Constant('kBemf',
                              '%f',
                              flywheels[0].motor.Kv * flywheels[0].G,
                              comment="// Radians/sec / volt"))
    loop_writer.AddConstant(
        control_loop.Constant('kResistance',
                              '%f',
                              flywheels[0].motor.resistance,
                              comment="// Ohms"))
    loop_writer.Write(plant_files[0], plant_files[1])

    integral_loop_writer = control_loop.ControlLoopWriter(
        'Integral' + name,
        integral_flywheels,
        namespaces=namespace,
        plant_type='StateFeedbackHybridPlant',
        observer_type='HybridKalman')
    integral_loop_writer.Write(controller_files[0], controller_files[1])