File content as of revision 6:60f122986f29:

#include "omuni.h"
 
 
int8_t omuni::map(int8_t in, int8_t inMin, int8_t inMax, int8_t outMin, int8_t outMax)
{
    // check it's within the range
    if (inMin<inMax) {
        if (in <= inMin)
            return outMin;
        if (in >= inMax)
            return outMax;
    } else { // cope with input range being backwards.
        if (in >= inMin)
            return outMin;
        if (in <= inMax)
            return outMax;
    }
    // calculate how far into the range we are
    float scale = float(in-inMin)/float(inMax-inMin);
    // calculate the output.
    return int8_t(outMin + scale*float(outMax-outMin));
}
 
 
omuni::omuni(I2C* i2c_,int8_t addr1,int8_t addr2,int8_t addr3, double delta)
    :i2c(i2c_),m1(i2c_,addr1),m2(i2c_,addr2),m3(i2c_,addr3)
{
    RXData[0] = 'H';
    RXData[1] =  0 ;
    RXData[2] =  0 ;
    RXData[3] =  0 ;
    RXData[4] =  0 ;
}
void omuni::out(char rxdata[dataNum],float L)
{
    signed char x1 = map(rxdata[1],-90,90,-100,100);
    signed char y1 = map(rxdata[2],-90,90,-100,100);
    signed char x2 = map(rxdata[3],-90,90,-100,100);
    signed char y2 = map(rxdata[4],-90,90,-100,100);
    float r1 = pow(x1*x1+y1*y1,0.5)/100;
    float r2 = pow(x2*x2+y2*y2,0.5)/100;
    float sieta1 = atan2(double(x1),double(y1));
    float sieta2 = atan2(double(x2),double(y2));
    if(r2 < 0.1F)r2 = 0;
    float alpha = PI/2;
    float x_2 = cos(sieta2);
    float y_2 = sin(sieta2);
    float w = 0;
    if(r1 > 0.1F) {
        if(sieta1 > PI/3 && sieta1 < PI*2/3.0)
            w = PI/3;
        if((sieta1 < -PI/3 && sieta1 > -2*PI/3.0))
            w = -PI/3;
    }
    
    if(r2)
    {
        m1 = r2*float(sin(alpha)*-x2*0.01) + float(L*w);
        m2 = r2*float(sin(alpha+4.0/3*PI)*-x2 - cos(alpha+4.0/3*PI)*y2)*0.01 + float(L*w);
        m3 = r2*float(sin(alpha+2.0/3*PI)*-x2 - cos(alpha+2.0/3*PI)*y2)*0.01 + float(L*w);
        m1.run();
        m2.run();
        m3.run();
    }
    else
    {
        float spdt = 0.5f * float(L*w);
        m1.direct_controll(spdt);
        m2.direct_controll(spdt);
        m3.direct_controll(spdt);
    }
    //printf("%5.2f : %5.2f : %5.2f : %5.2f",(float)x2,(float)y2,sieta1*180/PI,r1);
    //printf("\n");
}