omuni.cpp
- Committer:
- gaku_sigu
- Date:
- 2017-10-10
- Revision:
- 0:c0f3e14f1ed1
- Child:
- 1:75b51c0b7f47
- Child:
- 2:fe76af2dff4d
File content as of revision 0:c0f3e14f1ed1:
#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"); }