Jaesung Oh / Mbed 2 deprecated MPC_current_control

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?

UserRevisionLine numberNew 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 }