Diff: omuni.cpp
- Revision:
- 6:60f122986f29
- Parent:
- 5:5133533ed6d2
--- a/omuni.cpp Mon Oct 16 09:57:14 2017 +0000 +++ b/omuni.cpp Wed Oct 25 08:53:17 2017 +0000 @@ -1,6 +1,6 @@ #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 @@ -20,8 +20,8 @@ // 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) { @@ -52,15 +52,23 @@ if((sieta1 < -PI/3 && sieta1 > -2*PI/3.0)) w = -PI/3; } - 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; - m1.run(); - m2.run(); - m3.run(); + + 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"); } \ No newline at end of file