<commit_before>//----------------------------------------------------------------------------
// ACI-32020 Automaation reaaliaikajärjestelmät, syksy 2011
//
//         Harjoitustyö: Mikrosysteemitekniikan toimilaiteohjaus
//
// Vastaava assistentti: <NAME>, <EMAIL>, huone sd114
//              Ryhmä 8: <NAME> (189637), <NAME> (198905)
//
//----------------------------------------------------------------------------
//
//  RealtimeController -luokan toteutus
//
//----------------------------------------------------------------------------

#include "realtimecontroller.h"
#include "piezoactuator.h"
#include "piezosensor.h"
#include "hysteresissingleton.h"
#include "scopedmutex.h"
#include "qglobal.h"
#include <QTimer>
#include <QDebug>
#include <sys/mman.h>
#include <malloc.h>
#include <fcntl.h>
#include <sys/types.h>
#include <string.h>
#include <stdio.h>
#include <errno.h>
// xenomai includes
#include <native/task.h>
#include <native/pipe.h>

RT_TASK task_desc;
RT_PIPE pipe_desc;
int pipefd = 0;

int pid(int setValue, int measurement,  int kp, int ki, int kd)
{
    // muuten sama toteutus PID-säätimen algoritmille kuin
    // kantaluokassa mutta jakson aika T on parempi 2000Hz reaaliaika säikeelle
    // ja kp,ki ja kd parametreja on skaalattu vielä 100x pienemmiksi

    // PID-säätimen tilaa kuvaavat muuttujat
    static int u[3];
    static int e[3];
    static int k[3];

    const double T = 1/50.0 * 100.0;

    kp = kp / 10000.0;
    ki = ki / 100.0;
    kd = kd / 10000.0;

    e[0] = setValue - measurement;

    k[0] = kp + ki * T/2.0 + 2.0*kd/T;
    k[1] = ki*T -  4.0*kd/T;
    k[2] = -kp + ki*T/2.0 + 2.0*kd/T;

    u[0] = k[0] * e[0] +
               k[1] * e[1] +
               k[2] * e[2] + u[2];

    if( u[0] > 5.0 )
    {
        u[0] = 5.0;
    }
    else if( u[0] < -5.0 )
    {
        u[0] = -5.0;
    }

    e[2] = e[1];
    e[1] = e[0];

    u[2] = u[1];
    u[1] = u[0];

    qDebug()<<"PID-Debug:";
    qDebug()<<"S M E: " + QString::number(setValue) +" "+ QString::number(measurement)+" "+
      QString::number(setValue - measurement);
    qDebug()<<"P I D: " + QString::number(kp)+" "+ QString::number(ki)+" "+QString::number(kd);
    qDebug()<<"K     :" + QString::number(k[0])+" "+ QString::number(k[1])+" "+ QString::number(k[2]);
    qDebug()<<"E     :" + QString::number(e[0])+" "+ QString::number(e[1])+" "+ QString::number(e[2]);
    qDebug()<<"U     :" + QString::number(u[0])+" "+ QString::number(u[1])+" "+ QString::number(u[2]);

    return u[0];
}

void realtimeLoop(void *arg)
{
    Q_UNUSED(arg);
    int xenoRet = 0;
    char buf[7];
    int bufValue[1];
    int kp,ki,kd;
    int setValue = 0;
    int actuatorVoltage = 0;
    int actuatorVoltageToSend = 0;
    int sensorVoltage = 0;
    int sensorVoltageToSend = 0;
    int sensorValue = 0;
    int actuatorScaleFactor = 1;
    int sensorScaleFactor = 1;
    ControllerInterface::Mode mode = ControllerInterface::MODE_MANUAL;
    ControllerInterface::Mode modeToSend;
    ControllerInterface::State state = ControllerInterface::STOPPED;
    ControllerInterface::State stateToSend;
    int hysteresisAnalysisRunning = 0;
    int output = 0;
    PiezoSensor* sensor;
    PiezoActuator* actuator;

    // varataan muisti anturille ja alustetaa se
    sensor = new PiezoSensor();
    sensor->init();

    // varataan muisti toimilaitteelle ja alustetaan se
    actuator = new PiezoActuator();
    actuator->init();

    // Xenomai laajennoksen ajurin alustukset

    // määritetään jaksonaika 500us eli 500000ns
    RTIME interval = 500000;

    // määritetään taskia kutsuttavaksi jaksonajan välein
    xenoRet = rt_task_set_periodic(NULL, TM_NOW, interval);

    while(1)
    {
        // ajastaa silmukan kulkemaan määritetyissä jaksonajoissa
        if(rt_task_wait_period(NULL) < 0)
            qDebug("task error in realtime loop");

        if(state == ControllerInterface::STARTED)
        {
            sensor->getValue(sensorValue);
            sensorVoltage = -sensorValue * sensorScaleFactor;

            // normitilanteessa tarkastetaan ajetaanko PID-säätöä vai suoraa ohjausta
            if (!hysteresisAnalysisRunning)
            {
                if( mode == ControllerInterface::MODE_MANUAL)
                {
                    // suorassa jänniteohjauksessa laitetaan suoraan käyttäjän syöttämä skaalattu arvo
                    // ulostuloon
                    output = actuatorVoltage;
                }
                else
                {
                    // PID-säädössä ajetaan algoritmi ja asetetaan ulostulo
                    output = pid(setValue, sensorVoltage, kp, ki, kd) *
                            actuatorScaleFactor;
                }
            }
            // jos tehdään hystereesianalyysiä ei tehdä PID-säätöä tai suoraa ohjausta
            else
            {
                // TODO
                qDebug("not yet implemented");
            }
        }

        // asetetaan ulostulo oikeasti toimilaitteelle
        actuator->setValue(output);
        // qDebug()<< "Set " << output<< "to actuator";
<commit_msg>Fixed to RealtimeControllor<commit_after>//----------------------------------------------------------------------------
// ACI-32020 Automaation reaaliaikajärjestelmät, syksy 2011
//
//         Harjoitustyö: Mikrosysteemitekniikan toimilaiteohjaus
//
// Vastaava assistentti: <NAME>, <EMAIL>, huone sd114
//              Ryhmä 8: <NAME> (189637), <NAME> (198905)
//
//----------------------------------------------------------------------------
//
//  RealtimeController -luokan toteutus
//
//----------------------------------------------------------------------------

#include "realtimecontroller.h"
#include "piezoactuator.h"
#include "piezosensor.h"
#include "hysteresissingleton.h"
#include "scopedmutex.h"
#include "qglobal.h"
#include <QTimer>
#include <QDebug>
#include <sys/mman.h>
#include <malloc.h>
#include <fcntl.h>
#include <sys/types.h>
#include <string.h>
#include <stdio.h>
#include <errno.h>
// xenomai includes
#include <native/task.h>
#include <native/pipe.h>

RT_TASK task_desc;
RT_PIPE pipe_desc;
int pipefd = 0;

int pid(int setValue, int measurement,  int kp, int ki, int kd)
{
    // muuten sama toteutus PID-säätimen algoritmille kuin
    // kantaluokassa mutta jakson aika T on parempi 2000Hz reaaliaika säikeelle
    // ja kp,ki ja kd parametreja on skaalattu vielä 100x pienemmiksi

    // PID-säätimen tilaa kuvaavat muuttujat
    static int u[3];
    static int e[3];
    static int k[3];

    const double T = 1/50.0 * 100.0;

    kp = kp / 10000.0;
    ki = ki / 100.0;
    kd = kd / 10000.0;

    e[0] = setValue - measurement;

    k[0] = kp + ki * T/2.0 + 2.0*kd/T;
    k[1] = ki*T -  4.0*kd/T;
    k[2] = -kp + ki*T/2.0 + 2.0*kd/T;

    u[0] = k[0] * e[0] +
               k[1] * e[1] +
               k[2] * e[2] + u[2];

    if( u[0] > 5.0 )
    {
        u[0] = 5.0;
    }
    else if( u[0] < -5.0 )
    {
        u[0] = -5.0;
    }

    e[2] = e[1];
    e[1] = e[0];

    u[2] = u[1];
    u[1] = u[0];

    qDebug()<<"PID-Debug:";
    qDebug()<<"S M E: " + QString::number(setValue) +" "+ QString::number(measurement)+" "+
      QString::number(setValue - measurement);
    qDebug()<<"P I D: " + QString::number(kp)+" "+ QString::number(ki)+" "+QString::number(kd);
    qDebug()<<"K     :" + QString::number(k[0])+" "+ QString::number(k[1])+" "+ QString::number(k[2]);
    qDebug()<<"E     :" + QString::number(e[0])+" "+ QString::number(e[1])+" "+ QString::number(e[2]);
    qDebug()<<"U     :" + QString::number(u[0])+" "+ QString::number(u[1])+" "+ QString::number(u[2]);

    return u[0];
}

void realtimeLoop(void *arg)
{
    Q_UNUSED(arg);
    int xenoRet = 0;
    char buf[7];
    int bufValue[1];
    int kp = 0, ki = 0, kd = 0;
    int setValue = 0;
    int actuatorVoltage = 0;
    int actuatorVoltageToSend = 0;
    int sensorVoltage = 0;
    int sensorVoltageToSend = 0;
    int sensorValue = 0;
    int actuatorScaleFactor = 1;
    int sensorScaleFactor = 1;
    ControllerInterface::Mode mode = ControllerInterface::MODE_MANUAL;
    ControllerInterface::Mode modeToSend;
    ControllerInterface::State state = ControllerInterface::STOPPED;
    ControllerInterface::State stateToSend;
    int hysteresisAnalysisRunning = 0;
    int output = 0;
    PiezoSensor* sensor;
    PiezoActuator* actuator;

    // varataan muisti anturille ja alustetaa se
    sensor = new PiezoSensor();
    sensor->init();

    // varataan muisti toimilaitteelle ja alustetaan se
    actuator = new PiezoActuator();
    actuator->init();

    // Xenomai laajennoksen ajurin alustukset

    // määritetään jaksonaika 500us eli 500000ns
    RTIME interval = 500000;

    // määritetään taskia kutsuttavaksi jaksonajan välein
    xenoRet = rt_task_set_periodic(NULL, TM_NOW, interval);

    while(1)
    {
        // ajastaa silmukan kulkemaan määritetyissä jaksonajoissa
        if(rt_task_wait_period(NULL) < 0)
            qDebug("task error in realtime loop");

        if(state == ControllerInterface::STARTED)
        {
            sensor->getValue(sensorValue);
            sensorVoltage = sensorValue * sensorScaleFactor;

            // normitilanteessa tarkastetaan ajetaanko PID-säätöä vai suoraa ohjausta
            if (!hysteresisAnalysisRunning)
            {
                if( mode == ControllerInterface::MODE_MANUAL)
                {
                    // suorassa jänniteohjauksessa laitetaan suoraan käyttäjän syöttämä skaalattu arvo
                    // ulostuloon
                    output = actuatorVoltage;
                }
                else
                {
                    // PID-säädössä ajetaan algoritmi ja asetetaan ulostulo
                    output = pid(setValue, sensorVoltage, kp, ki, kd) *
                            actuatorScaleFactor;
                }
            }
            // jos tehdään hystereesianalyysiä ei tehdä PID-säätöä tai suoraa ohjausta
            else
            {
                // TODO
                qDebug("not yet implemented");
            }
        }

        // asetetaan ulostulo oikeasti toimilaitteelle
        actuator->setValue(output);
        // qDebug()<< "Set " << output<< "to actuator";

{'fraction_non_alphanumeric': 0.17445849260021562, 'fraction_numerical': 0.028128981672057237, 'mean_word_length': 1.9958896065766294, 'pattern_counts': {'":': 0, '<': 73, '<?xml version=': 0, '>': 43, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3270110', 'n_tokens_mistral': 3855, 'n_tokens_neox': 3535, 'n_words': 765}