/* robot_const.c: definisce i parametri dei robot
 *
 * 24/2/2008 Versione originale ....................................... N. Grimaldi
 * 23/7/2008 Introdotta conv_sign ..................................... F. Caccavale
 * 29/04/2009 Corrette qcal e qmin e qmax d conv_offs di S2 e S4        Marino, Pierri
 */

#include <robot_const.h>
#include <mathasm.h>

const struct rbt_data_t rbt_data[RBT_MAX_ROBOTS] = {
  /* COMAU Smart S2 */
  {
    n:           6,
    dh_d:       {0, 0.9, 0, 0.147, 0.65, -0.037, 0.13},
    dh_theta:   {0, 0, 0, 0, 0, 0, 0},
    dh_a:       {0, 0.15, 0.65, 0.1, 0, 0, 0},
    dh_alfa:    {0, M_PI_2, 0, M_PI_2, -M_PI_2, M_PI_2, 0},
    prismatic:  {0, 0, 0, 0, 0, 0},
    qmin:       {-152.5, -105, -65-90, -420, -125, -2700}, //era -65 sul terzo valore 29/04/2009
    qmax:       {152.5, 105, 215-90, 420, 125, 2700},   //era 215 sul terzo valore 29/04/2009
    qcal:       {0, 0, -90, 0, 0, 0}, //era 0 sul terzo valore 29/04/2009
    conv_ofs:   {0, -90, +90, 0, 0, -90}, //era 0 sul terzo valore 29/04/2009
    conv_mul:   {-DEG2RAD, -DEG2RAD, DEG2RAD, -DEG2RAD, -DEG2RAD, -DEG2RAD},
    /* conv_sign introdotta da F. Caccavale il 23/07/08 */
    conv_sign:   {-1, -1, 1, -1, -1, -1},
    gear_ratio: {81.869, 207.114, -60.004, -37.152, 37.152, -80},
    motor_kt:   {0.382, 0.382, 0.382, 0.262, 0.262, 0.262},
    iscale:     {45, 45, 45, 12, 12, 12},
    imax:       {23, 18, 18, 5.5, 4, 4},
    vmax:       {13098/81.869, 23814/207.114, 12000/60.004, 15972/37.152, 13374/37.152, 24000/80.0},
    /***** NOTA: amax sono quelle dello SMART3-S *****/
    amax:       {262, 262, 280, 720, 720, 1450},
    xmin:       {-1.158, -1.158, 0.074, -M_PI, -M_PI, -M_PI},
    xmax:       {1.458, 1.458, 2.208, M_PI, M_PI, M_PI},
    /***** NOTA: guadagni sono quelli tarati da Tecnospazio *****/
    pid_kp:     {0.015, 0.012, 0.01, 0.015, 0.04, 0.02},
    pid_kpv:    {2.74, 2.61, 2.60, 2.60, 2.85, 0.40},
    pid_kv:     {0.50, 0.65, 0.70, 0.60, 0.00, 0.50},
    pid_ki:     {0.0086, 0.0165, 0.025, 0.05, 0.027, 0.01},
    ftmax:      {130, 130, 260, 10, 10, 10},
    ft_ratio:   {10, 200},
    name:       "Robot SX",
    model:      SMART_S2
  },


  /* COMAU Smart S4 */
  {
    n:           6,
    dh_d:       {0, 0.9, 0, 0.1825, 0.64, 0, 0.1},
    dh_theta:   {M_PI, 0, 0, 0, 0, 0, 0},
    dh_a:       {0, 0.15, 0.65, 0.14, 0, 0, 0},
    dh_alfa:    {0, M_PI_2, 0, M_PI_2, -M_PI_2, -M_PI_2, 0},
    prismatic:  {0, 0, 0, 0, 0, 0},
    qmin:       {-152.5, -78, -65-90, -420, -125, -2700},  //era -65 sul terzo valore 29/04/2009
    qmax:       {152.5, 105, 215-90, 420, 125, 2700}, //era 215 sul terzo valore 29/04/2009
    qcal:       {0, 0, -90, 90, 0, 0}, //era 0 sul terzo e quarto valore 29/04/2009
    conv_ofs:   {0, -90, +90, 0, -180, -90}, //era 0 sul terzo valore 29/04/2009
    conv_mul:   {-DEG2RAD, -DEG2RAD, DEG2RAD, -DEG2RAD, -DEG2RAD, -DEG2RAD},
    /* conv_sign introdotta da F. Caccavale il 23/07/08 */
    conv_sign:   {-1, -1, 1, -1, -1, -1},
    gear_ratio: {81.869, 207.114, -60.004, 73.180, 90.380, -90.380},
    motor_kt:   {0.382, 0.382, 0.382, 0.283, 0.283, 0.283},
    iscale:     {45, 45, 45, 12, 12, 12},
    imax:       {23, 18, 18, 5, 5.5, 4},
    vmax:       {13098/81.869, 23814/207.114, 10800/60.004, 20490/73.180, 25302/90.380, 26208/90.380},
    /***** NOTA: amax sono quelle dello SMART3-S *****/
    amax:       {262, 262, 280, 720, 720, 1450},
    xmin:       {-1.141, -1.141, 0.077, -M_PI, -M_PI, -M_PI},
    xmax:       {1.455, 1.455, 2.205, M_PI, M_PI, M_PI},
    /***** NOTA: guadagni sono quelli dello SMART3-S *****/
    /*pid_kp:     {0.10, 0.15, 0.06, 0.075, 0.10, 0.10},
    pid_kpv:    {1.60, 3.20, 2.60, 2.10, 1.10, 0.40},
    pid_kv:     {1.0, 1.0, 1.0, 1.0, 1.0, 1.0},
    pid_ki:     {0.005, 0.0101, 0.0081, 0.0065, 0.0071, 0.0028},  */
    /*NOTA: guadagni sono quelli letti dal file pid_s.obj *****/
    pid_kp:     {0.029, 0.0271, 0.032, 0.01, 0.02, 0.0324}, // {0.029, 0.0271, 0.032, 0.0279, 0.029, 0.0324},
    pid_kpv:    {2.77, 1.5, 1.51, 1.5, 1.5, 0.907}, //{2.77, 1.5, 1.51, 1.89, 2.10, 0.907},
    pid_kv:     {1.0, 1.0, 1.0, 1.0, 1.0, 1.0},
    pid_ki:     {0.0554, 0.024, 0.0453, 0.0568, 0.05, 0.0121},
    ftmax:      {130, 130, 260, 10, 10, 10},
    ft_ratio:   {10, 200},
    name:       "Robot DX",
    model:       SMART_S4
  }

};