oboboboboobo
Fork of omuni by
Diff: omuni.cpp
- Revision:
- 0:c0f3e14f1ed1
- Child:
- 1:75b51c0b7f47
- Child:
- 2:fe76af2dff4d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/omuni.cpp Tue Oct 10 12:50:39 2017 +0000 @@ -0,0 +1,62 @@ +#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) + :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/4; + if((sieta1 < -PI/3 && sieta1 > -2*PI/3.0)) + w = -PI/4; + } + float s1 = r2*float(sin(alpha)*-x2*0.01) + float(L*w); + float s2 = r2*float(sin(alpha+4.0/3*PI)*-x2 - cos(alpha+4.0/3*PI)*y2)*0.01 + float(L*w); + float s3 = r2*float(sin(alpha+2.0/3*PI)*-x2 - cos(alpha+2.0/3*PI)*y2)*0.01 + float(L*w); + m1 = s1; + m2 = s2; + m3 = s3; + //printf("%5.2f : %5.2f : %5.2f : %5.2f",(float)x2,(float)y2,sieta1*180/PI,r1); + //printf("\n"); + } \ No newline at end of file