Diff: omuni.cpp
- Revision:
- 2:fe76af2dff4d
- Parent:
- 0:c0f3e14f1ed1
- Child:
- 3:51f47bf405a0
--- a/omuni.cpp Tue Oct 10 12:50:39 2017 +0000 +++ b/omuni.cpp Fri Oct 13 10:10:32 2017 +0000 @@ -1,62 +1,74 @@ #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 +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, delta),m2(i2c_,addr2, delta),m3(i2c_,addr3, delta) +{ + RXData[0] = 'H'; + RXData[1] = 0 ; + RXData[2] = 0 ; + RXData[3] = 0 ; + RXData[4] = 0 ; + tick.attach(callback(this, &omuni::compute_task), delta); +} +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"); +} + +void omuni::compute_task() +{ + m1.compute(); + m2.compute(); + m3.compute(); + m1.run(); + m2.run(); + m3.run(); +} \ No newline at end of file