/*****************************************************************************/
/*  Hopper control program */
/*****************************************************************************/

#include <stdio.h>
#include <math.h>
#include <time.h>

extern "C" {
#include "struct.h"
#include "swing.h"
}

/************************************************************************/
/*      main servo algorithm                                            */
/************************************************************************/
void servo1 ()
{

    tau.neck_y = -(ls.k_necky * (st.neck_y - st_d.neck_y)
		+ ls.k_neckyd * st.neck_yd);
    tau.hipl_y = -(ls.k_hipy * (st.hipl_y - st_d.hipl_y)
		+ ls.k_hipyd * st.hipl_yd);
	tau.kneel_y = -(ls.k_kneey * (st.kneel_y - st_d.kneel_y)
    	+ ls.k_kneeyd * st.kneel_yd);
    tau.hipr_y = -(ls.k_hipy * (st.hipr_y - st_d.hipr_y) 
		+ ls.k_hipyd * st.hipr_yd);
	tau.kneer_y = -(ls.k_kneey * (st.kneer_y - st_d.kneer_y)
    	+ ls.k_kneeyd * st.kneer_yd);
    tau.shldl_y = -(ls.k_shldy * (st.shldl_y - st_d.shldl_y)
		+ ls.k_shldyd * st.shldl_yd);
    tau.elbl_y = -(ls.k_elby * (st.elbl_y - st_d.elbl_y)
		+ ls.k_elbyd * st.elbl_yd);
    tau.wrstl_y = -(ls.k_wrsty * (st.wrstl_y - st_d.wrstl_y)
		+ ls.k_wrstyd * st.wrstl_yd);
    tau.shldr_y = -(ls.k_shldy * (st.shldr_y - st_d.shldr_y)
		+ ls.k_shldyd * st.shldr_yd);
    tau.elbr_y = -(ls.k_elby * (st.elbr_y - st_d.elbr_y)
		+ ls.k_elbyd * st.elbr_yd);
   	tau.wrstr_y = -(ls.k_wrsty * (st.wrstr_y - st_d.wrstr_y)
		+ ls.k_wrstyd * st.wrstr_yd);
    tau.anklel_y = -(ls.k_ankley * (st.anklel_y - st_d.anklel_y)
		+ ls.k_ankleyd * st.anklel_yd);
    tau.anklel_y = -(ls.k_ankley * (st.anklel_y - st_d.anklel_y)
		+ ls.k_ankleyd * st.anklel_yd);
    tau.ankler_y = -(ls.k_ankley * (st.ankler_y - st_d.ankler_y)
		+ ls.k_ankleyd * st.ankler_yd);

}