File content as of revision 2:e843c1b0b25c:

#include "mbed.h"
#include "FastPWM.h"

#define PIN_A PA_8 // channel 1
#define PIN_B PA_7 // complementary
#define PWM_ARR 4500  /// timer autoreload value 40k loop, 40k pwm 

#include "quadprog.h"
#include <cstdlib>
#include "Eigen/Dense.h"

using namespace std;
using namespace Eigen;

MatrixXd AA = MatrixXd::Zero(6,1);

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;


float dtc_a=0;
void Init_PWM();

AnalogIn Vfb(PA_5);

extern "C" void TIM1_UP_TIM10_IRQHandler(void)
{
    if (TIM1->SR & TIM_SR_UIF ) {

        float Vth = 0.51f;

        if(Vfb >= Vth) {    //PA_8 이게 하이
            dtc_a -= 0.001f;
        }

        else {
            dtc_a += 0.001f;
        }

        if(dtc_a>=0.9f)
            dtc_a = 0.9f;

        if(dtc_a <= 0.0f)
            dtc_a = 0.0f;

        dtc_a = 0.5f;

        TIM1->CCR1 = (PWM_ARR)*(1.0f-dtc_a);                        // Write duty cycles

    }
    TIM1->SR = 0x0;  // reset the status register
}

Timer t;

double t_old=0;

int main()
{

    MatrixXd Aa = MatrixXd::Zero(20,2);
    MatrixXd Ba = MatrixXd::Zero(20,10);
    MatrixXd Ga = MatrixXd::Zero(20,20);

    Aa <<
       1.000000,-1.000000
       ,0.000000,0.783386
       ,1.000000,-1.783386
       ,0.000000,0.613693
       ,1.000000,-2.397079
       ,0.000000,0.480759
       ,1.000000,-2.877838
       ,0.000000,0.376620
       ,1.000000,-3.254458
       ,0.000000,0.295039
       ,1.000000,-3.549496
       ,0.000000,0.231129
       ,1.000000,-3.780626
       ,0.000000,0.181063
       ,1.000000,-3.961689
       ,0.000000,0.141842
       ,1.000000,-4.103531
       ,0.000000,0.111117
       ,1.000000,-4.214648
       ,0.000000,0.087048;


    Ba <<
       0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
       ,0.471698,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
       ,-0.471698,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
       ,0.369522,0.471698,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
       ,-0.841220,-0.471698,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
       ,0.289478,0.369522,0.471698,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
       ,-1.130698,-0.841220,-0.471698,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
       ,0.226773,0.289478,0.369522,0.471698,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
       ,-1.357471,-1.130698,-0.841220,-0.471698,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000
       ,0.177651,0.226773,0.289478,0.369522,0.471698,0.000000,0.000000,0.000000,0.000000,0.000000
       ,-1.535122,-1.357471,-1.130698,-0.841220,-0.471698,0.000000,0.000000,0.000000,0.000000,0.000000
       ,0.139169,0.177651,0.226773,0.289478,0.369522,0.471698,0.000000,0.000000,0.000000,0.000000
       ,-1.674291,-1.535122,-1.357471,-1.130698,-0.841220,-0.471698,0.000000,0.000000,0.000000,0.000000
       ,0.109023,0.139169,0.177651,0.226773,0.289478,0.369522,0.471698,0.000000,0.000000,0.000000
       ,-1.783314,-1.674291,-1.535122,-1.357471,-1.130698,-0.841220,-0.471698,0.000000,0.000000,0.000000
       ,0.085407,0.109023,0.139169,0.177651,0.226773,0.289478,0.369522,0.471698,0.000000,0.000000
       ,-1.868721,-1.783314,-1.674291,-1.535122,-1.357471,-1.130698,-0.841220,-0.471698,0.000000,0.000000
       ,0.066907,0.085407,0.109023,0.139169,0.177651,0.226773,0.289478,0.369522,0.471698,0.000000
       ,-1.935628,-1.868721,-1.783314,-1.674291,-1.535122,-1.357471,-1.130698,-0.841220,-0.471698,0.000000
       ,0.052414,0.066907,0.085407,0.109023,0.139169,0.177651,0.226773,0.289478,0.369522,0.471698;


    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.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
       ,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
       ,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
       ,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
       ,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
       ,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
       ,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
       ,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
       ,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
       ,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
       ,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
       ,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
       ,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
       ,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
       ,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
       ,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
       ,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
       ,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
       ,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;

    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);

    Bt = ref_a - Aa*X_state - Ga*w0_a;
    At = Ba;

    MatrixXd Q = MatrixXd::Identity(20,20);
    MatrixXd R = MatrixXd::Identity(10,10);

    MatrixXd H;
    MatrixXd F;

    H = At.transpose() * Q * At + R;
    F = -At.transpose()*Bt;


    MatrixXd out;

    MatrixXd Aieq = MatrixXd::Zero(20,10);
    MatrixXd bieq = MatrixXd::Zero(20,1);

    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) = 10;
        bieq(i+10,0) = 10;
    }


    while(1) {

        js_qp.setNums(10,0,20);

        js_qp.make_HF(H,F,1);
        js_qp.make_IEQ(Aieq,bieq);

        t.reset();
        t.start();

        out = js_qp.solve_QP();

        t.stop();


        cout << "out : " << endl;
        cout << out << endl;
        cout << "time : " << endl;
        cout << t.read() << endl;


        wait(1);

    }
}



void Init_PWM()
{

    RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;                         // enable TIM1 clock

    FastPWM pwm_a(PIN_A);                                       // 단순히 핀 설정용
    FastPWM pwm_b(PIN_B);

    //ISR Setup

    NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn);                         //Enable TIM1 IRQ

    TIM1->DIER |= TIM_DIER_UIE;                                 // enable update interrupt
    TIM1->CR1 = 0x40;                                           // CMS = 10, interrupt only when counting up // Center-aligned mode
    TIM1->CR1 |= TIM_CR1_UDIS;
    TIM1->CR1 |= TIM_CR1_ARPE;                                  // autoreload on,
    TIM1->RCR |= 0x001;                                         // update event once per up/down count of tim1
    TIM1->EGR |= TIM_EGR_UG;



    //PWM Setup

    TIM1->PSC = 0;                                            // no prescaler, timer counts up in sync with the peripheral clock
    TIM1->ARR = PWM_ARR;                                          // set auto reload, 40 khz
//    TIM1->CCER |= ~(TIM_CCER_CC1NP);                            // Interupt when low side is on.
    TIM1->CR1 |= TIM_CR1_CEN;                                   // enable TIM1

    // for complementary
    TIM1->CCER|=0b1111;
}