HCB with MPC
Dependencies: mbed Eigen FastPWM
Diff: main.cpp
- Revision:
- 25:f83396e3d25c
- Parent:
- 24:ef6e1092e9e6
--- a/main.cpp Thu Sep 26 06:08:32 2019 +0000 +++ b/main.cpp Mon Oct 14 10:08:13 2019 +0000 @@ -1,3 +1,163 @@ + +//JS MODI +#include "quadprog.h" +#include <cstdlib> +#include "Eigen/Dense.h" + +using namespace std; +using namespace Eigen; + +class JS_QP +{ +private: + int NUMCOLS;//length of X + int NUMEQ; + int NUMINEQ; + + MatrixXd A_ineq,B_ineq; + MatrixXd A_eq,B_eq; + +public: + MatrixXd H,F; + MatrixXd X; + +public: + void setNums(int _xlength, int numEqConstraints, int numIneqConstraints) + { + NUMCOLS = _xlength;//should be 18+12+3*contact + NUMEQ = numEqConstraints;//NUMCOLS(dynamics) + NUMCOLS(contactconstraints + swingleg) + //contact to be weight? + NUMINEQ = numIneqConstraints;//friction cone approximation -ufz<fx<ufz, -ufz<fy<ufz ->4 + + //maybe matrix assignment here + X = MatrixXd::Zero(NUMCOLS,1); + A_eq = MatrixXd::Zero(NUMEQ, NUMCOLS); + B_eq = MatrixXd::Zero(NUMEQ, 1); + + A_ineq = MatrixXd::Zero(NUMINEQ, NUMCOLS); + B_ineq = MatrixXd::Zero(NUMINEQ, 1); + + + H = MatrixXd::Zero(NUMCOLS,NUMCOLS); + F = MatrixXd::Zero(NUMCOLS,1); + } + + void make_EQ(MatrixXd A, MatrixXd b) + { + //Aeq*X = Beq + A_eq = A; + B_eq = b; + } + + void make_IEQ(MatrixXd A, MatrixXd b) + { + // Aineq*X < Bineq + A_ineq = A; + B_ineq = b; + } + + void make_HF(MatrixXd A, MatrixXd b, int mode=-1) + { + //min (0.5* x H x + F x) + if(mode == -1) { + H = A.transpose() * A + MatrixXd::Identity(NUMCOLS,NUMCOLS)*1e-3; + F = -A.transpose() * b; + + } else { + H = A; + F = b; + + } + + } + + MatrixXd solve_QP() + { + quadprogpp::Vector<double> outX; + quadprogpp::Matrix<double> G; + quadprogpp::Vector<double> g0; + quadprogpp::Matrix<double> CE; + quadprogpp::Vector<double> ce0; + quadprogpp::Matrix<double> CI; + quadprogpp::Vector<double> ci0; + //min 0.5 * x G x + g0 x + //CE^T x + ce0 = 0 + //CI^T x + ci0 >= 0 + + G.resize(NUMCOLS,NUMCOLS); + g0.resize(NUMCOLS); + for(int i=0; i<NUMCOLS; i++) { + for(int j=0; j<NUMCOLS; j++) { + G[i][j] = H(i,j); + } + g0[i] = F(i,0); + } + CE.resize(NUMCOLS,NUMEQ); + ce0.resize(NUMEQ); + for(int i=0; i<NUMCOLS; i++) { + for(int j=0; j<NUMEQ; j++) { + CE[i][j] = -A_eq(j,i); + } + } + for(int j=0; j<NUMEQ; j++) { + ce0[j] = B_eq(j,0); + } + CI.resize(NUMCOLS,NUMINEQ); + ci0.resize(NUMINEQ); + for(int i=0; i<NUMCOLS; i++) { + for(int j=0; j<NUMINEQ; j++) { + CI[i][j] = -A_ineq(j,i); + } + } + for(int j=0; j<NUMINEQ; j++) { + ci0[j] = B_ineq(j,0); + } + outX.resize(NUMCOLS); + + solve_quadprog(G, g0, CE, ce0, CI, ci0, outX); + for(int i=0; i<NUMCOLS; i++) { + X(i,0) = outX[i]; + } + + return X; + + } + +public: + +}; + +JS_QP js_qp; + + +MatrixXd Aa = MatrixXd::Zero(20,2); +MatrixXd Ba = MatrixXd::Zero(20,10); +MatrixXd Ga = MatrixXd::Zero(20,20); + +MatrixXd ref_a = MatrixXd::Zero(20,1); +MatrixXd w0_a = MatrixXd::Zero(20,1); + +MatrixXd At; +MatrixXd Bt; + +MatrixXd X_state = MatrixXd::Zero(2,1); + +MatrixXd Q = MatrixXd::Identity(20,20); +MatrixXd R = MatrixXd::Identity(10,10); + +MatrixXd H; +MatrixXd F; + +MatrixXd out; + +MatrixXd Aieq = MatrixXd::Zero(20,10); +MatrixXd bieq = MatrixXd::Zero(20,1); + +float iq_err_sum = 0.0; +MatrixXd i_ref = MatrixXd::Zero(10,1); +float iq = 0.0; + +//original #include "mbed.h" #include "FastPWM.h" #include "INIT_HW.h" @@ -9,6 +169,8 @@ #include "stm32f4xx_flash.h" #include "FlashWriter.h" +Timer t; + // dac & check /////////////////////////////////////////// DigitalOut check(PC_2); DigitalOut check_2(PC_3); @@ -190,15 +352,15 @@ can.attach(&CAN_RX_HANDLER); CAN_ID_INIT(); make_delay(); - + //Timer priority NVIC_SetPriority(TIM3_IRQn, 2); NVIC_SetPriority(TIM2_IRQn, 3); NVIC_SetPriority(TIM4_IRQn, 4); - + //can.reset(); can.filter(msg.id, 0xFFFFF000, CANStandard); - + // spi _ enc spi_enc_set_init(); make_delay(); @@ -215,14 +377,102 @@ ID_index_array[i] = (i+1) * 0.5; } + + +//JS MODI + + + Aa << + 1.000000,-1.000000 + ,0.000000,-0.666667 + ,1.000000,-0.333333 + ,0.000000,0.444444 + ,1.000000,-0.777778 + ,0.000000,-0.296296 + ,1.000000,-0.481481 + ,0.000000,0.197531 + ,1.000000,-0.679012 + ,0.000000,-0.131687 + ,1.000000,-0.547325 + ,0.000000,0.087791 + ,1.000000,-0.635117 + ,0.000000,-0.058528 + ,1.000000,-0.576589 + ,0.000000,0.039018 + ,1.000000,-0.615607 + ,0.000000,-0.026012 + ,1.000000,-0.589595 + ,0.000000,0.017342; + + + Ba << + 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 + ,0.066667,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 + ,-0.066667,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 + ,-0.044444,0.066667,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 + ,-0.022222,-0.066667,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 + ,0.029630,-0.044444,0.066667,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 + ,-0.051852,-0.022222,-0.066667,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 + ,-0.019753,0.029630,-0.044444,0.066667,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 + ,-0.032099,-0.051852,-0.022222,-0.066667,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 + ,0.013169,-0.019753,0.029630,-0.044444,0.066667,0.000000,0.000000,0.000000,0.000000,0.000000 + ,-0.045267,-0.032099,-0.051852,-0.022222,-0.066667,0.000000,0.000000,0.000000,0.000000,0.000000 + ,-0.008779,0.013169,-0.019753,0.029630,-0.044444,0.066667,0.000000,0.000000,0.000000,0.000000 + ,-0.036488,-0.045267,-0.032099,-0.051852,-0.022222,-0.066667,0.000000,0.000000,0.000000,0.000000 + ,0.005853,-0.008779,0.013169,-0.019753,0.029630,-0.044444,0.066667,0.000000,0.000000,0.000000 + ,-0.042341,-0.036488,-0.045267,-0.032099,-0.051852,-0.022222,-0.066667,0.000000,0.000000,0.000000 + ,-0.003902,0.005853,-0.008779,0.013169,-0.019753,0.029630,-0.044444,0.066667,0.000000,0.000000 + ,-0.038439,-0.042341,-0.036488,-0.045267,-0.032099,-0.051852,-0.022222,-0.066667,0.000000,0.000000 + ,0.002601,-0.003902,0.005853,-0.008779,0.013169,-0.019753,0.029630,-0.044444,0.066667,0.000000 + ,-0.041040,-0.038439,-0.042341,-0.036488,-0.045267,-0.032099,-0.051852,-0.022222,-0.066667,0.000000 + ,-0.001734,0.002601,-0.003902,0.005853,-0.008779,0.013169,-0.019753,0.029630,-0.044444,0.066667; + + + + Ga << + 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 + ,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,0.000000 + ,1.000000,0.000000,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,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 + ,1.000000,0.000000,1.000000,0.000000,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,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 + ,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,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,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 + ,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,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,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 + ,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,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,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 + ,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,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,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000 + ,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,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,0.000000,-0.000000,0.000000,0.000000,0.000000,0.000000 + ,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,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,0.000000,-0.000000,0.000000,0.000000 + ,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,1.000000,0.000000,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,0.000000,-0.000000; + + + + Aieq.block(0,0,10,10) = MatrixXd::Identity(10,10); + Aieq.block(10,0,10,10) = -MatrixXd::Identity(10,10); + + for(int i=0; i<10; i++) { + bieq(i,0) = 5.0; + bieq(i+10,0) = 5.0; + } + + for(int i=0; i<10; i++) { + i_ref(i,0) = 10e-3; + } + /************************************ *** Program is operating! *************************************/ while(1) { if(timer_while==1000) { //i2c - read_field(i2c_slave_addr1); - if(DIR_VALVE_ENC < 0) value = 1023 - value; + //read_field(i2c_slave_addr1); + //if(DIR_VALVE_ENC < 0) value = 1023 - value; // if(LED==1) { // LED=0; @@ -278,8 +528,8 @@ } else if(REF_VALVE_POS < VALVE_MIN_POS) { REF_VALVE_POS = VALVE_MIN_POS; } - - + + // if(REF_VALVE_POS >= VALVE_POS_VS_PWM[0]) // { // if(REF_VALVE_POS <= VALVE_POS_VS_PWM[1]) { @@ -412,7 +662,7 @@ double alpha_update_cur = 1.0/(1.0+(FREQ_TMR4/2.0)/(2.0*3.14*1000.0)); // f_cutoff : 500Hz double cur_new = ((double)ADC3->DR-2048.0)*20.0/4096.0; // unit : mA cur.sen=cur.sen*(1.0-alpha_update_cur)+cur_new*(alpha_update_cur); - cur.sen = raw_cur; +// cur.sen = raw_cur; /******************************************************* *** Timer Counting & etc. @@ -434,6 +684,13 @@ if (TIM3->SR & TIM_SR_UIF ) { ENC_UPDATE(); + + + + + + + // CONTROL LOOP ------------------------------------------------------------ switch (CONTROL_MODE) { @@ -493,8 +750,8 @@ } case MODE_JOINT_POSITION_TORQUE_CONTROL_VALVE_POSITION: { - - + + double VALVE_POS_RAW_POS_FB = 0.0; // Valve Position by Position Feedback //double VALVE_POS_RAW_POS_FF = 0.0; // Valve Position by Position Feedforward double VALVE_POS_RAW_FORCE_FB = 0.0; // Valve Position by Force Feedback @@ -532,8 +789,8 @@ valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_JOINT_POS_FF(vel.ref) + VALVE_POS_RAW_FORCE_FB; //valve_pos.ref = VALVE_POS_RAW_POS_FB + DDV_CENTER; VALVE_POS_CONTROL(valve_pos.ref); - - + + break; } @@ -1622,6 +1879,37 @@ V_out = V_out; } + + iq = cur.sen*0.001; + iq_err_sum += i_ref(0,0) - iq; + X_state(0,0) = iq_err_sum; + X_state(1,0) = iq; + + for (int i=0; i<10; i++) { + w0_a(i*2,0) = i_ref(i,0); + w0_a(i*2+1,0) = 0.0; + + ref_a(i*2,0) = 0.0; + ref_a(i*2+1,0) = i_ref(i,0); + } + + Bt = ref_a - Aa*X_state - Ga*w0_a; + At = Ba; + + + H = At.transpose() * Q * At + R; + F = -At.transpose()*Bt; + + js_qp.setNums(10,0,20); + + js_qp.make_HF(H,F,1); + js_qp.make_IEQ(Aieq,bieq); + + + + out = js_qp.solve_QP(); + V_out = out(0,0) * 1000.0; + /******************************************************* *** PWM ********************************************************/ @@ -1647,6 +1935,9 @@ TIM4->CCR2 = (PWM_ARR)*(1.0-dtc_v); TIM4->CCR1 = (PWM_ARR)*(1.0-dtc_w); + + + CAN_TX_POSITION((int32_t) V_out, (int32_t) (cur.sen * 10.0)); } TIM3->SR = 0x0; // reset the status register @@ -1664,7 +1955,8 @@ //CAN ---------------------------------------------------------------------- if (flag_data_request[0] == HIGH) { //position+velocity - CAN_TX_POSITION((int32_t) pos.sen, (int32_t) vel.sen); + //CAN_TX_POSITION((int32_t) pos.sen, (int32_t) vel.sen); + CAN_TX_POSITION((int32_t) V_out, (int32_t) cur.sen); //CAN_TX_POSITION((int32_t) valve_pos.ref, (int32_t) 0); //CAN_TX_POSITION((int32_t) VALVE_PWM_RAW_FF, (int32_t) VALVE_POS_VS_PWM[10]); //pc.printf("can good"); @@ -1719,7 +2011,6 @@ } - void CurrentControl() { cur.err = cur.ref - cur.sen;