Jaesung Oh
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;