omuni.cpp

00001 #include "omuni.h"
00002  
00003  
00004 int8_t omuni::map(int8_t in, int8_t inMin, int8_t inMax, int8_t outMin, int8_t outMax)
00005 {
00006     // check it's within the range
00007     if (inMin<inMax) {
00008         if (in <= inMin)
00009             return outMin;
00010         if (in >= inMax)
00011             return outMax;
00012     } else { // cope with input range being backwards.
00013         if (in >= inMin)
00014             return outMin;
00015         if (in <= inMax)
00016             return outMax;
00017     }
00018     // calculate how far into the range we are
00019     float scale = float(in-inMin)/float(inMax-inMin);
00020     // calculate the output.
00021     return int8_t(outMin + scale*float(outMax-outMin));
00022 }
00023  
00024  
00025 omuni::omuni(I2C* i2c_,int8_t addr1,int8_t addr2,int8_t addr3, double delta)
00026     :i2c(i2c_),m1(i2c_,addr1),m2(i2c_,addr2),m3(i2c_,addr3)
00027 {
00028     RXData[0] = 'H';
00029     RXData[1] =  0 ;
00030     RXData[2] =  0 ;
00031     RXData[3] =  0 ;
00032     RXData[4] =  0 ;
00033 }
00034 void omuni::out(char rxdata[dataNum],float L)
00035 {
00036     signed char x1 = map(rxdata[1],-90,90,-100,100);
00037     signed char y1 = map(rxdata[2],-90,90,-100,100);
00038     signed char x2 = map(rxdata[3],-90,90,-100,100);
00039     signed char y2 = map(rxdata[4],-90,90,-100,100);
00040     float r1 = pow(x1*x1+y1*y1,0.5)/100;
00041     float r2 = pow(x2*x2+y2*y2,0.5)/100;
00042     float sieta1 = atan2(double(x1),double(y1));
00043     float sieta2 = atan2(double(x2),double(y2));
00044     if(r2 < 0.1F)r2 = 0;
00045     float alpha = PI/2;
00046     float x_2 = cos(sieta2);
00047     float y_2 = sin(sieta2);
00048     float w = 0;
00049     w = PI/3*x1*0.01;
00050    
00051     
00052     //if(r2)
00053     //{
00054         m1 = r2*float(sin(alpha)*-x2*0.01) + float(L*w);
00055         m2 = r2*float(sin(alpha+4.0/3*PI)*-x2 - cos(alpha+4.0/3*PI)*y2)*0.01 + float(L*w);
00056         m3 = r2*float(sin(alpha+2.0/3*PI)*-x2 - cos(alpha+2.0/3*PI)*y2)*0.01 + float(L*w);
00057     //}
00058     /*else
00059     {
00060         float spdt = 0.5f * float(L*w);
00061         m1.direct_controll(spdt);
00062         m2.direct_controll(spdt);
00063         m3.direct_controll(spdt);
00064     }*/
00065     //printf("%5.2f : %5.2f : %5.2f : %5.2f",(float)x2,(float)y2,sieta1*180/PI,r1);
00066     //printf("\n");
00067 }