control.cpp

Go to the documentation of this file.

/*********************************************************************
 *  Copyright (c) 2015 Robosavvy Ltd.
 *  Author: Vitor Matos
 *
 *********************************************************************/
#include "rsv_balance_gazebo_control/control.h"

const float g_fWheelRadius = 0.19;
const float g_fBaseWidth   = 0.5;
const float g_fI3 = .85;

const float A[4][4] = {
  { 0, 0, 0, 1 },
  { -1.2540568855640422, 0, 0, 0 },
  { 0, 0, 0, 0 },
  { 6.059985836147613, 0, 0, 0 }};

const float B[4][2] = {
  { 0.0, 0.0 },
  { 0.1635280961687897, 0.1635280961687897 },
  { -0.09719802837298575, 0.09719802837298575 },
  { -0.15573631660299134, -0.15573631660299134 }};

const float C[4][4] = {
  { 1.0, 0.0, 0.0, 0.0 },
  { 0.0, 1.0, 0.0, 0.0 },
  { 0.0, 0.0, 1.0, 0.0 },
  { 0.0, 0.0, 0.0, 1.0 }};

const float L[4][4] = {
  { 28.04011263,  -2.25919745,  -0.        ,  10.92301101 },
  { -2.25919745,  28.93520042,  -0.        ,   5.07012978 },
  { 0.,  0.,  20.,  0. },
  { 10.92301101,   5.07012978,  -0.        ,   5.48639076 }};

const float K[2][4] = {
  {-117.29162704,  -14.14213562,   -7.07106781,  -53.23791934},
  {-117.29162704,  -14.14213562,    7.07106781,  -53.23791934}};

const float A_BK[4][4] = {
  {  0.        ,   0.        ,   0.        ,   1.        },
  { 37.10689605,   4.62527303,   0.        ,  17.41179119},
  { 0.        ,   0.        ,  -1.3745877 ,   0.        },
  {-30.47314609,  -4.40488822,  -0.        , -16.58215492 }};