File content as of revision 1:33fdaabcdeda:

/*
Hear cold stand a discription

*/


#include "teach.h"


teach::teach(void)                          // constructor
{

    lowcount = 0, highcount = 0;            //sets startvalue
    data_vec.clear();
}

void teach::setstart(float posx,float posy,float posz, int time)
{
    lowcount = 0;

    temp.start = true;
    temp.end = false;
    temp.hi = highcount;
    temp.low = lowcount;
    temp.time = time;
    temp.pos[0] = posx;
    temp.pos[1] = posy;
    temp.pos[2] = posz;
    data_vec.push_back(temp);

    lowcount++;
    highcount++;

    contr_sice();
}

void teach::setpoint(float posx,float posy,float posz, int time)
{
    lowcount++;

    temp.start = false;
    temp.end = false;
    temp.hi = highcount;
    temp.low = lowcount;
    temp.time = time;
    temp.pos[0] = posx;
    temp.pos[1] = posy;
    temp.pos[2] = posz;
    data_vec.push_back(temp);

    contr_sice();
}

void teach::setend(float posx,float posy,float posz, int time)
{
    temp.start = false;
    temp.end = true;
    temp.hi = highcount;
    temp.low = lowcount;
    temp.time = time;
    temp.pos[0] = posx;
    temp.pos[1] = posy;
    temp.pos[2] = posz;
    data_vec.push_back(temp);

    lowcount = 0;
    highcount++;

    contr_sice();
}

void teach::save(void)
{
    //store data_vec to memory
}

void teach::stop(void)
{

}

void teach::process(void)
{
    int i=0, k=0, p=0, count=0,forward = 100000, backward = 100000;
    float tempsum[3] = {};
    int max = data_vec.size();

    for(i=0; i<=max; i++) {                                                     // avrage starting Point
        if(data_vec[i].start) {
            tempsum[0] += data_vec[i].pos[0];
            tempsum[1] += data_vec[i].pos[1];
            tempsum[2] += data_vec[i].pos[2];
            k++;
        }
    }
    tempsum[0] = tempsum[0]/k;
    tempsum[1] = tempsum[1]/k;
    tempsum[2] = tempsum[2]/k;

    // save startpoint to memory

    i = 0, k = 0, tempsum[0] = 0, tempsum[1] = 0, tempsum[2] = 0;               // reset counters

    for(i=0; i<=max; i++) {                                                     // avrage end Point
        if(data_vec[i].end) {
            tempsum[0] += data_vec[i].pos[0];
            tempsum[1] += data_vec[i].pos[1];
            tempsum[2] += data_vec[i].pos[2];
            k++;
        }
    }
    tempsum[0] = tempsum[0]/k;
    tempsum[1] = tempsum[1]/k;
    tempsum[2] = tempsum[2]/k;

    // save endpoint to memory

    i = 0, k = 0, tempsum[0] = 0, tempsum[1] = 0, tempsum[2] = 0;               // reset counters

    for(i=0; i<=max; i++) {                                                     // forward point number

        while(!data_vec[k].end) {
            count++;
            k++;
        }

        if(count<forward) {
            forward = count;
        }
        while(!data_vec[k].start) {
            k++;
        }
        i = k;
        count = 0;

    }

    k = 0;
    for(i=0; i<=max; i++) {                                                   // backward point number

        while(!data_vec[k].end) {
            k++;
        }

        while(!data_vec[k].start) {
            count++;
            k++;
        }
        if(count<backward) {
            backward = count;
        }
        i = k;
        count = 0;

    }

    k = 0;
    struct temp_arry forw[forward];                                         // temp array initialise and zerowing
    for(i=0; i<forward; i++) {
        forw[i].time=0;
        forw[i].pos[0]=0;
        forw[i].pos[1]=0;
        forw[i].pos[2]=0;
    }

    for(i=0; i<=max; i++) {                                                 // avrage forward points
        for(p=0; p<=forward; p++) {
            forw[p].time += data_vec[k].time;
            forw[p].pos[0] += data_vec[k].pos[0];
            forw[p].pos[1] += data_vec[k].pos[1];
            forw[p].pos[2] += data_vec[k].pos[2];
            k++;
        }

        while(!data_vec[k].start) {
            k++;
        }
        i = k;
        count++;

    }
    for(i=0; i<=forward; i++) {
        forw[i].time = (forw[i].time)/count;
        forw[i].pos[0] = (forw[i].pos[0])/count;
        forw[i].pos[1] = (forw[i].pos[1])/count;
        forw[i].pos[2] = (forw[i].pos[2])/count;
    }
    count = 0;
    k = 0, i = 0, p = 0;                                                    // reset counters
    
    // Save forward way to memory

    struct temp_arry backw[backward];                                       // temp array initialise and zerowing
    for(i=0; i<backward; i++) {
        backw[i].time=0;
        backw[i].pos[0]=0;
        backw[i].pos[1]=0;
        backw[i].pos[2]=0;
    }

    for(i=0; i<=max; i++) {                                                 // avrage backward points
    
        while(!data_vec[k].end) {
            k++;
        }

        for(p=0; p<=backward; p++) {
            backw[p].time += data_vec[k].time;
            backw[p].pos[0] += data_vec[k].pos[0];
            backw[p].pos[1] += data_vec[k].pos[1];
            backw[p].pos[2] += data_vec[k].pos[2];
            k++;
        }
        i = k;
        count++;
    }
    
    for(i=0; i<=backward; i++) {
        backw[i].time = (backw[i].time)/count;
        backw[i].pos[0] = (backw[i].pos[0])/count;
        backw[i].pos[0] = (backw[i].pos[0])/count;
        backw[i].pos[0] = (backw[i].pos[0])/count;
    }
    count = 0;
    k = 0, i = 0, p = 0;                                                       // reset counters
    
    // save backward way to memory

}

void teach::temp_store()
{

}

void teach::contr_sice()
{
    if((data_vec.size()*sizeof(posdata)) >= MAX_SIZE) {
        // fehlermeldung oder speichern bei zu grossem temp speicher
    }
}