Diff: omuni.cpp

Revision:

6:60f122986f29

Parent:

5:5133533ed6d2

Child:

7:76e11784ce6b

--- 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