Diff: omuni.cpp

Revision:

1:75b51c0b7f47

Parent:

0:c0f3e14f1ed1

--- a/omuni.cpp	Tue Oct 10 12:50:39 2017 +0000
+++ b/omuni.cpp	Wed Oct 11 08:42:46 2017 +0000
@@ -1,62 +1,99 @@
 #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)
+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);
+    if(s1 || s2 || s3)
+        acceleration();
+    else
+        speed_reset();
+    m1 = speed * s1;
+    m2 = speed * s2;
+    m3 = speed * s3;
+    //printf("%5.2f : %5.2f : %5.2f : %5.2f",(float)x2,(float)y2,sieta1*180/PI,r1);
+    //printf("\n");
+}
+
+void omuni::acceleration()
+{
+    int time = timer.read_ms();
+    if(time < 1000)
+    {
+        if(time == 0)
         {
-            RXData[0] = 'H';
-            RXData[1] =  0 ;
-            RXData[2] =  0 ;
-            RXData[3] =  0 ;
-            RXData[4] =  0 ;
+            timer.start();
+            time = 10;
         }
-        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
+        //if(time >= 1000) timer.stop();
+        speed = time / 1000.0f;
+    }
+    else
+    {
+        timer.stop();
+        speed = 1.0;
+    }
+}
+
+void omuni::speed_reset()
+{
+    speed = 0;
+    timer.stop();
+    timer.reset();
+}
\ No newline at end of file