twiddle_example.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "tp: 0.495 ti: 0.0013 td: 4.475 error: 0.02691177178\n",
      "tp: 0.505 ti: 0.0013 td: 4.374999999999999 error: 0.0215027481901\n",
      "tp: 0.516 ti: 0.0013 td: 4.265 error: 0.0145420384825\n",
      "tp: 0.5281 ti: 0.0013 td: 4.144 error: 0.00809229095749\n",
      "tp: 0.5414100000000001 ti: 0.0013 td: 4.0108999999999995 error: 0.000545630988112\n",
      "tp: 0.5414100000000001 ti: 0.0013 td: 4.0109 error: 0.000545630988109\n",
      "tp: 0.5414100000000002 ti: 0.0013 td: 4.0109 error: 0.000545630988112\n",
      "tp: 0.5414100000000002 ti: 0.0013 td: 4.0109 error: 0.000545630988112\n",
      "tp: 0.5414100000000001 ti: 0.0013 td: 4.0109 error: 0.000545630988109\n",
      "tp: 0.5414100000000002 ti: 0.0013 td: 4.0109 error: 0.000545630988112\n",
      "tp: 0.5414100000000003 ti: 0.0013 td: 4.0109 error: 0.000545630988112\n",
      "tp: 0.5414100000000002 ti: 0.0013 td: 4.0109 error: 0.000545630988112\n",
      "tp: 0.5414100000000002 ti: 0.0013 td: 4.0109 error: 0.000545630988112\n",
      "tp: 0.5414100000000002 ti: 0.0013 td: 4.0109 error: 0.000545630988112\n",
      "tp: 0.5414100000000001 ti: 0.0013 td: 4.0109 error: 0.000545630988109\n",
      "tp: 0.5414100000000002 ti: 0.0013 td: 4.0109 error: 0.000545630988112\n",
      "tp: 0.5414100000000002 ti: 0.0013 td: 4.0109 error: 0.000545630988112\n",
      "tp: 0.5414100000000002 ti: 0.0013000000000000002 td: 4.0109 error: 0.000545630988112\n",
      "tp: 0.5376884542407437 ti: 0.0013000000000000004 td: 4.0108999999999995 error: 0.000434692735318\n",
      "tp: 0.5376884542407439 ti: 0.0013000000000000004 td: 4.0108999999999995 error: 0.000434692735318\n",
      "tp: 0.5376884542407439 ti: 0.0013000000000000004 td: 4.0108999999999995 error: 0.000434692735318\n",
      "tp: 0.5376884542407437 ti: 0.0013000000000000006 td: 4.0108999999999995 error: 0.000434692735318\n",
      "tp: 0.5406727617850915 ti: 0.0013000000000000006 td: 4.0109 error: 0.000354134886904\n",
      "tp: 0.5406727617850915 ti: 0.0013000000000000006 td: 4.0109 error: 0.000354134886904\n",
      "tp: 0.5406727617850916 ti: 0.0013000000000000006 td: 4.0108999999999995 error: 0.000354134886906\n",
      "tp: 0.5380137437630778 ti: 0.0013000000000000004 td: 3.9930999620013945 error: 0.000341829507588\n",
      "tp: 0.5380137437630778 ti: 0.0013000000000000006 td: 3.993099962001395 error: 0.000341829507588\n",
      "tp: 0.5380137437630778 ti: 0.0013000000000000006 td: 4.0107219996200145 error: 0.00034072816051\n",
      "tp: 0.5403829288206922 ti: 0.0013000000000000008 td: 4.0107219996200145 error: 0.000285308076961\n",
      "tp: 0.540382928820692 ti: 0.0013000000000000008 td: 4.0107219996200145 error: 0.00028530807696\n",
      "tp: 0.5403829288206922 ti: 0.0013000000000000006 td: 4.0107219996200145 error: 0.000285308076961\n",
      "tp: 0.5382719849343578 ti: 0.0013000000000000006 td: 4.010721999620015 error: 0.000271843837868\n",
      "tp: 0.5382719849343577 ti: 0.0013000000000000004 td: 3.9980039988502822 error: 0.000221003979776\n",
      "tp: 0.5382719849343576 ti: 0.0013000000000000002 td: 3.9980039988502822 error: 0.000221003979776\n",
      "tp: 0.5382719849343576 ti: 0.0013000000000000002 td: 3.9980039988502822 error: 0.000221003979776\n",
      "tp: 0.5382719849343577 ti: 0.0013000000000000002 td: 4.009335737536115 error: 0.000217831007972\n",
      "tp: 0.539795474246564 ti: 0.0013000000000000002 td: 4.009335737536114 error: 0.000184668552766\n",
      "tp: 0.5397954742465639 ti: 0.0013000000000000002 td: 4.009335737536115 error: 0.000184668552767\n",
      "tp: 0.5397954742465639 ti: 0.0013000000000000002 td: 4.009335737536115 error: 0.000184668552767\n",
      "tp: 0.5384380452693881 ti: 0.0013000000000000002 td: 4.009335737536116 error: 0.000173687307564\n",
      "tp: 0.5384380452693881 ti: 0.0013000000000000004 td: 4.001157508409163 error: 0.000143135884807\n",
      "tp: 0.5384380452693881 ti: 0.0013000000000000004 td: 4.001157508409164 error: 0.000143135884809\n",
      "tp: 0.5384380452693881 ti: 0.0013000000000000002 td: 4.001157508409165 error: 0.000143135884809\n",
      "tp: 0.5384380452693881 ti: 0.0013000000000000002 td: 4.008444310561279 error: 0.000139037249264\n",
      "tp: 0.5394177153365057 ti: 0.0013000000000000002 td: 4.008444310561279 error: 0.000119777268331\n",
      "tp: 0.5394177153365056 ti: 0.0013000000000000002 td: 4.008444310561279 error: 0.00011977726833\n",
      "tp: 0.5394177153365057 ti: 0.0013000000000000002 td: 4.00844431056128 error: 0.00011977726833\n",
      "tp: 0.538544829306704 ti: 0.0013 td: 4.00844431056128 error: 0.000110714130442\n",
      "tp: 0.538544829306704 ti: 0.0013 td: 4.0031853525800765 error: 9.29149598435e-05\n",
      "tp: 0.5385448293067041 ti: 0.0013 td: 4.0031853525800765 error: 9.29149598435e-05\n",
      "tp: 0.5385448293067041 ti: 0.0012999999999999997 td: 4.0031853525800765 error: 9.29149598436e-05\n",
      "tp: 0.5378448619994061 ti: 0.0012999999999999997 td: 3.9984996210188255 error: 8.93877497458e-05\n",
      "tp: 0.5378448619994061 ti: 0.0012999999999999995 td: 3.998499621018826 error: 8.93877497454e-05\n",
      "tp: 0.5378448619994061 ti: 0.0012999999999999995 td: 3.998499621018826 error: 8.93877497454e-05\n",
      "tp: 0.5372211911286036 ti: 0.0012999999999999995 td: 3.998499621018826 error: 7.56105299516e-05\n",
      "tp: 0.5372211911286034 ti: 0.0012999999999999995 td: 3.9947421328798582 error: 7.01263101689e-05\n",
      "tp: 0.5372211911286036 ti: 0.0012999999999999997 td: 3.9947421328798582 error: 7.01263101685e-05\n",
      "tp: 0.5372211911286037 ti: 0.0012999999999999997 td: 3.9947421328798582 error: 7.01263101685e-05\n",
      "tp: 0.536721069457307 ti: 0.0012999999999999995 td: 3.994742132879858 error: 6.237529651e-05\n",
      "tp: 0.5367210694573071 ti: 0.0012999999999999995 td: 3.99172900314122 error: 5.45681387529e-05\n",
      "tp: 0.5367210694573072 ti: 0.0012999999999999997 td: 3.99172900314122 error: 5.45681387527e-05\n",
      "tp: 0.5367210694573071 ti: 0.0012999999999999995 td: 3.99172900314122 error: 5.45681387529e-05\n",
      "tp: 0.5363200218890943 ti: 0.0012999999999999995 td: 3.9917290031412205 error: 5.17730854995e-05\n",
      "tp: 0.5363200218890944 ti: 0.0012999999999999995 td: 3.989312774403807 error: 4.20589696804e-05\n",
      "tp: 0.5363200218890944 ti: 0.0012999999999999993 td: 3.9893127744038064 error: 4.20589696797e-05\n",
      "tp: 0.5363200218890946 ti: 0.0012999999999999995 td: 3.9893127744038064 error: 4.20589696795e-05\n",
      "tp: 0.5363200218890944 ti: 0.0012999999999999995 td: 3.991465634208842 error: 4.15429291279e-05\n",
      "tp: 0.5366094619295493 ti: 0.0012999999999999997 td: 3.991465634208842 error: 3.52419890278e-05\n",
      "tp: 0.5366094619295492 ti: 0.0012999999999999997 td: 3.9914656342088417 error: 3.52419890278e-05\n",
      "tp: 0.5366094619295492 ti: 0.0012999999999999997 td: 3.9914656342088417 error: 3.52419890278e-05\n",
      "tp: 0.5363515708535039 ti: 0.0013 td: 3.9914656342088417 error: 3.31719297947e-05\n",
      "tp: 0.5363515708535039 ti: 0.0013 td: 3.9899118937589493 error: 2.71888115955e-05\n",
      "tp: 0.5363515708535038 ti: 0.0013 td: 3.9899118937589493 error: 2.71888115955e-05\n",
      "tp: 0.5363515708535037 ti: 0.0013 td: 3.9899118937589493 error: 2.7188811596e-05\n",
      "tp: 0.5363515708535036 ti: 0.0013 td: 3.9912962764998032 error: 2.65964934251e-05\n",
      "tp: 0.5365376934219962 ti: 0.0013000000000000002 td: 3.9912962764998032 error: 2.28064720585e-05\n",
      "tp: 0.5365376934219962 ti: 0.0013000000000000004 td: 3.9912962764998032 error: 2.28064720584e-05\n",
      "tp: 0.5365376934219962 ti: 0.0013000000000000004 td: 3.9912962764998037 error: 2.28064720581e-05\n",
      "tp: 0.5363718582134692 ti: 0.0013000000000000006 td: 3.991296276499804 error: 2.12158704588e-05\n",
      "tp: 0.5363718582134692 ti: 0.00129973027839441 td: 3.991296276499804 error: 1.73513751473e-05\n",
      "tp: 0.5363718582134692 ti: 0.00129973027839441 td: 3.991296276499804 error: 1.73513751473e-05\n",
      "tp: 0.5363718582134692 ti: 0.0012997302783944098 td: 3.9921055660228046 error: 1.40723893942e-05\n",
      "tp: 0.5363718582134691 ti: 0.00129973027839441 td: 3.9921055660228046 error: 1.40723893944e-05\n",
      "tp: 0.536371858213469 ti: 0.00129973027839441 td: 3.9921055660228046 error: 1.40723893944e-05\n",
      "tp: 0.5363718582134689 ti: 0.001299535617614439 td: 3.9921055660228046 error: 1.3769399865e-05\n",
      "tp: 0.5362749134215087 ti: 0.001299535617614439 td: 3.992105566022804 error: 1.19653860941e-05\n",
      "tp: 0.5362749134215088 ti: 0.001299535617614439 td: 3.9915214936811596 error: 1.07316220934e-05\n",
      "tp: 0.5362749134215088 ti: 0.001299535617614439 td: 3.99152149368116 error: 1.07316220934e-05\n",
      "tp: 0.5362749134215089 ti: 0.001299535617614439 td: 3.99152149368116 error: 1.07316220943e-05\n",
      "tp: 0.536197173392836 ti: 0.001299535617614439 td: 3.99152149368116 error: 9.9078295889e-06\n",
      "tp: 0.536197173392836 ti: 0.0012994091778460778 td: 3.9915214936811605 error: 8.18279853511e-06\n",
      "tp: 0.536197173392836 ti: 0.0012994091778460776 td: 3.9915214936811605 error: 8.18279853503e-06\n",
      "tp: 0.5361971733928361 ti: 0.0012994091778460773 td: 3.99190087144588 error: 6.56657797293e-06\n",
      "tp: 0.5361971733928362 ti: 0.0012994091778460773 td: 3.99190087144588 error: 6.56657797365e-06\n",
      "tp: 0.5361971733928361 ti: 0.0012994091778460773 td: 3.9919008714458806 error: 6.56657797301e-06\n",
      "tp: 0.5361971733928361 ti: 0.0012993179250008532 td: 3.99190087144588 error: 6.49124272511e-06\n",
      "tp: 0.5361517277304004 ti: 0.0012993179250008532 td: 3.9919008714458797 error: 5.58406686835e-06\n",
      "tp: 0.5361517277304004 ti: 0.001299317925000853 td: 3.991627070719304 error: 5.0646832723e-06\n",
      "tp: 0.5361517277304003 ti: 0.0012993179250008532 td: 3.991627070719304 error: 5.06468327169e-06\n",
      "tp: 0.5361517277304003 ti: 0.001299317925000853 td: 3.991627070719304 error: 5.06468327175e-06\n",
      "best parameters: [0.5361152848536931, 0.001299317925000853, 3.9916270707193036]\n",
      "(dp): [4.0087164377953636e-05, 5.927228183404502e-08, 0.0002195608026410836]\n",
      "last error: 4.07680854707e-06\n",
      "average error of last 100 steps: 4.61910604572e-06\n",
      "average error 0.563265292048\n"
     ]
    }
   ],
   "source": [
    "from simple_pid import *\n",
    "\n",
    "p = [0.495, 0.0013, 4.475]\n",
    "dp = [0.01, 0.001, 0.1]\n",
    "big_factor = 1.1\n",
    "small_factor = 0.9\n",
    "N = 100 # number of iterations to perform twiddle\n",
    "func = follow_line\n",
    "n = 300\n",
    "settings = {'tolerance': 0.001, \n",
    "            'length': 20.,\n",
    "            'max_steering': pi / 4.,\n",
    "            'steering_drift': 10. * pi / 180.,\n",
    "            'steering_noise': 0., #adding noise introduces randomness to error \n",
    "            'speed_noise': 0.}\n",
    "position = {'x': 0., 'y': 10., 'theta': 0.}\n",
    "\n",
    "twiddle(p, dp, big_factor, small_factor, N, func, n, settings, position)"
   ]
  }
 ],
 "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.5.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}