Revision 13:9a51747773af, committed 2016-05-30

Comitter:

aktk

Date:

Mon May 30 09:18:50 2016 +0000

Parent:

12:b3e07a2220bc

Commit message:

all function has briefly implemented.;

Changed in this revision

diff -r b3e07a2220bc -r 9a51747773af CubicSpline.cpp
--- a/CubicSpline.cpp	Sun May 29 12:15:19 2016 +0000
+++ b/CubicSpline.cpp	Mon May 30 09:18:50 2016 +0000
@@ -1,9 +1,3 @@
-#define DEBUG
-#define VERSION_C
-#define DEBUG_MAKE_MODEL
-//#define DEBUG_SOLVE
-#define DEBUG_GETX "DEBUG_GETX\n"
-//#define DEBUG_GETY "DEBUG_GETY\n"
 #include "CubicSpline.h"
 
 //  To get voltage of TRP105F
@@ -26,7 +20,7 @@
     _Sample_Num = 5;
     _Sample_Set = (Vxyt  *)malloc(_Sample_Num * sizeof(Vxyt));
     _Last_Point = (Vxyt) {
-        0,0,0
+        0.0, 0.0, 0.0
     };
     for(int i = 0; i < 4; i++) {
         _C_x[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
@@ -43,7 +37,7 @@
     _Sample_Num = arg_num;
     _Sample_Set = (Vxyt  *)malloc(_Sample_Num * sizeof(Vxyt));
     _Last_Point = (Vxyt) {
-        0,0,0
+        0.0, 0.0, 0.0
     };
     for(int i = 0; i < 4; i++) {
         _C_x[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
@@ -61,7 +55,7 @@
     _Sample_Num = arg_num;
     _Sample_Set = (Vxyt  *)malloc(_Sample_Num * sizeof(Vxyt));
     _Last_Point = (Vxyt) {
-        0,0,0
+        0.0, 0.0, 0.0
     };
     for(int i = 0; i < 4; i++) {
         _C_x[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
@@ -98,14 +92,14 @@
             //  Recieve a Distance datus and store it into member
             //
             g_Serial_Signal.printf("X:");
-            _Sample_Set[i].x = 0;
+            _Sample_Set[i].x = 0.0;
             do {
                 sig = g_Serial_Signal.getc();
                 if('0' <= sig && sig <= '9') {
                     if(floatflag == 0) {
-                        _Sample_Set[i].x = 10 * _Sample_Set[i].x + sig - 48;
+                        _Sample_Set[i].x = 10.0 * _Sample_Set[i].x + (double)(sig - 48);
                     } else {
-                        _Sample_Set[i].x = _Sample_Set[i].x + (sig - 48) * pow((double)10, (double)- floatflag);
+                        _Sample_Set[i].x = _Sample_Set[i].x + (double)(sig - 48) * pow(10.0, (double)- floatflag);
                         floatflag++;
                     }
                     g_Serial_Signal.putc(char(sig));
@@ -115,7 +109,7 @@
                         g_Serial_Signal.putc(char(sig));
                     }
                 } else if(sig == 0x08) {
-                    _Sample_Set[i].x = 0;
+                    _Sample_Set[i].x = 0.0;
                     g_Serial_Signal.printf("[canseled!]");
                     g_Serial_Signal.putc('\n');
                     g_Serial_Signal.putc('>');
@@ -131,13 +125,13 @@
             //  Get 10 data and store mean as a sample.
             //  After get one original sample, system waits for 0.1 sec,
             //  thus it takes 1 sec evry sampling.
-            _Sample_Set[i].y = 0;
+            _Sample_Set[i].y = 0.0;
             for(int j = 0; j < 10; j++) {
                 tmp_set.y = g_Sensor_Voltage.read();
 #ifdef DEBUG
                 g_Serial_Signal.printf("%f,",tmp_set.y);
 #endif
-                _Sample_Set[i].y += (tmp_set.y / 10);
+                _Sample_Set[i].y += (tmp_set.y / 10.0);
                 wait(0.1);
             }
 #ifdef DEBUG
@@ -146,8 +140,8 @@
         }
 
         //  if the input data is over the bound, it is calibrated
-        if (_Sample_Set[i].x < 0)
-            _Sample_Set[i].x = 0;
+        if (_Sample_Set[i].x < 0.0)
+            _Sample_Set[i].x = 0.0;
     }
     //
     //  Sort set data array in x-Ascending order
@@ -166,7 +160,7 @@
             }
             // if a same dst has been input, calcurate mean.
             else if (_Sample_Set[i].x == _Sample_Set[j].x) {
-                tmp_set.y = (_Sample_Set[i].y + _Sample_Set[j].y)/2;
+                tmp_set.y = (_Sample_Set[i].y + _Sample_Set[j].y)/2.0;
                 _Sample_Set[i].y = tmp_set.y;
                 for(int k = j; k < _Sample_Num - 1; k++)
                     _Sample_Set[k] = _Sample_Set[k+1];
@@ -186,12 +180,12 @@
 #endif
 
     //  generate t which is parameter related to x,y
-    _Sample_Set[0].t = 0;
+    _Sample_Set[0].t = 0.0;
     for(int i = 1; i < _Sample_Num; i++)
         _Sample_Set[i].t =
             _Sample_Set[i-1].t
-            + sqrt(pow(_Sample_Set[i].x - _Sample_Set[i-1].x, 2)
-                   +pow(_Sample_Set[i].y - _Sample_Set[i-1].y, 2));
+            + sqrt(pow(_Sample_Set[i].x - _Sample_Set[i-1].x, 2.0)
+                   +pow(_Sample_Set[i].y - _Sample_Set[i-1].y, 2.0));
 }
 
 //
@@ -241,14 +235,14 @@
     double *Upper = (double*)malloc((arg_num - 2) * sizeof(double));
     double *Lower = (double*)malloc((arg_num - 2) * sizeof(double));
 #ifdef DEBUG_MAKE_MODEL
-    _printOutDataCouple(arg_sampled_t, arg_sampled_ft, arg_num, "\nargment set\n");
+    _printOutDataCouple(arg_sampled_t, arg_sampled_ft, arg_num, "\nargument set\n");
 #endif
 
     for(int i = 0; i < arg_num - 1; i++)
         h[i] =  (double)(arg_sampled_t[i + 1] - arg_sampled_t[i]);
 
     for(int i = 0; i < arg_num - 2; i++)
-        v[i] = 6 * (
+        v[i] = 6.0 * (
                    ((double)(arg_sampled_ft[i + 2] - arg_sampled_ft[i + 1])) / h[i + 1]
                    -
                    ((double)(arg_sampled_ft[i + 1] - arg_sampled_ft[i]))     / h[i]
@@ -257,11 +251,11 @@
     //
     //  LU decomposition
     //
-    Upper[0] = 2 * (h[0] + h[1]);
-    Lower[0] = 0;
+    Upper[0] = 2.0 * (h[0] + h[1]);
+    Lower[0] = 0.0;
     for (int i = 1; i < arg_num - 2; i++) {
         Lower[i] = h[i] / Upper[i - 1];
-        Upper[i] = 2 * (h[i] + h[i + 1]) - Lower[i] * h[i];
+        Upper[i] = 2.0 * (h[i] + h[i + 1]) - Lower[i] * h[i];
     }
     //
     //  forward substitution
@@ -314,6 +308,30 @@
 {
     _makeModel(arg_t, arg_ft, arg_C, _Sample_Num);
 }
+
+
+void CubicSpline2d::calibrateSensor()
+{
+    double t[_Sample_Num];
+    double ft[_Sample_Num];
+
+    _sampleData();
+    _Last_Point = _Sample_Set[0];
+
+    for(int i = 0; i < _Sample_Num; i++) {
+        t[i] = _Sample_Set[i].t;
+        ft[i]= _Sample_Set[i].x;
+    }
+    _makeModel(t,ft,_C_x);
+
+    for(int i = 0; i < _Sample_Num; i++) {
+        ft[i]= _Sample_Set[i].y;
+    }
+    _makeModel(t,ft,_C_y);
+
+}
+
+
 //
 //  Fuction to return the value of Cubic polynomial f(t)
 //
@@ -321,16 +339,18 @@
 {
     double ft;  //the value of Spline f(t).
 
-    ft = arg_C[3] * pow(arg_t, 3) + arg_C[2] * pow(arg_t, 2) + arg_C[1] * arg_t + arg_C[0];
+    ft = arg_C[3] * pow(arg_t, 3.0) + arg_C[2] * pow(arg_t, 2.0) + arg_C[1] * arg_t + arg_C[0];
 
     return ft;
 }
+
+
 //
 //  Function to solve a cubic polinomial
 //  by using Gardano-Tartaglia formula
 //
 void CubicSpline2d::_solve_cubic_f(
-    std::complex<double>* arg_t,
+    std::complex<double> arg_t[3],
     const double  arg_C[4],
     const double  arg_ft)
 {
@@ -339,49 +359,65 @@
         g_Serial_Signal.printf("C%d: %f\n", i, arg_C[i]);
 #endif
 
+    //arg_t: solution that's expected to be solved in this function.
+    //t_sol: the solution that's actually wanted as Sprine's solution.
+    //t0_ival: _Sample_Set[ival].t
+    //arg_t  = t_sol - t0_ival
+    //=>
+    //arg_ft = C[3](t_sol - t0_ival)^3 + C[2](t_sol - t0_ival)^2 + C[1](t_sol - t0_ival) + C[0]
+    //       = C[3]arg_t^3 + C[2]arg_t^2 + C[1]arg_t + C[0]
     double c[3];
-    //f(t)  = arg_ft/arg_C[3]
-    //      = t^3 + c[2]*t^2 + c[1]*t + c[0].
+    //f(t) := arg_ft/arg_C[3]
+    //      = arg_t^3 + c[2]*arg_t^2 + c[1]*arg_t + c[0].
     for(int i = 0; i < 3; i++) {
         c[i] = arg_C[i] / arg_C[3];
     }
     //modify the formula
-    //t^3 + c[2]*t^2 + c[1]*t + (c[0] - ft) = 0.
+    //t^3 + c[2]*t^2 + c[1]*t + (c[0] - f(t)) = 0.
     c[0] -= arg_ft / arg_C[3];
 #ifdef DEBUG_SOLVE
     for(int i = 0; i < 3; i++)
         g_Serial_Signal.printf("c%d: %f\n", i, c[i]);
 #endif
 
+    std::complex<double> d;
+    //d := c[2] / 3
+    //T := t + d (== arg_t - t_iavl + c[2]/3)
+    d = std::complex<double>(c[2] / 3.0, 0.0);
+    //=>    T^3 + 3pT + 2q = 0.
+    double p,q;
     //The values defined from coefficients of the formula
     //that identify solutions
-    double p,q,d;
-    p = (   -pow(c[2], 2) + 3 * c[1]) / 9;
-    q = (2 * pow(c[2], 3) - 9 * c[2] * c[1] + 27 * c[0]) / 54;
-    d = - c[2] / 3;
+    p = (     -pow(c[2], 2.0) + 3.0 * c[1]) / 9.0;
+    q = (2.0 * pow(c[2], 3.0) - 9.0 * c[2] * c[1] + 27.0 * c[0]) / 54.0;
 
     //Discriminant section
     double D;
-    D = pow(p, 3) + pow(q, 2);
+    D = pow(p, 3.0) + pow(q, 2.0);
 #ifdef DEBUG_SOLVE
     g_Serial_Signal.printf("p: %f\n", p);
     g_Serial_Signal.printf("q: %f\n", q);
-    g_Serial_Signal.printf("d: %f\n", d);
     g_Serial_Signal.printf("D: %f\n", D);
 #endif
 
+    //For all T, u, there exsists v: T = u + v;  increment degree of freedom.
+    //Futhermore,
+    //  u^3 + v^3 - 2q = 0
+    //  uv + p = 0 <=> u^3v^3 = -p^3
+    //those because: T = u + v, T^3 + 3pT + 2q = 0,
+    //=>    (u + v)^3 + 3p(u + v) + 2q = 0
+    //=>    u^3 + v^3 + 3(uv + p)(u+v) + 2q = 0
     //The values defined from p and q
     //that idetify solutions
     std::complex<double> u,v;
-
     //Real root only
-    if(D <= 0) {
+    if(D <= 0.0) {
         u = std::complex<double>(-q, sqrt(-D));
         v = std::complex<double>(-q,-sqrt(-D));
         //u = pow(u, 1/3);
         //v = pow(v, 1/3);
         u = std::exp(std::log(u)/std::complex<double>(3.0,0.0));
-        v = std::exp(std::log(u)/std::complex<double>(3.0,0.0));
+        v = std::exp(std::log(v)/std::complex<double>(3.0,0.0));
     }
     //One real root and two complex root
     else {
@@ -393,19 +429,20 @@
 #ifdef DEBUG_SOLVE
     g_Serial_Signal.printf("u: %f + (%f)i\n", u.real(), u.imag());
     g_Serial_Signal.printf("v: %f + (%f)i\n", v.real(), v.imag());
+    g_Serial_Signal.printf("d: %f + (%f)i\n", d.real(), d.imag());
 #endif
 
     //Cubic root of 1
     std::complex<double> omega[3]= {
         std::complex<double>( 1.0, 0.0),
-        std::complex<double>(-1/2, sqrt(3.0)/2),
-        std::complex<double>(-1/2,-sqrt(3.0)/2)
+        std::complex<double>(-1.0/2.0, sqrt(3.0)/2.0),
+        std::complex<double>(-1.0/2.0,-sqrt(3.0)/2.0)
     };
 
     //Solution of the formula
-    arg_t[0] = omega[0] * u + omega[0] * v + d;
-    arg_t[1] = omega[1] * u + omega[2] * v + d;
-    arg_t[2] = omega[2] * u + omega[1] * v + d;
+    arg_t[0] = omega[0] * u + omega[0] * v - d;
+    arg_t[1] = omega[1] * u + omega[2] * v - d;
+    arg_t[2] = omega[2] * u + omega[1] * v - d;
 
 #ifdef DEBUG_SOLVE
     for(int i = 0; i < 3; i++)
@@ -435,20 +472,27 @@
     for(int ival = 0; ival < _Sample_Num - 1; ival++) {
         for(int i = 0; i < 4; i++) C[i] = _C_y[i][ival];
         _solve_cubic_f(t_sol, C, arg_y);
+        for(int i = 0; i < 3; i++) t_sol[i] += _Sample_Set[ival].t;
 #ifdef DEBUG_GETX
-        g_Serial_Signal.printf("interval:%d \t %f < t < %f\n", ival, _Sample_Set[ival].t, _Sample_Set[ival+1].t);
+        g_Serial_Signal.printf("interval:%d \t %f < t < %f\n", ival, _Sample_Set[ival].t, _Sample_Set[ival + 1].t);
 #endif
         for(int i = 0; i < 3; i++) {
             //  regarding only real solution
             //  acuracy (error range) is supposed +-10E-3 here(groundless)
             if(std::abs(t_sol[i].imag()) < 0.000001) {
-                /*   if (ival == 0               && t_sol[i].real() < _Sample_Set[ival].t) {
-                    t_real.push_back(_Sample_Set[ival].t);
+                /*  */ if (ival == 0               && t_sol[i].real() < _Sample_Set[ival].t) {
+                    t_real.push_back(t_sol[i].real());
                     t_ival.push_back(ival);
+#ifdef DEBUG_GETX
+                    g_Serial_Signal.printf("(t, i) = (%f, %d)\n", t_real[t_real.size() - 1], ival);
+#endif
                 } else if (ival == _Sample_Num - 2 && _Sample_Set[ival + 1].t <= t_sol[i].real()) {
-                    t_real.push_back(_Sample_Set[ival + 1].t);
+                    t_real.push_back(t_sol[i].real());
                     t_ival.push_back(ival);
-                } else*/ if (_Sample_Set[ival].t <= t_sol[i].real() && t_sol[i].real() < _Sample_Set[ival+1].t) {
+#ifdef DEBUG_GETX
+                    g_Serial_Signal.printf("(t, i) = (%f, %d)\n", t_real[t_real.size() - 1], ival);
+#endif
+                } else if (_Sample_Set[ival].t <= t_sol[i].real() && t_sol[i].real() < _Sample_Set[ival + 1].t) {
                     t_real.push_back(t_sol[i].real());
                     t_ival.push_back(ival);
 #ifdef DEBUG_GETX
@@ -459,7 +503,7 @@
         }
     }
 
-    if(t_real.size() > 0) {
+    if(!t_real.empty()) {
         the_t = t_real[0];
         the_i = t_ival[0];
         //if t's size is bigger than 1
@@ -478,6 +522,9 @@
             if(_Sample_Set[i].t <= the_t && the_t <= _Sample_Set[i+1].t)
                 the_i = i;
     }
+    /* */if (the_t < _Sample_Set[0].t) the_t = _Sample_Set[0].t;
+    else if (_Sample_Set[_Sample_Num - 1].t <= the_t) the_t = _Sample_Set[_Sample_Num - 1].t;
+
     for(int i = 0; i < 4; i++) C[i] = _C_x[i][the_i];
     x = _cubic_f(the_t - _Sample_Set[the_i].t, C);
 #ifdef DEBUG_GETX
@@ -502,6 +549,7 @@
 
 #ifdef DEBUG_GETY
     g_Serial_Signal.printf(DEBUG_GETY);
+    g_Serial_Signal.printf("arg_x: %f\n", arg_x);
 #endif
     //  For the every Intervals of Spline,
     //it solves the polynomial defined by C[i] of the interval,
@@ -512,20 +560,36 @@
     for(int ival = 0; ival < _Sample_Num - 1; ival++) {
         for(int i = 0; i < 4; i++) C[i] = _C_x[i][ival];
         _solve_cubic_f(t_sol, C, arg_x);
+        for(int i = 0; i < 3; i++) t_sol[i] += _Sample_Set[ival].t;
+        //arg_ft = C[3](t_sol - t0_ival)^3 + C[2](t_sol - t0_ival)^2 + C[1](t_sol - t0_ival) + C[0]
+        //       = C[3]arg_t^3 + C[2]arg_t^2 + C[1]arg_t + C[0]
 #ifdef DEBUG_GETY
-        g_Serial_Signal.printf("interval:%d \t %f < t < %f\n", ival, _Sample_Set[ival].t, _Sample_Set[ival+1].t);
+        g_Serial_Signal.printf("interval:%d \t %f <= t < %f\n", ival, _Sample_Set[ival].t, _Sample_Set[ival + 1].t);
+        for(int i = 0; i < 3; i++)
+            g_Serial_Signal.printf("t%d \t %f + (%f)i\n", i, t_sol[i].real(), t_sol[i].imag());
 #endif
         for(int i = 0; i < 3; i++) {
             //  regarding only real solution
-            //  acuracy (error range) is supposed +-10E-3 here(groundless)
+            //  acuracy (error range) is supposed +-10E-6 here(groundless)
             if(std::abs(t_sol[i].imag()) < 0.000001) {
-                /*   if (ival == 0               && t_sol[i].real() < _Sample_Set[ival].t) {
-                    t_real.push_back(_Sample_Set[ival].t);
+                /**/ if (ival == 0               && t_sol[i].real() < _Sample_Set[ival].t) {
+                    t_real.push_back(t_sol[i].real());
                     t_ival.push_back(ival);
-                } else if (ival == _Sample_Num - 2 && _Sample_Set[ival + 1].t <= t_sol[i].real()) {
-                    t_real.push_back(_Sample_Set[ival + 1].t);
+#ifdef DEBUG_GETY
+                    g_Serial_Signal.printf("(t, i) = (%f, %d)\n", t_real[t_real.size() - 1], ival);
+#endif
+                }//
+                else if (ival == _Sample_Num - 2 && _Sample_Set[ival + 1].t <= t_sol[i].real()) {
+                    t_real.push_back(t_sol[i].real());
                     t_ival.push_back(ival);
-                } else*/ if (_Sample_Set[ival].t <= t_sol[i].real() && t_sol[i].real() < _Sample_Set[ival+1].t) {
+#ifdef DEBUG_GETY
+                    g_Serial_Signal.printf("(t, i) = (%f, %d)\n", t_real[t_real.size() - 1], ival);
+#endif
+                }
+                //  There sometimes be a so fucking small error in solutions,
+                //  so acuracy is set at 10E-6(groundless)
+                else if (static_cast<int>(_Sample_Set[ival].t * std::pow(10., 6.)) <= static_cast<int>(t_sol[i].real() * std::pow(10., 6.))
+                         &&  static_cast<int>(t_sol[i].real() * std::pow(10., 6.))     <  static_cast<int>(_Sample_Set[ival + 1].t * std::pow(10., 6.))) {
                     t_real.push_back(t_sol[i].real());
                     t_ival.push_back(ival);
 #ifdef DEBUG_GETY
@@ -536,7 +600,7 @@
         }
     }
 
-    if(t_real.size() > 0) {
+    if(!t_real.empty()) {
         the_t = t_real[0];
         the_i = t_ival[0];
         //if t's size is bigger than 1
@@ -546,7 +610,9 @@
                 the_i = t_ival[i];
             }
         }
-    } else {
+    }
+    //if no solution muched due to any errors
+    else {
 #ifdef DEBUG_GETY
         g_Serial_Signal.printf("LastPoint\n");
 #endif
@@ -555,6 +621,12 @@
             if(_Sample_Set[i].t <= the_t && the_t <= _Sample_Set[i+1].t)
                 the_i = i;
     }
+
+    //
+    /* */if (the_t < _Sample_Set[0].t) the_t = _Sample_Set[0].t;
+    else if (_Sample_Set[_Sample_Num - 1].t < the_t) the_t = _Sample_Set[_Sample_Num - 1].t;
+
+    //
     for(int i = 0; i < 4; i++) C[i] = _C_y[i][the_i];
     y = _cubic_f(the_t - _Sample_Set[the_i].t, C);
 #ifdef DEBUG_GETY
@@ -567,26 +639,6 @@
 }
 
 
-void CubicSpline2d::calibrateSensor()
-{
-    double t[_Sample_Num];
-    double ft[_Sample_Num];
-
-    _sampleData();
-    _Last_Point = _Sample_Set[0];
-
-    for(int i = 0; i < _Sample_Num; i++) {
-        t[i] = _Sample_Set[i].t;
-        ft[i]= _Sample_Set[i].x;
-    }
-    _makeModel(t,ft,_C_x);
-    
-    for(int i = 0; i < _Sample_Num; i++) {
-        ft[i]= _Sample_Set[i].y;
-    }
-    _makeModel(t,ft,_C_y);
-
-}
 
 void CubicSpline2d::saveSetting()
 {
@@ -762,19 +814,23 @@
 void CubicSpline2d::printOutData()
 {
     FILE *fp;
-    double x,y;
-    double d = (_Sample_Set[_Sample_Num - 1].x - _Sample_Set[0].x) / 100;
+    double x,y,t;
+    double d = (_Sample_Set[_Sample_Num - 1].x - _Sample_Set[0].x) / 100.0;
 
     fp = fopen("/local/log.txt", "w");  // open file in writing mode
 
     fprintf(fp, "x, y, (t)\n");
     for(int ival = 0; ival < _Sample_Num - 1; ival++) {
         fprintf(fp, "ival: %d \n", ival);
-        for(x = _Sample_Set[ival].x; x < _Sample_Set[ival + 1].x; x += d) {
+        for(x = _Sample_Set[ival].x; x <= _Sample_Set[ival + 1].x; x += d) {
             y = getY(x);
-            fprintf(fp, "%f,%f,(%f)\n", x, y, sqrt(x*x + y*y));
+            if(ival == 0)
+                t = sqrt((x - _Sample_Set[ival].x)*(x - _Sample_Set[ival].x) + (x - _Sample_Set[ival].y)*(x - _Sample_Set[ival].y));
+            else
+                t = _Sample_Set[ival-1].t + sqrt((x - _Sample_Set[ival-1].x)*(x - _Sample_Set[ival-1].x) + (x - _Sample_Set[ival].y)*(x - _Sample_Set[ival].y));
+            fprintf(fp, "%f,%f,(%f)\n", x, y, t);
         }
-        fprintf(fp, "ival: %d \n", ival);
+        fprintf(fp, "-----------------------------------------\n");
     }
 
     fprintf(fp, "\nSample:dst, vol\n");

diff -r b3e07a2220bc -r 9a51747773af CubicSpline.h
--- a/CubicSpline.h	Sun May 29 12:15:19 2016 +0000
+++ b/CubicSpline.h	Mon May 30 09:18:50 2016 +0000
@@ -12,6 +12,13 @@
 #ifndef Cubic_Spline_H
 #define Cubic_Spline_H
 
+#define DEBUG
+#define VERSION_C
+//#define DEBUG_MAKE_MODEL
+//#define DEBUG_SOLVE
+//#define DEBUG_GETX "DEBUG_GETX\n"
+//#define DEBUG_GETY "DEBUG_GETY\n"
+
 #include "mbed.h"
 #include <cmath>
 #include <complex>