convert.c

hoap2 robot source code

#include <math.h>
#include "convert.h"

int jp[20][3] = {
/*	 S	pls	R	*/
	{1,	440,	171}, //RLEG_JOINT1
	{1,	440,	171}, //RLEG_JOINT2
	{-1,	440,	171}, //RLEG_JOINT3
	{1,	440,	171}, //RLEG_JOINT4
	{1,	440,	171}, //RLEG_JOINT5
	{-1,	440,	171}, //RLEG_JOINT6
	{1,	440,	171}, //RARM_JOINT1
	{1,	440,	171}, //RARM_JOINT2
	{-1,	440,	144}, //RARM_JOINT3
	{-1,	440,	144}, //RARM_JOINT4
	{1,	440,	171}, //LLEG_JOINT1
	{1,	440,	171}, //LLEG_JOINT2
	{1,	440,	171}, //LLEG_JOINT3
	{-1,	440,	171}, //LLEG_JOINT4
	{-1,	440,	171}, //LLEG_JOINT5
	{-1,	440,	171}, //LLEG_JOINT6
	{-1,	440,	171}, //LARM_JOINT1
	{1,	440,	171}, //LRAM_JOINT2
	{-1,	440,	144}, //LARM_JOINT3
	{1,	440,	144}  //LARM_JOINT4
};

double A(int part)  //pulse/deg
{
	return (jp[part][PLS] * jp[part][R] / 360);
}

double B(int part)  //pulse/rad
{
	return (jp[part][PLS] * jp[part][R] / (2*M_PI));
}


int DegToPulse(double deg, int part)
{
	long double ret;
 
	part--;
	ret = jp[part][S] * deg * A(part);
	if(ret > 0) ret += 0.5;
	if(ret < 0) ret -= 0.5;
	return (int)ret;
//	return (int)(jp[part][S] * deg * A(part));
}

int RadToPulse(double rad, int part)
{
	long double ret;

	part--;
	ret = jp[part][S] * rad * A(part);
	if(ret > 0) ret += 0.5;
	if(ret < 0) ret -= 0.5;
	return (int)ret;

//	return (int)(jp[part][S] * rad * B(part));
}

double PulseToDeg(int pulse, int part)
{
	part--;
	return (double)(pulse * jp[part][S] / A(part));
}

double PulseToRad(int pulse, int part)
{
	part--;
	return (double)(pulse * jp[part][S] / B(part));
}