quadprog++ and Eigen library test
Dependencies: mbed Eigen FastPWM
main.cpp@2:e843c1b0b25c, 2019-09-24 (annotated)
- Committer:
- jsoh91
- Date:
- Tue Sep 24 00:20:12 2019 +0000
- Revision:
- 2:e843c1b0b25c
- Parent:
- 1:27f1640d930d
QP and Eigen library test project;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
GiJeongKim | 0:8a1dc1fdba10 | 1 | #include "mbed.h" |
GiJeongKim | 0:8a1dc1fdba10 | 2 | #include "FastPWM.h" |
GiJeongKim | 0:8a1dc1fdba10 | 3 | |
GiJeongKim | 1:27f1640d930d | 4 | #define PIN_A PA_8 // channel 1 |
GiJeongKim | 1:27f1640d930d | 5 | #define PIN_B PA_7 // complementary |
jsoh91 | 2:e843c1b0b25c | 6 | #define PWM_ARR 4500 /// timer autoreload value 40k loop, 40k pwm |
jsoh91 | 2:e843c1b0b25c | 7 | |
jsoh91 | 2:e843c1b0b25c | 8 | #include "quadprog.h" |
jsoh91 | 2:e843c1b0b25c | 9 | #include <cstdlib> |
jsoh91 | 2:e843c1b0b25c | 10 | #include "Eigen/Dense.h" |
jsoh91 | 2:e843c1b0b25c | 11 | |
jsoh91 | 2:e843c1b0b25c | 12 | using namespace std; |
jsoh91 | 2:e843c1b0b25c | 13 | using namespace Eigen; |
jsoh91 | 2:e843c1b0b25c | 14 | |
jsoh91 | 2:e843c1b0b25c | 15 | MatrixXd AA = MatrixXd::Zero(6,1); |
jsoh91 | 2:e843c1b0b25c | 16 | |
jsoh91 | 2:e843c1b0b25c | 17 | class JS_QP |
jsoh91 | 2:e843c1b0b25c | 18 | { |
jsoh91 | 2:e843c1b0b25c | 19 | private: |
jsoh91 | 2:e843c1b0b25c | 20 | int NUMCOLS;//length of X |
jsoh91 | 2:e843c1b0b25c | 21 | int NUMEQ; |
jsoh91 | 2:e843c1b0b25c | 22 | int NUMINEQ; |
jsoh91 | 2:e843c1b0b25c | 23 | |
jsoh91 | 2:e843c1b0b25c | 24 | MatrixXd A_ineq,B_ineq; |
jsoh91 | 2:e843c1b0b25c | 25 | MatrixXd A_eq,B_eq; |
jsoh91 | 2:e843c1b0b25c | 26 | |
jsoh91 | 2:e843c1b0b25c | 27 | public: |
jsoh91 | 2:e843c1b0b25c | 28 | MatrixXd H,F; |
jsoh91 | 2:e843c1b0b25c | 29 | MatrixXd X; |
jsoh91 | 2:e843c1b0b25c | 30 | |
jsoh91 | 2:e843c1b0b25c | 31 | public: |
jsoh91 | 2:e843c1b0b25c | 32 | void setNums(int _xlength, int numEqConstraints, int numIneqConstraints) |
jsoh91 | 2:e843c1b0b25c | 33 | { |
jsoh91 | 2:e843c1b0b25c | 34 | NUMCOLS = _xlength;//should be 18+12+3*contact |
jsoh91 | 2:e843c1b0b25c | 35 | NUMEQ = numEqConstraints;//NUMCOLS(dynamics) + NUMCOLS(contactconstraints + swingleg) |
jsoh91 | 2:e843c1b0b25c | 36 | //contact to be weight? |
jsoh91 | 2:e843c1b0b25c | 37 | NUMINEQ = numIneqConstraints;//friction cone approximation -ufz<fx<ufz, -ufz<fy<ufz ->4 |
jsoh91 | 2:e843c1b0b25c | 38 | |
jsoh91 | 2:e843c1b0b25c | 39 | //maybe matrix assignment here |
jsoh91 | 2:e843c1b0b25c | 40 | X = MatrixXd::Zero(NUMCOLS,1); |
jsoh91 | 2:e843c1b0b25c | 41 | A_eq = MatrixXd::Zero(NUMEQ, NUMCOLS); |
jsoh91 | 2:e843c1b0b25c | 42 | B_eq = MatrixXd::Zero(NUMEQ, 1); |
jsoh91 | 2:e843c1b0b25c | 43 | |
jsoh91 | 2:e843c1b0b25c | 44 | A_ineq = MatrixXd::Zero(NUMINEQ, NUMCOLS); |
jsoh91 | 2:e843c1b0b25c | 45 | B_ineq = MatrixXd::Zero(NUMINEQ, 1); |
jsoh91 | 2:e843c1b0b25c | 46 | |
jsoh91 | 2:e843c1b0b25c | 47 | |
jsoh91 | 2:e843c1b0b25c | 48 | H = MatrixXd::Zero(NUMCOLS,NUMCOLS); |
jsoh91 | 2:e843c1b0b25c | 49 | F = MatrixXd::Zero(NUMCOLS,1); |
jsoh91 | 2:e843c1b0b25c | 50 | } |
jsoh91 | 2:e843c1b0b25c | 51 | |
jsoh91 | 2:e843c1b0b25c | 52 | void make_EQ(MatrixXd A, MatrixXd b) |
jsoh91 | 2:e843c1b0b25c | 53 | { |
jsoh91 | 2:e843c1b0b25c | 54 | //Aeq*X = Beq |
jsoh91 | 2:e843c1b0b25c | 55 | A_eq = A; |
jsoh91 | 2:e843c1b0b25c | 56 | B_eq = b; |
jsoh91 | 2:e843c1b0b25c | 57 | } |
jsoh91 | 2:e843c1b0b25c | 58 | |
jsoh91 | 2:e843c1b0b25c | 59 | void make_IEQ(MatrixXd A, MatrixXd b) |
jsoh91 | 2:e843c1b0b25c | 60 | { |
jsoh91 | 2:e843c1b0b25c | 61 | // Aineq*X < Bineq |
jsoh91 | 2:e843c1b0b25c | 62 | A_ineq = A; |
jsoh91 | 2:e843c1b0b25c | 63 | B_ineq = b; |
jsoh91 | 2:e843c1b0b25c | 64 | } |
jsoh91 | 2:e843c1b0b25c | 65 | |
jsoh91 | 2:e843c1b0b25c | 66 | void make_HF(MatrixXd A, MatrixXd b, int mode=-1) |
jsoh91 | 2:e843c1b0b25c | 67 | { |
jsoh91 | 2:e843c1b0b25c | 68 | //min (0.5* x H x + F x) |
jsoh91 | 2:e843c1b0b25c | 69 | if(mode == -1) { |
jsoh91 | 2:e843c1b0b25c | 70 | H = A.transpose() * A + MatrixXd::Identity(NUMCOLS,NUMCOLS)*1e-3; |
jsoh91 | 2:e843c1b0b25c | 71 | F = -A.transpose() * b; |
GiJeongKim | 0:8a1dc1fdba10 | 72 | |
jsoh91 | 2:e843c1b0b25c | 73 | } else { |
jsoh91 | 2:e843c1b0b25c | 74 | H = A; |
jsoh91 | 2:e843c1b0b25c | 75 | F = b; |
jsoh91 | 2:e843c1b0b25c | 76 | |
jsoh91 | 2:e843c1b0b25c | 77 | } |
jsoh91 | 2:e843c1b0b25c | 78 | |
jsoh91 | 2:e843c1b0b25c | 79 | } |
jsoh91 | 2:e843c1b0b25c | 80 | |
jsoh91 | 2:e843c1b0b25c | 81 | MatrixXd solve_QP() |
jsoh91 | 2:e843c1b0b25c | 82 | { |
jsoh91 | 2:e843c1b0b25c | 83 | quadprogpp::Vector<double> outX; |
jsoh91 | 2:e843c1b0b25c | 84 | quadprogpp::Matrix<double> G; |
jsoh91 | 2:e843c1b0b25c | 85 | quadprogpp::Vector<double> g0; |
jsoh91 | 2:e843c1b0b25c | 86 | quadprogpp::Matrix<double> CE; |
jsoh91 | 2:e843c1b0b25c | 87 | quadprogpp::Vector<double> ce0; |
jsoh91 | 2:e843c1b0b25c | 88 | quadprogpp::Matrix<double> CI; |
jsoh91 | 2:e843c1b0b25c | 89 | quadprogpp::Vector<double> ci0; |
jsoh91 | 2:e843c1b0b25c | 90 | //min 0.5 * x G x + g0 x |
jsoh91 | 2:e843c1b0b25c | 91 | //CE^T x + ce0 = 0 |
jsoh91 | 2:e843c1b0b25c | 92 | //CI^T x + ci0 >= 0 |
jsoh91 | 2:e843c1b0b25c | 93 | |
jsoh91 | 2:e843c1b0b25c | 94 | G.resize(NUMCOLS,NUMCOLS); |
jsoh91 | 2:e843c1b0b25c | 95 | g0.resize(NUMCOLS); |
jsoh91 | 2:e843c1b0b25c | 96 | for(int i=0; i<NUMCOLS; i++) { |
jsoh91 | 2:e843c1b0b25c | 97 | for(int j=0; j<NUMCOLS; j++) { |
jsoh91 | 2:e843c1b0b25c | 98 | G[i][j] = H(i,j); |
jsoh91 | 2:e843c1b0b25c | 99 | } |
jsoh91 | 2:e843c1b0b25c | 100 | g0[i] = F(i,0); |
jsoh91 | 2:e843c1b0b25c | 101 | } |
jsoh91 | 2:e843c1b0b25c | 102 | CE.resize(NUMCOLS,NUMEQ); |
jsoh91 | 2:e843c1b0b25c | 103 | ce0.resize(NUMEQ); |
jsoh91 | 2:e843c1b0b25c | 104 | for(int i=0; i<NUMCOLS; i++) { |
jsoh91 | 2:e843c1b0b25c | 105 | for(int j=0; j<NUMEQ; j++) { |
jsoh91 | 2:e843c1b0b25c | 106 | CE[i][j] = -A_eq(j,i); |
jsoh91 | 2:e843c1b0b25c | 107 | } |
jsoh91 | 2:e843c1b0b25c | 108 | } |
jsoh91 | 2:e843c1b0b25c | 109 | for(int j=0; j<NUMEQ; j++) { |
jsoh91 | 2:e843c1b0b25c | 110 | ce0[j] = B_eq(j,0); |
jsoh91 | 2:e843c1b0b25c | 111 | } |
jsoh91 | 2:e843c1b0b25c | 112 | CI.resize(NUMCOLS,NUMINEQ); |
jsoh91 | 2:e843c1b0b25c | 113 | ci0.resize(NUMINEQ); |
jsoh91 | 2:e843c1b0b25c | 114 | for(int i=0; i<NUMCOLS; i++) { |
jsoh91 | 2:e843c1b0b25c | 115 | for(int j=0; j<NUMINEQ; j++) { |
jsoh91 | 2:e843c1b0b25c | 116 | CI[i][j] = -A_ineq(j,i); |
jsoh91 | 2:e843c1b0b25c | 117 | } |
jsoh91 | 2:e843c1b0b25c | 118 | } |
jsoh91 | 2:e843c1b0b25c | 119 | for(int j=0; j<NUMINEQ; j++) { |
jsoh91 | 2:e843c1b0b25c | 120 | ci0[j] = B_ineq(j,0); |
jsoh91 | 2:e843c1b0b25c | 121 | } |
jsoh91 | 2:e843c1b0b25c | 122 | outX.resize(NUMCOLS); |
jsoh91 | 2:e843c1b0b25c | 123 | |
jsoh91 | 2:e843c1b0b25c | 124 | solve_quadprog(G, g0, CE, ce0, CI, ci0, outX); |
jsoh91 | 2:e843c1b0b25c | 125 | for(int i=0; i<NUMCOLS; i++) { |
jsoh91 | 2:e843c1b0b25c | 126 | X(i,0) = outX[i]; |
jsoh91 | 2:e843c1b0b25c | 127 | } |
jsoh91 | 2:e843c1b0b25c | 128 | |
jsoh91 | 2:e843c1b0b25c | 129 | return X; |
jsoh91 | 2:e843c1b0b25c | 130 | |
jsoh91 | 2:e843c1b0b25c | 131 | } |
jsoh91 | 2:e843c1b0b25c | 132 | |
jsoh91 | 2:e843c1b0b25c | 133 | public: |
jsoh91 | 2:e843c1b0b25c | 134 | |
jsoh91 | 2:e843c1b0b25c | 135 | }; |
jsoh91 | 2:e843c1b0b25c | 136 | |
jsoh91 | 2:e843c1b0b25c | 137 | JS_QP js_qp; |
jsoh91 | 2:e843c1b0b25c | 138 | |
jsoh91 | 2:e843c1b0b25c | 139 | //MatrixXd AA; |
GiJeongKim | 1:27f1640d930d | 140 | |
GiJeongKim | 0:8a1dc1fdba10 | 141 | |
jsoh91 | 2:e843c1b0b25c | 142 | float dtc_a=0; |
jsoh91 | 2:e843c1b0b25c | 143 | void Init_PWM(); |
jsoh91 | 2:e843c1b0b25c | 144 | |
jsoh91 | 2:e843c1b0b25c | 145 | AnalogIn Vfb(PA_5); |
jsoh91 | 2:e843c1b0b25c | 146 | |
jsoh91 | 2:e843c1b0b25c | 147 | extern "C" void TIM1_UP_TIM10_IRQHandler(void) |
jsoh91 | 2:e843c1b0b25c | 148 | { |
jsoh91 | 2:e843c1b0b25c | 149 | if (TIM1->SR & TIM_SR_UIF ) { |
jsoh91 | 2:e843c1b0b25c | 150 | |
jsoh91 | 2:e843c1b0b25c | 151 | float Vth = 0.51f; |
jsoh91 | 2:e843c1b0b25c | 152 | |
jsoh91 | 2:e843c1b0b25c | 153 | if(Vfb >= Vth) { //PA_8 이게 하이 |
jsoh91 | 2:e843c1b0b25c | 154 | dtc_a -= 0.001f; |
jsoh91 | 2:e843c1b0b25c | 155 | } |
jsoh91 | 2:e843c1b0b25c | 156 | |
jsoh91 | 2:e843c1b0b25c | 157 | else { |
jsoh91 | 2:e843c1b0b25c | 158 | dtc_a += 0.001f; |
jsoh91 | 2:e843c1b0b25c | 159 | } |
jsoh91 | 2:e843c1b0b25c | 160 | |
jsoh91 | 2:e843c1b0b25c | 161 | if(dtc_a>=0.9f) |
jsoh91 | 2:e843c1b0b25c | 162 | dtc_a = 0.9f; |
jsoh91 | 2:e843c1b0b25c | 163 | |
jsoh91 | 2:e843c1b0b25c | 164 | if(dtc_a <= 0.0f) |
jsoh91 | 2:e843c1b0b25c | 165 | dtc_a = 0.0f; |
jsoh91 | 2:e843c1b0b25c | 166 | |
jsoh91 | 2:e843c1b0b25c | 167 | dtc_a = 0.5f; |
jsoh91 | 2:e843c1b0b25c | 168 | |
jsoh91 | 2:e843c1b0b25c | 169 | TIM1->CCR1 = (PWM_ARR)*(1.0f-dtc_a); // Write duty cycles |
jsoh91 | 2:e843c1b0b25c | 170 | |
GiJeongKim | 0:8a1dc1fdba10 | 171 | } |
jsoh91 | 2:e843c1b0b25c | 172 | TIM1->SR = 0x0; // reset the status register |
GiJeongKim | 0:8a1dc1fdba10 | 173 | } |
GiJeongKim | 0:8a1dc1fdba10 | 174 | |
jsoh91 | 2:e843c1b0b25c | 175 | Timer t; |
GiJeongKim | 0:8a1dc1fdba10 | 176 | |
jsoh91 | 2:e843c1b0b25c | 177 | double t_old=0; |
jsoh91 | 2:e843c1b0b25c | 178 | |
jsoh91 | 2:e843c1b0b25c | 179 | int main() |
jsoh91 | 2:e843c1b0b25c | 180 | { |
jsoh91 | 2:e843c1b0b25c | 181 | |
jsoh91 | 2:e843c1b0b25c | 182 | MatrixXd Aa = MatrixXd::Zero(20,2); |
jsoh91 | 2:e843c1b0b25c | 183 | MatrixXd Ba = MatrixXd::Zero(20,10); |
jsoh91 | 2:e843c1b0b25c | 184 | MatrixXd Ga = MatrixXd::Zero(20,20); |
jsoh91 | 2:e843c1b0b25c | 185 | |
jsoh91 | 2:e843c1b0b25c | 186 | Aa << |
jsoh91 | 2:e843c1b0b25c | 187 | 1.000000,-1.000000 |
jsoh91 | 2:e843c1b0b25c | 188 | ,0.000000,0.783386 |
jsoh91 | 2:e843c1b0b25c | 189 | ,1.000000,-1.783386 |
jsoh91 | 2:e843c1b0b25c | 190 | ,0.000000,0.613693 |
jsoh91 | 2:e843c1b0b25c | 191 | ,1.000000,-2.397079 |
jsoh91 | 2:e843c1b0b25c | 192 | ,0.000000,0.480759 |
jsoh91 | 2:e843c1b0b25c | 193 | ,1.000000,-2.877838 |
jsoh91 | 2:e843c1b0b25c | 194 | ,0.000000,0.376620 |
jsoh91 | 2:e843c1b0b25c | 195 | ,1.000000,-3.254458 |
jsoh91 | 2:e843c1b0b25c | 196 | ,0.000000,0.295039 |
jsoh91 | 2:e843c1b0b25c | 197 | ,1.000000,-3.549496 |
jsoh91 | 2:e843c1b0b25c | 198 | ,0.000000,0.231129 |
jsoh91 | 2:e843c1b0b25c | 199 | ,1.000000,-3.780626 |
jsoh91 | 2:e843c1b0b25c | 200 | ,0.000000,0.181063 |
jsoh91 | 2:e843c1b0b25c | 201 | ,1.000000,-3.961689 |
jsoh91 | 2:e843c1b0b25c | 202 | ,0.000000,0.141842 |
jsoh91 | 2:e843c1b0b25c | 203 | ,1.000000,-4.103531 |
jsoh91 | 2:e843c1b0b25c | 204 | ,0.000000,0.111117 |
jsoh91 | 2:e843c1b0b25c | 205 | ,1.000000,-4.214648 |
jsoh91 | 2:e843c1b0b25c | 206 | ,0.000000,0.087048; |
jsoh91 | 2:e843c1b0b25c | 207 | |
jsoh91 | 2:e843c1b0b25c | 208 | |
jsoh91 | 2:e843c1b0b25c | 209 | Ba << |
jsoh91 | 2:e843c1b0b25c | 210 | 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 211 | ,0.471698,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 212 | ,-0.471698,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 213 | ,0.369522,0.471698,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 214 | ,-0.841220,-0.471698,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 215 | ,0.289478,0.369522,0.471698,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 216 | ,-1.130698,-0.841220,-0.471698,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 217 | ,0.226773,0.289478,0.369522,0.471698,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 218 | ,-1.357471,-1.130698,-0.841220,-0.471698,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 219 | ,0.177651,0.226773,0.289478,0.369522,0.471698,0.000000,0.000000,0.000000,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 220 | ,-1.535122,-1.357471,-1.130698,-0.841220,-0.471698,0.000000,0.000000,0.000000,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 221 | ,0.139169,0.177651,0.226773,0.289478,0.369522,0.471698,0.000000,0.000000,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 222 | ,-1.674291,-1.535122,-1.357471,-1.130698,-0.841220,-0.471698,0.000000,0.000000,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 223 | ,0.109023,0.139169,0.177651,0.226773,0.289478,0.369522,0.471698,0.000000,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 224 | ,-1.783314,-1.674291,-1.535122,-1.357471,-1.130698,-0.841220,-0.471698,0.000000,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 225 | ,0.085407,0.109023,0.139169,0.177651,0.226773,0.289478,0.369522,0.471698,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 226 | ,-1.868721,-1.783314,-1.674291,-1.535122,-1.357471,-1.130698,-0.841220,-0.471698,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 227 | ,0.066907,0.085407,0.109023,0.139169,0.177651,0.226773,0.289478,0.369522,0.471698,0.000000 |
jsoh91 | 2:e843c1b0b25c | 228 | ,-1.935628,-1.868721,-1.783314,-1.674291,-1.535122,-1.357471,-1.130698,-0.841220,-0.471698,0.000000 |
jsoh91 | 2:e843c1b0b25c | 229 | ,0.052414,0.066907,0.085407,0.109023,0.139169,0.177651,0.226773,0.289478,0.369522,0.471698; |
jsoh91 | 2:e843c1b0b25c | 230 | |
jsoh91 | 2:e843c1b0b25c | 231 | |
jsoh91 | 2:e843c1b0b25c | 232 | Ga << |
jsoh91 | 2:e843c1b0b25c | 233 | 1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 234 | ,0.000000,-0.198113,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 235 | ,1.000000,0.198113,1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 236 | ,0.000000,-0.155199,0.000000,-0.198113,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 237 | ,1.000000,0.353312,1.000000,0.198113,1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 238 | ,0.000000,-0.121581,0.000000,-0.155199,0.000000,-0.198113,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 239 | ,1.000000,0.474893,1.000000,0.353312,1.000000,0.198113,1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 240 | ,0.000000,-0.095245,0.000000,-0.121581,0.000000,-0.155199,0.000000,-0.198113,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 241 | ,1.000000,0.570138,1.000000,0.474893,1.000000,0.353312,1.000000,0.198113,1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 242 | ,0.000000,-0.074613,0.000000,-0.095245,0.000000,-0.121581,0.000000,-0.155199,0.000000,-0.198113,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 243 | ,1.000000,0.644751,1.000000,0.570138,1.000000,0.474893,1.000000,0.353312,1.000000,0.198113,1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 244 | ,0.000000,-0.058451,0.000000,-0.074613,0.000000,-0.095245,0.000000,-0.121581,0.000000,-0.155199,0.000000,-0.198113,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 245 | ,1.000000,0.703202,1.000000,0.644751,1.000000,0.570138,1.000000,0.474893,1.000000,0.353312,1.000000,0.198113,1.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 246 | ,0.000000,-0.045790,0.000000,-0.058451,0.000000,-0.074613,0.000000,-0.095245,0.000000,-0.121581,0.000000,-0.155199,0.000000,-0.198113,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 247 | ,1.000000,0.748992,1.000000,0.703202,1.000000,0.644751,1.000000,0.570138,1.000000,0.474893,1.000000,0.353312,1.000000,0.198113,1.000000,0.000000,0.000000,0.000000,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 248 | ,0.000000,-0.035871,0.000000,-0.045790,0.000000,-0.058451,0.000000,-0.074613,0.000000,-0.095245,0.000000,-0.121581,0.000000,-0.155199,0.000000,-0.198113,0.000000,0.000000,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 249 | ,1.000000,0.784863,1.000000,0.748992,1.000000,0.703202,1.000000,0.644751,1.000000,0.570138,1.000000,0.474893,1.000000,0.353312,1.000000,0.198113,1.000000,0.000000,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 250 | ,0.000000,-0.028101,0.000000,-0.035871,0.000000,-0.045790,0.000000,-0.058451,0.000000,-0.074613,0.000000,-0.095245,0.000000,-0.121581,0.000000,-0.155199,0.000000,-0.198113,0.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 251 | ,1.000000,0.812964,1.000000,0.784863,1.000000,0.748992,1.000000,0.703202,1.000000,0.644751,1.000000,0.570138,1.000000,0.474893,1.000000,0.353312,1.000000,0.198113,1.000000,0.000000 |
jsoh91 | 2:e843c1b0b25c | 252 | ,0.000000,-0.022014,0.000000,-0.028101,0.000000,-0.035871,0.000000,-0.045790,0.000000,-0.058451,0.000000,-0.074613,0.000000,-0.095245,0.000000,-0.121581,0.000000,-0.155199,0.000000,-0.198113; |
jsoh91 | 2:e843c1b0b25c | 253 | |
jsoh91 | 2:e843c1b0b25c | 254 | MatrixXd ref_a = MatrixXd::Zero(20,1); |
jsoh91 | 2:e843c1b0b25c | 255 | MatrixXd w0_a = MatrixXd::Zero(20,1); |
jsoh91 | 2:e843c1b0b25c | 256 | |
jsoh91 | 2:e843c1b0b25c | 257 | MatrixXd At; |
jsoh91 | 2:e843c1b0b25c | 258 | MatrixXd Bt; |
jsoh91 | 2:e843c1b0b25c | 259 | |
jsoh91 | 2:e843c1b0b25c | 260 | MatrixXd X_state = MatrixXd::Zero(2,1); |
jsoh91 | 2:e843c1b0b25c | 261 | |
jsoh91 | 2:e843c1b0b25c | 262 | Bt = ref_a - Aa*X_state - Ga*w0_a; |
jsoh91 | 2:e843c1b0b25c | 263 | At = Ba; |
jsoh91 | 2:e843c1b0b25c | 264 | |
jsoh91 | 2:e843c1b0b25c | 265 | MatrixXd Q = MatrixXd::Identity(20,20); |
jsoh91 | 2:e843c1b0b25c | 266 | MatrixXd R = MatrixXd::Identity(10,10); |
jsoh91 | 2:e843c1b0b25c | 267 | |
jsoh91 | 2:e843c1b0b25c | 268 | MatrixXd H; |
jsoh91 | 2:e843c1b0b25c | 269 | MatrixXd F; |
jsoh91 | 2:e843c1b0b25c | 270 | |
jsoh91 | 2:e843c1b0b25c | 271 | H = At.transpose() * Q * At + R; |
jsoh91 | 2:e843c1b0b25c | 272 | F = -At.transpose()*Bt; |
jsoh91 | 2:e843c1b0b25c | 273 | |
jsoh91 | 2:e843c1b0b25c | 274 | |
jsoh91 | 2:e843c1b0b25c | 275 | MatrixXd out; |
jsoh91 | 2:e843c1b0b25c | 276 | |
jsoh91 | 2:e843c1b0b25c | 277 | MatrixXd Aieq = MatrixXd::Zero(20,10); |
jsoh91 | 2:e843c1b0b25c | 278 | MatrixXd bieq = MatrixXd::Zero(20,1); |
jsoh91 | 2:e843c1b0b25c | 279 | |
jsoh91 | 2:e843c1b0b25c | 280 | Aieq.block(0,0,10,10) = MatrixXd::Identity(10,10); |
jsoh91 | 2:e843c1b0b25c | 281 | Aieq.block(10,0,10,10) = -MatrixXd::Identity(10,10); |
jsoh91 | 2:e843c1b0b25c | 282 | |
jsoh91 | 2:e843c1b0b25c | 283 | for(int i=0; i<10; i++) { |
jsoh91 | 2:e843c1b0b25c | 284 | bieq(i,0) = 10; |
jsoh91 | 2:e843c1b0b25c | 285 | bieq(i+10,0) = 10; |
jsoh91 | 2:e843c1b0b25c | 286 | } |
jsoh91 | 2:e843c1b0b25c | 287 | |
jsoh91 | 2:e843c1b0b25c | 288 | |
jsoh91 | 2:e843c1b0b25c | 289 | while(1) { |
jsoh91 | 2:e843c1b0b25c | 290 | |
jsoh91 | 2:e843c1b0b25c | 291 | js_qp.setNums(10,0,20); |
jsoh91 | 2:e843c1b0b25c | 292 | |
jsoh91 | 2:e843c1b0b25c | 293 | js_qp.make_HF(H,F,1); |
jsoh91 | 2:e843c1b0b25c | 294 | js_qp.make_IEQ(Aieq,bieq); |
jsoh91 | 2:e843c1b0b25c | 295 | |
jsoh91 | 2:e843c1b0b25c | 296 | t.reset(); |
jsoh91 | 2:e843c1b0b25c | 297 | t.start(); |
jsoh91 | 2:e843c1b0b25c | 298 | |
jsoh91 | 2:e843c1b0b25c | 299 | out = js_qp.solve_QP(); |
jsoh91 | 2:e843c1b0b25c | 300 | |
jsoh91 | 2:e843c1b0b25c | 301 | t.stop(); |
jsoh91 | 2:e843c1b0b25c | 302 | |
jsoh91 | 2:e843c1b0b25c | 303 | |
jsoh91 | 2:e843c1b0b25c | 304 | cout << "out : " << endl; |
jsoh91 | 2:e843c1b0b25c | 305 | cout << out << endl; |
jsoh91 | 2:e843c1b0b25c | 306 | cout << "time : " << endl; |
jsoh91 | 2:e843c1b0b25c | 307 | cout << t.read() << endl; |
jsoh91 | 2:e843c1b0b25c | 308 | |
jsoh91 | 2:e843c1b0b25c | 309 | |
jsoh91 | 2:e843c1b0b25c | 310 | wait(1); |
jsoh91 | 2:e843c1b0b25c | 311 | |
jsoh91 | 2:e843c1b0b25c | 312 | } |
GiJeongKim | 0:8a1dc1fdba10 | 313 | } |
GiJeongKim | 0:8a1dc1fdba10 | 314 | |
GiJeongKim | 0:8a1dc1fdba10 | 315 | |
GiJeongKim | 0:8a1dc1fdba10 | 316 | |
jsoh91 | 2:e843c1b0b25c | 317 | void Init_PWM() |
jsoh91 | 2:e843c1b0b25c | 318 | { |
GiJeongKim | 0:8a1dc1fdba10 | 319 | |
GiJeongKim | 0:8a1dc1fdba10 | 320 | RCC->APB2ENR |= RCC_APB2ENR_TIM1EN; // enable TIM1 clock |
GiJeongKim | 0:8a1dc1fdba10 | 321 | |
jsoh91 | 2:e843c1b0b25c | 322 | FastPWM pwm_a(PIN_A); // 단순히 핀 설정용 |
GiJeongKim | 1:27f1640d930d | 323 | FastPWM pwm_b(PIN_B); |
jsoh91 | 2:e843c1b0b25c | 324 | |
jsoh91 | 2:e843c1b0b25c | 325 | //ISR Setup |
jsoh91 | 2:e843c1b0b25c | 326 | |
GiJeongKim | 0:8a1dc1fdba10 | 327 | NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn); //Enable TIM1 IRQ |
GiJeongKim | 0:8a1dc1fdba10 | 328 | |
GiJeongKim | 0:8a1dc1fdba10 | 329 | TIM1->DIER |= TIM_DIER_UIE; // enable update interrupt |
GiJeongKim | 0:8a1dc1fdba10 | 330 | TIM1->CR1 = 0x40; // CMS = 10, interrupt only when counting up // Center-aligned mode |
GiJeongKim | 0:8a1dc1fdba10 | 331 | TIM1->CR1 |= TIM_CR1_UDIS; |
jsoh91 | 2:e843c1b0b25c | 332 | TIM1->CR1 |= TIM_CR1_ARPE; // autoreload on, |
jsoh91 | 2:e843c1b0b25c | 333 | TIM1->RCR |= 0x001; // update event once per up/down count of tim1 |
GiJeongKim | 0:8a1dc1fdba10 | 334 | TIM1->EGR |= TIM_EGR_UG; |
GiJeongKim | 1:27f1640d930d | 335 | |
GiJeongKim | 1:27f1640d930d | 336 | |
jsoh91 | 2:e843c1b0b25c | 337 | |
GiJeongKim | 0:8a1dc1fdba10 | 338 | //PWM Setup |
GiJeongKim | 0:8a1dc1fdba10 | 339 | |
jsoh91 | 2:e843c1b0b25c | 340 | TIM1->PSC = 0; // no prescaler, timer counts up in sync with the peripheral clock |
GiJeongKim | 0:8a1dc1fdba10 | 341 | TIM1->ARR = PWM_ARR; // set auto reload, 40 khz |
GiJeongKim | 1:27f1640d930d | 342 | // TIM1->CCER |= ~(TIM_CCER_CC1NP); // Interupt when low side is on. |
GiJeongKim | 0:8a1dc1fdba10 | 343 | TIM1->CR1 |= TIM_CR1_CEN; // enable TIM1 |
jsoh91 | 2:e843c1b0b25c | 344 | |
jsoh91 | 2:e843c1b0b25c | 345 | // for complementary |
GiJeongKim | 1:27f1640d930d | 346 | TIM1->CCER|=0b1111; |
jsoh91 | 2:e843c1b0b25c | 347 | } |