empc_sts_10k_tracking.h

2812例程以及 控制直流无刷电机程序

/*  mpt_searchTree.c
  
  Identifies a control law associated to a given state X using a binary search tree.

  Usage:
    region = mpt_searchTree(*X, *U)
    
    if "region" is smaller 1 (region < 1), there is no control law associated to
    a given state.

   Please note that all code in this file is provided under the terms of the
   GNU General Public License, which implies that if you include it directly
   into your commercial application, you will need to comply with the license.
   If you feel this is not a good solution for you or your company, feel free 
   to contact me at [e-mail], I can re-license this specific 
   piece of code to you free of charge.
*/

/* Copyright (C) 2006 by Michal Kvasnica ([e-mail]) */

/*  This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/ 

#define MPT_NU 6
#define MPT_NX 6
const float MPT_ST[] = {
0.000000e+000,	0.000000e+000,	-1.000000e+000,	0.000000e+000,	0.000000e+000,
0.000000e+000,	-2.618000e+000,	2.000000e+000,	3.000000e+000,	0.000000e+000,
0.000000e+000,	-1.000000e+000,	0.000000e+000,	0.000000e+000,	0.000000e+000,
-1.550800e+000,	4.000000e+000,	5.000000e+000,	0.000000e+000,	0.000000e+000,
1.000000e+000,	0.000000e+000,	0.000000e+000,	0.000000e+000,	4.712400e+000,
6.000000e+000,	7.000000e+000,	-7.071068e-001,	0.000000e+000,	0.000000e+000,
0.000000e+000,	7.071068e-001,	0.000000e+000,	0.000000e+000,	8.000000e+000,
9.000000e+000,	0.000000e+000,	0.000000e+000,	-1.000000e+000,	0.000000e+000,
0.000000e+000,	0.000000e+000,	-2.598000e+000,	1.000000e+001,	1.100000e+001,
-6.807015e-001,	0.000000e+000,	1.636511e-001,	0.000000e+000,	7.140475e-001,
0.000000e+000,	9.425427e-001,	1.200000e+001,	1.300000e+001,	0.000000e+000,
0.000000e+000,	1.000000e+000,	0.000000e+000,	0.000000e+000,	0.000000e+000,
3.665200e+000,	1.400000e+001,	1.500000e+001,	0.000000e+000,	0.000000e+000,
1.000000e+000,	0.000000e+000,	0.000000e+000,	0.000000e+000,	5.236000e-001,
1.600000e+001,	1.700000e+001,	0.000000e+000,	0.000000e+000,	1.000000e+000,
0.000000e+000,	0.000000e+000,	0.000000e+000,	5.236000e-001,	1.800000e+001,
1.900000e+001,	0.000000e+000,	6.807015e-001,	-1.636511e-001,	0.000000e+000,
-7.140475e-001,	0.000000e+000,	-2.570571e-001,	2.000000e+001,	2.100000e+001,
6.807015e-001,	0.000000e+000,	1.636511e-001,	0.000000e+000,	-7.140475e-001,
0.000000e+000,	4.284285e-001,	2.200000e+001,	2.300000e+001,	0.000000e+000,
0.000000e+000,	-1.000000e+000,	0.000000e+000,	0.000000e+000,	0.000000e+000,
-5.739600e+000,	2.400000e+001,	2.500000e+001,	0.000000e+000,	0.000000e+000,
-1.000000e+000,	0.000000e+000,	0.000000e+000,	0.000000e+000,	-5.739600e+000,
2.600000e+001,	2.700000e+001,	0.000000e+000,	0.000000e+000,	-1.000000e+000,
0.000000e+000,	0.000000e+000,	0.000000e+000,	-4.692400e+000,	2.800000e+001,
2.900000e+001,	0.000000e+000,	0.000000e+000,	1.000000e+000,	0.000000e+000,
0.000000e+000,	0.000000e+000,	3.645200e+000,	3.000000e+001,	3.100000e+001,
5.478672e-001,	5.478672e-001,	-2.634314e-001,	0.000000e+000,	5.747060e-001,
0.000000e+000,	-2.758589e-001,	3.200000e+001,	3.300000e+001,	5.627065e-001,
5.627065e-001,	1.352833e-001,	0.000000e+000,	5.902722e-001,	0.000000e+000,
-7.083266e-002,	3.400000e+001,	3.500000e+001,	5.478672e-001,	5.478672e-001,
-2.634314e-001,	0.000000e+000,	5.747060e-001,	0.000000e+000,	-2.758589e-001,
3.600000e+001,	3.700000e+001,	1.717134e-002,	-1.717134e-002,	-4.128254e-003,
-9.996961e-001,	-0.000000e+000,	-9.996961e-004,	2.161503e-003,	3.800000e+001,
3.900000e+001,	-3.432516e-002,	-1.716258e-002,	1.237845e-002,	9.991862e-001,
-0.000000e+000,	9.991862e-004,	1.080200e-002,	4.000000e+001,	4.100000e+001,
0.000000e+000,	0.000000e+000,	1.000000e+000,	0.000000e+000,	0.000000e+000,
0.000000e+000,	1.570800e+000,	4.200000e+001,	4.300000e+001,	-1.714664e-002,
-3.429327e-002,	-4.122316e-003,	9.992562e-001,	-0.000000e+000,	-0.000000e+000,
-1.077757e-002,	4.400000e+001,	4.500000e+001,	-1.714664e-002,	-3.429327e-002,
-4.122316e-003,	9.992562e-001,	-0.000000e+000,	-0.000000e+000,	-1.082076e-002,
4.600000e+001,	4.700000e+001,	0.000000e+000,	-6.900039e-001,	0.000000e+000,
0.000000e+000,	7.238056e-001,	0.000000e+000,	0.000000e+000,	4.800000e+001,
4.900000e+001,	0.000000e+000,	0.000000e+000,	1.000000e+000,	0.000000e+000,
0.000000e+000,	0.000000e+000,	5.759600e+000,	5.000000e+001,	5.100000e+001,
0.000000e+000,	-6.900039e-001,	0.000000e+000,	0.000000e+000,	7.238056e-001,
0.000000e+000,	0.000000e+000,	5.200000e+001,	5.300000e+001,	1.798544e-002,
3.597088e-002,	0.000000e+000,	0.000000e+000,	0.000000e+000,	9.991910e-001,
0.000000e+000,	5.400000e+001,	5.500000e+001,	-5.478672e-001,	-5.478672e-001,
-2.634314e-001,	0.000000e+000,	-5.747060e-001,	0.000000e+000,	-1.103435e+000,
5.600000e+001,	5.700000e+001,	0.000000e+000,	-6.807015e-001,	-1.636511e-001,
0.000000e+000,	7.140475e-001,	0.000000e+000,	-7.711713e-001,	5.800000e+001,
5.900000e+001,	5.627065e-001,	5.627065e-001,	-1.352833e-001,	0.000000e+000,
5.902722e-001,	0.000000e+000,	-3.541633e-001,	6.000000e+001,	6.100000e+001,
-6.900039e-001,	0.000000e+000,	0.000000e+000,	0.000000e+000,	7.238056e-001,
0.000000e+000,	0.000000e+000,	6.200000e+001,	6.300000e+001,	1.715435e-002,
-1.715435e-002,	-0.000000e+000,	-9.997057e-001,	-0.000000e+000,	-0.000000e+000,
4.304326e-003,	6.400000e+001,	6.500000e+001,	1.715435e-002,	-1.715435e-002,
-0.000000e+000,	-9.997057e-001,	-0.000000e+000,	-0.000000e+000,	4.304326e-003,
6.600000e+001,	6.700000e+001,	0.000000e+000,	0.000000e+000,	-1.000000e+000,
0.000000e+000,	0.000000e+000,	0.000000e+000,	-5.036000e-001,	6.800000e+001,
6.900000e+001,	0.000000e+000,	0.000000e+000,	-1.000000e+000,	0.000000e+000,
0.000000e+000,	0.000000e+000,	-5.036000e-001,	7.000000e+001,	7.100000e+001,
1.715435e-002,	-1.715435e-002,	-0.000000e+000,	-9.997057e-001,	-0.000000e+000,
-0.000000e+000,	4.304326e-003,	7.200000e+001,	7.300000e+001,	1.715435e-002,
-1.715435e-002,	-0.000000e+000,	-9.997057e-001,	-0.000000e+000,	-0.000000e+000,
4.304326e-003,	7.400000e+001,	7.500000e+001,	5.627065e-001,	5.627065e-001,
1.352833e-001,	-0.000000e+000,	5.902722e-001,	-0.000000e+000,	-2.857779e-001,
7.600000e+001,	7.700000e+001,	0.000000e+000,	0.000000e+000,	-1.000000e+000,
0.000000e+000,	0.000000e+000,	0.000000e+000,	-5.036000e-001,	7.800000e+001,
7.900000e+001,	0.000000e+000,	0.000000e+000,	1.000000e+000,	0.000000e+000,
0.000000e+000,	0.000000e+000,	1.570800e+000,	8.000000e+001,	8.100000e+001,
3.429094e-002,	1.714547e-002,	-1.236611e-002,	0.000000e+000,	0.000000e+000,
-9.991883e-001,	3.604272e-003,	8.200000e+001,	8.300000e+001,	3.597088e-002,
1.798544e-002,	0.000000e+000,	0.000000e+000,	0.000000e+000,	-9.991910e-001,
0.000000e+000,	8.400000e+001,	8.500000e+001,	-3.429094e-002,	-1.714547e-002,
1.236611e-002,	9.991883e-001,	-0.000000e+000,	-0.000000e+000,	1.080563e-002,
8.600000e+001,	8.700000e+001,	-1.714664e-002,	-3.429327e-002,	-4.122316e-003,
9.992562e-001,	0.000000e+000,	0.000000e+000,	3.604517e-003,	8.800000e+001,
8.900000e+001,	-1.714664e-002,	-3.429327e-002,	-4.122316e-003,	9.992562e-001,
-0.000000e+000,	-0.000000e+000,	-1.082076e-002,	9.000000e+001,	9.100000e+001,
-1.714664e-002,	-3.429327e-002,	-4.122316e-003,	9.992562e-001,	-0.000000e+000,
-0.000000e+000,	-1.077757e-002,	9.200000e+001,	9.300000e+001,	1.714664e-002,
3.429327e-002,	4.122316e-003,	-9.992562e-001,	0.000000e+000,	0.000000e+000,
2.518845e-002,	9.400000e+001,	9.500000e+001,	-3.597088e-002,	-1.798544e-002,
0.000000e+000,	0.000000e+000,	0.000000e+000,	-9.991910e-001,	0.000000e+000,
9.600000e+001,	9.700000e+001,	-3.429240e-002,	-1.714620e-002,	8.244421e-003,
-9.992307e-001,	-0.000000e+000,	-0.000000e+000,	4.746905e-002,	9.800000e+001,
9.900000e+001,	1.798544e-002,	3.597088e-002,	0.000000e+000,	0.000000e+000,
0.000000e+000,	9.991910e-001,	0.000000e+000,	1.000000e+002,	1.010000e+002,
-1.716375e-002,	-3.432750e-002,	4.126430e-003,	-9.992542e-001,	-0.000000e+000,
-9.992542e-004,	2.376602e-002,	1.020000e+002,	1.030000e+002,	-3.597088e-002,
-1.798544e-002,	0.000000e+000,	0.000000e+000,	0.000000e+000,	-9.991910e-001,
0.000000e+000,	1.040000e+002,	1.050000e+002,	-3.432662e-002,	-1.716331e-002,
8.252649e-003,	-9.992287e-001,	-0.000000e+000,	-9.992287e-004,	4.753083e-002,
1.060000e+002,	1.070000e+002,	-1.716375e-002,	-3.432750e-002,	4.126430e-003,
-9.992542e-001,	-0.000000e+000,	-9.992542e-004,	2.376602e-002,	1.080000e+002,
1.090000e+002,	-1.714664e-002,	-3.429327e-002,	4.122316e-003,	-9.992562e-001,
-0.000000e+000,	-0.000000e+000,	2.377112e-002,	1.100000e+002,	1.110000e+002,
1.715435e-002,	-1.715435e-002,	-0.000000e+000,	9.997057e-001,	-0.000000e+000,
-0.000000e+000,	-4.304326e-003,	1.120000e+002,	1.130000e+002,	1.715435e-002,
-1.715435e-002,	-0.000000e+000,	9.997057e-001,	-0.000000e+000,	-0.000000e+000,
-4.304326e-003,	1.140000e+002,	1.150000e+002,	-3.432516e-002,	-1.716258e-002,
-1.237845e-002,	-9.991862e-001,	-0.000000e+000,	-9.991862e-004,	-4.968921e-002,
1.160000e+002,	1.170000e+002,	-3.429094e-002,	-1.714547e-002,	-1.236611e-002,
-9.991883e-001,	-0.000000e+000,	-0.000000e+000,	-4.965407e-002,	1.180000e+002,
1.190000e+002,	-1.715420e-002,	1.715420e-002,	-4.124135e-003,	0.000000e+000,
0.000000e+000,	-9.996972e-001,	-2.519957e-002,	1.200000e+002,	1.210000e+002,
1.717134e-002,	-1.717134e-002,	4.128254e-003,	9.996961e-001,	-0.000000e+000,
9.996961e-004,	1.080751e-002,	1.220000e+002,	1.230000e+002,	-1.716331e-002,
-3.432662e-002,	8.252649e-003,	9.992287e-001,	-0.000000e+000,	9.992287e-004,
2.160492e-002,	1.240000e+002,	1.250000e+002,	1.714620e-002,	3.429240e-002,
-8.244421e-003,	0.000000e+000,	0.000000e+000,	-9.992307e-001,	-7.187267e-003,
1.260000e+002,	1.270000e+002,	1.715435e-002,	-1.715435e-002,	0.000000e+000,
-9.997057e-001,	0.000000e+000,	0.000000e+000,	1.872169e-002,	1.280000e+002,
1.290000e+002,	1.799417e-002,	-1.799417e-002,	0.000000e+000,	0.000000e+000,
0.000000e+000,	-9.996762e-001,	0.000000e+000,	1.300000e+002,	1.310000e+002,
1.715435e-002,	-1.715435e-002,	0.000000e+000,	-9.997057e-001,	0.000000e+000,
0.000000e+000,	1.872169e-002,	1.320000e+002,	1.330000e+002,	1.799417e-002,
-1.799417e-002,	0.000000e+000,	0.000000e+000,	0.000000e+000,	-9.996762e-001,
0.000000e+000,	1.340000e+002,	1.350000e+002,	-1.716331e-002,	-3.432662e-002,
-8.252649e-003,	-9.992287e-001,	-0.000000e+000,	-9.992287e-004,	4.320985e-003,
1.360000e+002,	1.370000e+002,	1.717134e-002,	-1.717134e-002,	-4.128254e-003,
-9.996961e-001,	-0.000000e+000,	-9.996961e-004,	2.161503e-003,	1.380000e+002,
1.390000e+002,	-1.716331e-002,	-3.432662e-002,	-8.252649e-003,	-9.992287e-001,
-0.000000e+000,	-9.992287e-004,	4.320985e-003,	1.400000e+002,	1.410000e+002,
1.715420e-002,	-1.715420e-002,	-4.124135e-003,	-9.996972e-001,	-0.000000e+000,
-0.000000e+000,	2.144943e-003,	1.420000e+002,	1.430000e+002,	1.715435e-002,
-1.715435e-002,	0.000000e+000,	-9.997057e-001,	0.000000e+000,	0.000000e+000,
1.872169e-002,	1.440000e+002,	1.450000e+002,	1.799417e-002,	-1.799417e-002,
0.000000e+000,	0.000000e+000,	0.000000e+000,	-9.996762e-001,	0.000000e+000,
1.460000e+002,	1.470000e+002,	1.715435e-002,	-1.715435e-002,	0.000000e+000,
-9.997057e-001,	0.000000e+000,	0.000000e+000,	1.872169e-002,	1.480000e+002,
1.490000e+002,	1.799417e-002,	-1.799417e-002,	0.000000e+000,	0.000000e+000,
0.000000e+000,	-9.996762e-001,	0.000000e+000,	1.500000e+002,	1.510000e+002,
0.000000e+000,	0.000000e+000,	-1.000000e+000,	0.000000e+000,	0.000000e+000,