00001 /********************************************************************* 00002 * Copyright (c) 2015 Robosavvy Ltd. 00003 * Author: Vitor Matos 00004 * 00005 *********************************************************************/ 00006 00007 #ifndef RSV_BALANCE_GAZEBO_CONTROL_BALANCE_GAZEBO_CONTROL_H 00008 #define RSV_BALANCE_GAZEBO_CONTROL_BALANCE_GAZEBO_CONTROL_H 00009 00010 #include "rsv_balance_gazebo_control/control.h" 00011 00012 namespace balance_control 00013 { 00014 00015 enum STATES 00016 { 00017 theta, 00018 dx, 00019 dphi, 00020 dtheta 00021 }; 00022 00023 enum INPUTS 00024 { 00025 tauL, 00026 tauR 00027 }; 00028 00034 class BalanceControl 00035 { 00036 public: 00037 BalanceControl(); 00038 00039 void resetControl(); 00040 void stepControl(double dt, const double (&x_desired)[4], const double (&y_fbk)[4]); 00041 double *getControl(); 00042 00043 private: 00044 double t; 00045 double x_hat[4]; 00046 double dx_hat[4]; 00047 double x_reference[4]; 00048 double x_r[4]; 00049 double x_adjust[4]; 00050 double u_output[2]; 00051 }; 00052 00053 } // namespace balance_control 00054 00055 #endif // RSV_BALANCE_GAZEBO_CONTROL_BALANCE_GAZEBO_CONTROL_H