64 lines
1.3 KiB
Python
64 lines
1.3 KiB
Python
import numpy as np
|
|
import matplotlib.pyplot as plt
|
|
import math
|
|
|
|
from mpc_func import MpcController
|
|
# from simulator_func import FirstOrderSystem
|
|
from control import matlab
|
|
|
|
def main():
|
|
dt = 0.01
|
|
simulation_time = 100 # in seconds
|
|
iteration_num = int(simulation_time / dt)
|
|
|
|
# you must be care about this matrix
|
|
# these A and B are for continuos system if you want to use discret system matrix please skip this step
|
|
tau = 0.53
|
|
A = np.array([[1./tau, 0., 0., 0.],
|
|
[0., 1./tau, 0., 0.],
|
|
[1., 0., 0., 0.],
|
|
[1., 0., 0., 0.]])
|
|
B = np.array([[1./tau, 0.],
|
|
[0., 1./tau],
|
|
[0., 0.],
|
|
[0., 0.]])
|
|
|
|
C = np.eye(4)
|
|
D = np.zeros((4, 2))
|
|
|
|
# create system
|
|
sysc = matlab.ss(A, B, C, D)
|
|
# discrete system
|
|
sysd = matlab.c2d(sysc, dt)
|
|
|
|
Ad = sysd.A
|
|
Bd = sysd.B
|
|
|
|
Q = np.diag([1., 1., 1., 1.])
|
|
R = np.diag([1., 1.])
|
|
pre_step = 3
|
|
|
|
# make controller
|
|
controller = MpcController(Ad, Bd, Q, R, pre_step)
|
|
controller.initialize_controller()
|
|
|
|
# make simulator
|
|
# plant = FirstOrderSystem(tau)
|
|
|
|
"""
|
|
for i in range(iteration_num):
|
|
"""
|
|
|
|
# states = plant.states
|
|
# controller.calc_input
|
|
|
|
if __name__ == "__main__":
|
|
main()
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|