homeo2serial.h

gehe zur Dokumentation dieser Datei

#ifndef HOMEO2SERIAL_H
#define HOMEO2SERIAL_H

#define _POSIX_SOURCE 1 /* POSIX compliant source */
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>

#include <fstream>

#include "output.h"
#include "../microadam.h"
#include "../microeva.h"
#include "../../external/gnuplot/gnuplot.h"

extern "C"
{
  #include <opencv/highgui.h>
}

#define BAUDRATE B2400
#define FALSE 0
#define TRUE 1

namespace seemicro
{

class Homeo2Serial : public OutputModule
{
  Micro &micro;

  Gnuplot<std::string> gs;
  Gnuplot<std::string> gs01;

  static Homeo2Serial *home;

  static const char TBNAME_C[], TBNAME_C0[];

  struct termios oldtio;

  int serialFd;

  char buf[255];

  //float wt, wt1;

  float c, c0;

  float epsilon, freqFaktor;

  int iMittelAlpha, iMittelBeta, iEpsilon, ic, ic0, iGyroSkal, iFreqFaktor;

  float lastAlpha, deltaAlpha, deltaAlphaGemittelt, lastBeta, betaGemittelt;

  float gyro_skal;

public:
  Homeo2Serial(Micro& Am, string serialDevice = "/dev/ttyS0") :
    micro(Am)
  {
    home = this;

    // init homeokinesis parameters
    c = 1.2;
    c0 = 0;
    epsilon = 0.1;
    freqFaktor = 1;
    gyro_skal = 2;
    setTrackbarVariables();

    if(!gs.pipe || !gs01.pipe)
    {
      cerr << "Homeo2Serial: Failed gnuplot pipe initialization. Is gnuplot in your path?\n";
    }
    else
    {
      gs01.addChannel("deltaAlpha");
      gs01.addChannel("deltaAlphaGemittelt");
      gs.addChannel("c");
      gs.addChannel("c0");
      gs.addChannel("gyro_skal");
      gs.addChannel("freqFaktor");
      gs01.addChannel("mittelAlphaKoeff");
      gs01.addChannel("mittelBetaKoeff");
      gs01.addChannel("betaSkaliert");
      gs01.addChannel("mittelBeta");
      gs01.addChannel("epsilon");
      gs01.addChannel("toServo");
    }

    cvNamedWindow("Homeo2Serial", 1);

    iMittelAlpha = 500;
    cvCreateTrackbar("Mittelwertkoeffizient für alpha (0 .. 1)", "Homeo2Serial",
      &iMittelAlpha, 1000, callback);
    iMittelBeta = 500;
    cvCreateTrackbar("Mittelwertkoeffizient für beta (0 .. 1)", "Homeo2Serial",
      &iMittelBeta, 1000, callback);
    cvCreateTrackbar("Epsilon (1e-7 .. 1e3)", "Homeo2Serial", &iEpsilon, 1000, callback);
    cvCreateTrackbar(TBNAME_C, "Homeo2Serial", &ic, 2000, callback);
    cvCreateTrackbar(TBNAME_C0, "Homeo2Serial", &ic0, 2000, callback);
    cvCreateTrackbar("gyro_skal (-10 .. 10)", "Homeo2Serial", &iGyroSkal, 2000, callback);
    cvCreateTrackbar("freqFaktor (-100 bis 100)", "Homeo2Serial", &iFreqFaktor, 2000, callback);

    struct termios newtio;

    /* open the device */
    if(DEBUGLEVEL>=2) cerr << "Opening serial device: '" << serialDevice << "'\n";
    serialFd = open(serialDevice.c_str(), O_RDWR | O_SYNC);
    if(serialFd < 0)
    {
      perror(serialDevice.c_str());
      exit(-1);
    }

    /* save current port settings */
    tcgetattr(serialFd, &oldtio);

    /* set new port settings */
    newtio.c_cflag = BAUDRATE | CS8 | CLOCAL;
    newtio.c_iflag = IGNPAR ;
    newtio.c_oflag = 0;
    newtio.c_lflag = 0;
    newtio.c_cc[VMIN]=1;
    newtio.c_cc[VTIME]=0;
    tcsetattr(serialFd, TCSANOW, &newtio);
    tcflush(serialFd, TCIFLUSH);

    cvResizeWindow("Homeo2Serial", 300, 400);
  }

  ~Homeo2Serial()
  {
    cvDestroyWindow("Homeo2Serial");

    /* restore old port settings */
    if(DEBUGLEVEL>=2) cerr << "Resetting and closing serial port...";
    tcsetattr(serialFd, TCSANOW, &oldtio);
    close(serialFd);
    if(DEBUGLEVEL>=2) cerr << endl;
  }

  void setTrackbarVariables()
  {
    iEpsilon = int( (log(epsilon)/log(10.)+7)*100 );
    ic = int(c*10)+1000;
    ic0 = int(c0*10)+1000;
    iGyroSkal = int(gyro_skal*100)+1000;
    iFreqFaktor = int(freqFaktor*10)+1000;
  }

  void putTrackbarVariables()
  {
    // set variables from trackbars
    epsilon = pow(10., iEpsilon*0.01-7);
    c = (ic-1000)/10.;
    c0 = (ic0-1000)/10.;
    gyro_skal = (iGyroSkal-1000)/100.;
    freqFaktor = (iFreqFaktor-1000)/10.;
  }

  float homeokinesis(float gyro_value, float beta, float alpha)
  {
    static float tt = 0;


    gyro_value = gyro_value*4/M_PI;
    if(gyro_value>1) gyro_value=1;
    if(gyro_value<-1) gyro_value=-1;

//    cerr << "beta: " << beta << "gyro_value: " << gyro_value;

    float y = 1.2*tanh(c*beta+c0) + 0.01 * sin(tt);// Output des Neurons
    c = c - epsilon*(2*y*y*c-1);

    float epsQ = freqFaktor*epsilon*gyro_value*gyro_value+0.01;
    c0 = c0-(epsQ*y);

    float sig = gyro_value*gyro_skal;
    y += sig;

    if (y>1) y=1;
    if (y<-1) y=-1;

    tt++;
    return y;
  }

  void output(bool keyframe)
  {
    if(DEBUGLEVEL>2) cerr << "->Homeo2Serial::output\n";
    MicroAdam& adam = dynamic_cast<MicroAdam&>(micro);

    deltaAlpha = micro.alpha - lastAlpha;
    lastAlpha = micro.alpha;
    if(deltaAlpha>=M_PI) deltaAlpha -= 2*M_PI;
    if(deltaAlpha<=-M_PI) deltaAlpha += 2*M_PI;

    deltaAlphaGemittelt += iMittelAlpha*0.001*(deltaAlpha-deltaAlphaGemittelt);

    betaGemittelt += iMittelBeta*0.001*(adam.beta-betaGemittelt);

    float betaSkaliert = (betaGemittelt*180./M_PI/10.+0.25)/1.75;// -1 bis 1
    float toServo = homeokinesis(deltaAlphaGemittelt, betaSkaliert, micro.alpha);

    // serial output
    sprintf(buf, "%f\n", toServo);
    if(DEBUGLEVEL>=3)
      printf("\rdeltaAlpha=% 1.6f deltaAlphaGemittelt=% 1.4f toServo=% 1.4f",
        deltaAlpha, deltaAlphaGemittelt, toServo);
    write(serialFd, buf, strlen(buf));

    // gnuplot output
    if(gs.pipe)
    {
      gs.putData("c", c);
      gs.putData("c0", c0);
      gs.putData("gyro_skal", gyro_skal);
  //    gs.putData("freqFaktor", freqFaktor);
      gs.plot();
    }

    if(gs01.pipe)
    {
//      gs01.putData("deltaAlpha", deltaAlpha);
      gs01.putData("deltaAlphaGemittelt", deltaAlphaGemittelt);
      gs01.putData("mittelAlphaKoeff", iMittelAlpha/1000.);
      gs01.putData("mittelBetaKoeff", iMittelBeta/1000.);
//      gs01.putData("mittelBeta", betaGemittelt);
      gs01.putData("betaSkaliert", betaSkaliert);
      gs01.putData("epsilon", epsilon);
      gs01.putData("toServo", toServo);
      gs01.plot();
    }

    if(!keyframe) return;

    if(DEBUGLEVEL>=3) cerr << "Setting trackbars...";
    setTrackbarVariables();
    cvSetTrackbarPos(TBNAME_C, "Homeo2Serial", ic);
    cvSetTrackbarPos(TBNAME_C0, "Homeo2Serial", ic0);
    if(DEBUGLEVEL>=3) cerr << "OK\n";
  }

  static void callback(int dummy)
  {
    if(!home) return;
    float epsilonAlt = home->epsilon;
    float gyroSkalAlt = home->gyro_skal;
    float freqAlt = home->freqFaktor;

    home->putTrackbarVariables();

    if(DEBUGLEVEL>=1 && epsilonAlt != home->epsilon)
    {
      cout << "epsilon=" << home->epsilon << endl;
    }

    if(DEBUGLEVEL>=1 && gyroSkalAlt != home->gyro_skal)
    {
      cout << "gyro_skal=" << home->gyro_skal << endl;
    }

    if(DEBUGLEVEL>=1 && freqAlt != home->freqFaktor)
    {
      cout << "freqFaktor=" << home->freqFaktor << endl;
    }
  }

  bool loadParamsFromFile(string filename)
  {
    ifstream f(filename.c_str());
    if(!f.is_open()) return false;
    f >> c;
    f >> c0;
    f >> epsilon;
    f >> freqFaktor;
    f >> gyro_skal;
    f >> iMittelAlpha;
    f >> iMittelBeta;
    setTrackbarVariables();
    return true;// success
  }

  bool saveParamsToFile(string filename)
  {
    ofstream f(filename.c_str());
    if(!f.is_open()) return false;
    f << c << endl;
    f << c0 << endl;
    f << epsilon << endl;
    f << freqFaktor << endl;
    f << gyro_skal << endl;
    f << iMittelAlpha << endl;
    f << iMittelBeta << endl;
    return true;// success
  }

}; // Homeo2Serial

} // seemicro

#endif

---