jupyter-notebooks/aula08-controle-introducao.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "try:\n",
    "    import sim\n",
    "except:\n",
    "    print ('--------------------------------------------------------------')\n",
    "    print ('\"sim.py\" could not be imported. This means very probably that')\n",
    "    print ('either \"sim.py\" or the remoteApi library could not be found.')\n",
    "    print ('Make sure both are in the same folder as this file,')\n",
    "    print ('or appropriately adjust the file \"sim.py\"')\n",
    "    print ('--------------------------------------------------------------')\n",
    "    print ('')\n",
    "\n",
    "import time\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Bang-bang"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Program started\n",
      "Connected to remote API server\n",
      "0 [-11.069377899169922, 0.9948067665100098, 0.19417336583137512]\n",
      "Program ended\n"
     ]
    }
   ],
   "source": [
    "####################################################################################\n",
    "#                                                                                  #\n",
    "#                 LEMBRE-SE QUE A SIMULAÇÃO DEVE ESTAR EM EXECUÇÃO!                #\n",
    "#                                                                                  #\n",
    "####################################################################################\n",
    "\n",
    "print ('Program started')\n",
    "sim.simxFinish(-1) # just in case, close all opened connections\n",
    "clientID=sim.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to CoppeliaSim\n",
    "\n",
    "if clientID!=-1:\n",
    "    print ('Connected to remote API server')\n",
    "\n",
    "    robotname = 'Manta'\n",
    "    returnCode, robotHandle = sim.simxGetObjectHandle(clientID, robotname, sim.simx_opmode_oneshot_wait)    \n",
    "    \n",
    "    returnCode, steerHandle = sim.simxGetObjectHandle(clientID, 'steer_joint', sim.simx_opmode_oneshot_wait)\n",
    "    returnCode, motorHandle = sim.simxGetObjectHandle(clientID, 'motor_joint', sim.simx_opmode_oneshot_wait)\n",
    "    \n",
    "    motor_torque = 60\n",
    "    returnCode = sim.simxSetJointForce(clientID, motorHandle, motor_torque, sim.simx_opmode_oneshot_wait)\n",
    "    \n",
    "    motor_velocity = 12\n",
    "    returnCode = sim.simxSetJointTargetVelocity(clientID, motorHandle, motor_velocity, sim.simx_opmode_oneshot_wait)\n",
    "  \n",
    "    returnCode, robotPos = sim.simxGetObjectPosition(clientID, robotHandle, -1, sim.simx_opmode_oneshot_wait)\n",
    "    print(returnCode, robotPos)\n",
    "    \n",
    "    max_steer = np.deg2rad(10)\n",
    "    \n",
    "    # Lembrar de habilitar o 'Real-time mode'\n",
    "    t = 0\n",
    "    lastTime = time.time()\n",
    "    while t < 25:\n",
    "        \n",
    "        now = time.time()\n",
    "        dt = now - lastTime\n",
    "        \n",
    "        returnCode, robotPos = sim.simxGetObjectPosition(clientID, robotHandle, -1, sim.simx_opmode_streaming + 10)\n",
    "        #print(returnCode, robotPos)\n",
    "        \n",
    "        # Entrada de controle\n",
    "        u = max_steer\n",
    "        if robotPos[1] > 0:\n",
    "            u = -max_steer\n",
    "            \n",
    "        returnCode = sim.simxSetJointTargetPosition(clientID, steerHandle, u, sim.simx_opmode_streaming + 10)\n",
    "        \n",
    "        t = t + dt        \n",
    "        lastTime = now \n",
    "        \n",
    "\n",
    "    motor_velocity = 0\n",
    "    returnCode = sim.simxSetJointTargetVelocity(clientID, motorHandle, motor_velocity, sim.simx_opmode_oneshot_wait)\n",
    "\n",
    "    # Now close the connection to CoppeliaSim:\n",
    "    sim.simxFinish(clientID)\n",
    "else:\n",
    "    print ('Failed connecting to remote API server')\n",
    "    \n",
    "print ('Program ended')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# PID"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Program started\n",
      "Connected to remote API server\n",
      "0 [-11.039521217346191, 0.998039722442627, 0.19415727257728577]\n",
      "Program ended\n"
     ]
    }
   ],
   "source": [
    "####################################################################################\n",
    "#                                                                                  #\n",
    "#                 LEMBRE-SE QUE A SIMULAÇÃO DEVE ESTAR EM EXECUÇÃO!                #\n",
    "#                                                                                  #\n",
    "####################################################################################\n",
    "\n",
    "print ('Program started')\n",
    "sim.simxFinish(-1) # just in case, close all opened connections\n",
    "clientID=sim.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to CoppeliaSim\n",
    "\n",
    "if clientID!=-1:\n",
    "    print ('Connected to remote API server')\n",
    "\n",
    "    robotname = 'Manta'\n",
    "    returnCode, robotHandle = sim.simxGetObjectHandle(clientID, robotname, sim.simx_opmode_oneshot_wait)    \n",
    "    \n",
    "    returnCode, steerHandle = sim.simxGetObjectHandle(clientID, 'steer_joint', sim.simx_opmode_oneshot_wait)\n",
    "    returnCode, motorHandle = sim.simxGetObjectHandle(clientID, 'motor_joint', sim.simx_opmode_oneshot_wait)\n",
    "    \n",
    "    motor_torque = 60\n",
    "    returnCode = sim.simxSetJointForce(clientID, motorHandle, motor_torque, sim.simx_opmode_oneshot_wait)\n",
    "    \n",
    "    motor_velocity = 12\n",
    "    returnCode = sim.simxSetJointTargetVelocity(clientID, motorHandle, motor_velocity, sim.simx_opmode_oneshot_wait)\n",
    "  \n",
    "    returnCode, robotPos = sim.simxGetObjectPosition(clientID, robotHandle, -1, sim.simx_opmode_oneshot_wait)\n",
    "    print(returnCode, robotPos)\n",
    "    \n",
    "    max_steer = np.deg2rad(20)\n",
    "    \n",
    "    # Estado objetivo é y = 0\n",
    "    setpoint = 0\n",
    "    \n",
    "    error = 0\n",
    "    lastError = 0\n",
    "    lastTime = 0\n",
    "    cumError = 0\n",
    "    \n",
    "    # kp = .4, kd = .5, ki = .01\n",
    "    # Lembrar de habilitar o 'Real-time mode'\n",
    "    t = 0\n",
    "    lastTime = time.time()\n",
    "    while t < 25:\n",
    "        \n",
    "        time.sleep(0.005)\n",
    "        \n",
    "        returnCode, robotPos = sim.simxGetObjectPosition(clientID, robotHandle, -1, sim.simx_opmode_streaming + 10)\n",
    "        #print(returnCode, robotPos)\n",
    "        \n",
    "        now = time.time()\n",
    "        dt = now - lastTime\n",
    "            \n",
    "        # Error        \n",
    "        error = setpoint - robotPos[1]\n",
    "        dError = (error - lastError) / dt\n",
    "        cumError += error * dt\n",
    "               \n",
    "        # Proporcional\n",
    "        kp = .4\n",
    "        up = kp*error\n",
    "        \n",
    "        # Derivativo\n",
    "        kd = .5\n",
    "        ud = kd*dError\n",
    "        \n",
    "        # Integral\n",
    "        ki = .01\n",
    "        ui = ki*cumError\n",
    "    \n",
    "        # Controller\n",
    "        u = up + ud + ui\n",
    "    \n",
    "        # Limitando o valor para +/- max\n",
    "        u = max(min(u, max_steer), -max_steer)\n",
    "        \n",
    "        returnCode = sim.simxSetJointTargetPosition(clientID, steerHandle, u, sim.simx_opmode_streaming + 10)\n",
    "\n",
    "        t = t + dt        \n",
    "        lastTime = now \n",
    "        lastError = error\n",
    "\n",
    "    motor_velocity = 0\n",
    "    returnCode = sim.simxSetJointTargetVelocity(clientID, motorHandle, motor_velocity, sim.simx_opmode_oneshot_wait)\n",
    "\n",
    "    # Now close the connection to CoppeliaSim:\n",
    "    sim.simxFinish(clientID)\n",
    "else:\n",
    "    print ('Failed connecting to remote API server')\n",
    "    \n",
    "print ('Program ended')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Para praticar\n",
    "\n",
    "- Altere os ganhos e verifique o impacto de cada um no resultado final."
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "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.8.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}