{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## EX 1:  Mass-spring damper system\n",
    "\n",
    "Let us consider mass-spring damper system\n",
    "![TP1_SDSystem.png](attachment:TP1_SDSystem.png)\n",
    "\n",
    "with the following system parameters:\n",
    "\n",
    "      mass  m = 1.0 kg\n",
    "\n",
    "      spring constant k = 5.0 N/m\n",
    "\n",
    "      damping constant $\\rho$ = 2 Ns/m\n",
    "\n",
    "\n",
    "Let us suppose that measured output of the system is a position of the mass and its velocity. Then the state space model of the system is the following:\n",
    "\n",
    "$$\\dot x = Ax + Bu$$\n",
    "$$ y = Cx$$\n",
    "\n",
    "where\n",
    "state vector $x = (p,v)$ (p - position, v - velocity), and state and control matrices are the following:\n",
    "$$ A = \\begin{pmatrix} 0&1\\\\ -\\frac{k}{m}&-\\frac{\\rho}{m}\\end{pmatrix},\\ B = \\begin{pmatrix} 0\\\\ \\frac{1}{m} \n",
    "\\end{pmatrix}$$\n",
    "\n",
    "## TODO \n",
    "    1. Disretise the system with sampling time T = 0.1s\n",
    "    2. Design a PID controller for a discrete system, which ensures that output y_k of the closed-loop system tracks $y_{ref,k} = 1.$ \n",
    "    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## EX 2: Inverted Pendulum\n",
    "\n",
    "The system in this example consists of an inverted pendulum mounted to a motorized cart. The inverted pendulum system is an example commonly found in control system textbooks and research literature. Its popularity derives in part from the fact that it is unstable without control, that is, the pendulum will simply fall over if the cart isn't moved to balance it. Additionally, the dynamics of the system are nonlinear. The objective of the control system is to balance the inverted pendulum by applying a force to the cart that the pendulum is attached to. A real-world example that relates directly to this inverted pendulum system is the attitude control of a booster rocket at takeoff.\n",
    "The system in this example consists of an inverted pendulum mounted to a motorized cart. The inverted pendulum system is an example commonly found in control system textbooks and research literature. Its popularity derives in part from the fact that it is unstable without control, that is, the pendulum will simply fall over if the cart isn't moved to balance it. Additionally, the dynamics of the system are nonlinear. The objective of the control system is to balance the inverted pendulum by applying a force to the cart that the pendulum is attached to. A real-world example that relates directly to this inverted pendulum system is the attitude control of a booster rocket at takeoff.\n",
    "\n",
    "![2024-01-23_13-32-20.png](attachment:2024-01-23_13-32-20.png)\n",
    "\n",
    "Let us consider the system with the following system parameters\n",
    "    \n",
    "    (M)       mass of the cart                         0.5 kg\n",
    "    \n",
    "    (m)       mass of the pendulum                     0.2 kg\n",
    "    \n",
    "    (l)       length to pendulum center of mass        0.3 m\n",
    "    \n",
    "    (b)       coefficient of friction for cart         0.1 N/m/sec\n",
    "    \n",
    "    (I)       mass moment of inertia of the pendulum   0.006 kg.m^2\n",
    "    \n",
    "    (F)       force applied to the cart\n",
    "    \n",
    "    (y)       cart position coordinate\n",
    "    \n",
    "    (theta)   angle between the pendulum and the vertical axis\n",
    "\n",
    "Inverted pendulum on the cart can be modeled as follows\n",
    "\n",
    "$$(M+m)\\ddot{y} + b\\dot{y} + ml\\ddot{\\theta}\\cos\\theta -ml\\dot\\theta^2\\sin(\\theta) = F$$\n",
    "\n",
    "$$ml\\cos(\\theta)\\ddot{y} + (I+ml^2)\\ddot{\\theta} - mgl\\sin\\theta = 0$$    \n",
    "\n",
    "Let $y_1 = \\dot{y}$ and $\\theta_1 = \\dot{\\theta}$\n",
    "\n",
    "Then linearalised model have the following form\n",
    "\n",
    "$$\\dot x = Ax + Bu$$\n",
    "\n",
    "where state vector $x = (y,y_1,\\theta,\\theta_1)'$, control vector $u=F$. \n",
    "\n",
    "$$\\left[\\begin{array}{c}\\dot{y} \\\\ \\dot{y1} \\\\ \\dot{\\theta} \\\\ \\dot{\\theta_1}\\end{array}\\right]=\n",
    "\\left[\\begin{array}{cccc}0 & 1 & 0 & 0 \\\\\n",
    "0 & \\frac{-\\left(I+m l^2\\right) b}{I(M+m)+M m l^2}& \\frac{-g m^2 l^2}{I(M+m)+M m l^2} & 0 \\\\\n",
    "0 & 0 & 0 & 1 \\\\\n",
    "0 & \\frac{m l b}{I(M+m)+M m l^2} & \\frac{m g l(M+m)}{I(M+m)+M m l^2} & 0\\end{array}\\right]\n",
    "\\left[\\begin{array}{c}y \\\\ y_1\\\\ \\theta \\\\ \\theta_1\\end{array}\\right]+\n",
    "\\left[\\begin{array}{c}0 \\\\ \\frac{I+m l^2}{I(M+m)+M m l^2} \\\\ 0\\\\ \\frac{-m l}{I(M+m)+M m l^2}\\end{array}\\right] u$$\n",
    "\n",
    "## TODO\n",
    "1. Use the following library to implement an MPC controller\n",
    "    https://github.com/forgi86/pyMPC/blob/master/README.md \n",
    "   that stabilize the system while ensuring that following state and input constraints are satisfied\n",
    "\n",
    "   $x_{min} = [-5,-100,-100,-100]\\leq x \\leq x_{max}= [5,100,100,100]$\n",
    "   \n",
    "   $u_{min} = -20 \\leq u\\leq u_{max}=20$\n",
    "\n",
    "2. Plot the closed-loop trajectory and the corresponding control input.\n",
    "   \n",
    "    \n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}