/*************************************************************
*     pwm_module.h  RT-Task Module's Parameter Ver.1.0       *
*                                          Date:14 May 2004  *
*                                Programed by Hiroki.Segawa  *
*************************************************************/

/* parameter */
#define Vmax     7.8   /* Maximum Voltage */
#define Vmin     5     /* Minimum Voltage */
#define vshift   25    /* Shift of Duty */
#define drshift  45    /* Shift of right Duty */
#define dlshift  45    /* Shift of left Duty */
#define maxcount 2000  /* counter maximum */
#define dmax     127   /* limit of duty */
#define errmax   0.02  /* error of control */
#define Kpl      0.1   /* PID-Control gain of right */
#define Kpr      0.1   /* PID-Control gain of right */

/* parameter of lqr control */
static float Ad[][6] = {
    {0.9896, 0.0019, 0.0088, 0.0001, 0.0470, -0.0120},
    {0.0026, 0.9915, 0.0002, 0.0090, -0.0146, 0.0366},
    {-1.9895, 0.3623, 0.7683, 0.0270, 8.9849, -2.2592},
    {0.4800, -1.6408, 0.0390, 0.7982, -2.7489, 7.0408},
    {0.0, 0.0, 0.0, 0.0, 1.0000, 0.0},
    {0.0, 0.0, 0.0, 0.0, 0.0, 1.0000}};

static float Bd[][4] = {
    {0.0002, -0.0000, -0.0002, 0.0000},
    {-0.0001, 0.0001, 0.0000, -0.0001},
    {0.0470, -0.0120, -0.0470, 0.0120},
    {-0.0146, 0.0366, 0.0146, -0.0366},
    {0.0100, 0.0, -0.0100, 0.0},
    {0.0, 0.0100, 0.0, -0.0100}};

static float Cd[][6] = {
    {-7.1366, -0.3740, -0.3903, -0.0966, 31.6034, -1.1062},
    {-0.8912, -7.6375, -0.1609, -0.5092, 1.1062, 31.6034}};