May 25, 2020

MMT servos - KSS - Feed Forward velocity calculator

The code implements this using routines called vel_init() and vel_calc(). The original servo (generated with Matlab 2007b) used an SOS filter for this, the newer Matlab (in particular 2012) used a "DTF" filter. My implementation of the SOS filter has been tested, but the DTF filter has not. Given this, I would prefer to use the older SOS implementation.

The following give the filter parameters for each: 

/* Implement a filter which calculates velocity given position.
 * (this is the numdd / dendd filter)
 */

/* Tested OK 8-12-2010 */
/* Tested OK 1-30-2014 (to 2.09E-7) */
struct sos_info *
sos_vel_init ( void )
{
        static double a_data[6] = { 5.4339722579649870E+00, -1.2323933999816752E+01, 1.4932970989366085E+01,
                            -1.0196735972380264E+01, 3.7203798044981644E+00, -5.6665418371664267E-01 };
        static double c_data[6] = { -2.1211305009828152E+01, 9.7139573942412653E+01, -1.7841465511416345E+02,
                            1.6431763855865029E+02, -7.5899825782665289E+01, 1.4068412605619130E+01 };
        static double d_data = 1.4564101417467634E+02;

        return sos_init ( a_data, c_data, d_data, 6 );
}
And here is the setup for the newer DTF implementation. 
struct dtf_info *
vel_init ( void )
{
        static double num[7] =
          { 145.64101417467634, -812.62053565690485, 1892.0098202975, -2353.26769464646,
          1649.3806068475164, -617.73971362476209, 96.596502608434335 };

        static double den[7] =
            { 1.0, -5.433972257964987, 12.323933999816752, -14.932970989366085,
            10.196735972380264, -3.7203798044981644, 0.56665418371664267 };

        return dtf_init ( num, den, 7 );
}