File content as of revision 1:584f36ed8717:

#include "mbed.h"
#include "dynamixel.h"
#include "RobotControl_7Axis.h"

extern Serial pc;


#if (DEBUG)
	#define DBGMSG(x)  pc.printf x;
#else
    #define DBGMSG(x)
#endif



unsigned char getMapAxisNO(unsigned char index)
{
	return gMapAxisNO[index];

}

unsigned char getMapAxisID(unsigned char index)
{
	return gMapAxisID[index];
}


int ROM_Setting() //new
{
	//==calculate Max torque to set in rom 
	const short int Max_torque[MAX_AXIS_NUM]=
	{
		AXIS1_MAX_TORQUE,
		AXIS2_MAX_TORQUE,
		AXIS3_MAX_TORQUE,
		AXIS4_MAX_TORQUE,
		AXIS5_MAX_TORQUE,
		AXIS6_MAX_TORQUE,
		AXIS7_MAX_TORQUE
	};


	//==Calculate angle limit==//
	const short int R2M_OFFSET_DEG[MAX_AXIS_NUM]=
	{
		AXIS1_R2M_OFFSET_DEG,
		AXIS2_R2M_OFFSET_DEG,
		AXIS3_R2M_OFFSET_DEG,
		AXIS4_R2M_OFFSET_DEG,
		AXIS5_R2M_OFFSET_DEG,
		AXIS6_R2M_OFFSET_DEG,
		AXIS7_R2M_OFFSET_DEG
	};

	const short int ROBOT_LIM_DEG_L[MAX_AXIS_NUM]=
	{
		AXIS1_ROBOT_LIM_DEG_L,
		AXIS2_ROBOT_LIM_DEG_L,
		AXIS3_ROBOT_LIM_DEG_L,
		AXIS4_ROBOT_LIM_DEG_L,
		AXIS5_ROBOT_LIM_DEG_L,
		AXIS6_ROBOT_LIM_DEG_L,
		AXIS7_ROBOT_LIM_DEG_L
	};

	const short int ROBOT_LIM_DEG_H[MAX_AXIS_NUM]=
	{
		AXIS1_ROBOT_LIM_DEG_H,
		AXIS2_ROBOT_LIM_DEG_H,
		AXIS3_ROBOT_LIM_DEG_H,
		AXIS4_ROBOT_LIM_DEG_H,
		AXIS5_ROBOT_LIM_DEG_H,
		AXIS6_ROBOT_LIM_DEG_H,
		AXIS7_ROBOT_LIM_DEG_H
	};

	unsigned short int Motor_lim_pulse_h[MAX_AXIS_NUM]={0};
	unsigned short int Motor_lim_pulse_l[MAX_AXIS_NUM]={0};
	

	int i=0;
	for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
	{
		Motor_lim_pulse_l[i]=(ROBOT_LIM_DEG_L[i]+R2M_OFFSET_DEG[i])*DEF_RATIO_DEG_TO_PUS;
		Motor_lim_pulse_h[i]=(ROBOT_LIM_DEG_H[i]+R2M_OFFSET_DEG[i])*DEF_RATIO_DEG_TO_PUS;
	}



	//==writing to ROM==//
	for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
	{
		//==Set MAX_torgue==//
		dxl_write_word(gMapAxisID[i],MAX_TORQUE,Max_torque[i]);//  more safe
	
		//==Set angel limit==//
		dxl_write_word(gMapAxisID[i],CW_ANGLE_LIMIT_L,Motor_lim_pulse_l[i]);
		dxl_write_word(gMapAxisID[i],CCW_ANGLE_LIMIT_L,Motor_lim_pulse_h[i]);  

		//==Set MULTITURN_OFFSET to 0==//
		dxl_write_word(gMapAxisID[i],MULTITURN_OFFSET,0);
	}
	

	//==read and check ==//
	int	txrx_result=0;
	short int max_torque=0;
	short int cw_angel_lim=0,ccw_angle_lim=0;
	short int multi_turn_offset=0;
	for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
	{
		pc.printf("===AXIS_%d===\n",gMapAxisNO[i]);

		//==MAX_torgue==//
		max_torque = dxl_read_word(gMapAxisID[i], MAX_TORQUE);
		txrx_result = dxl_get_result();
		if(txrx_result!=COMM_RXSUCCESS)
			pc.printf("Failed read MAX_TORQUE error=%d\n",txrx_result);
		else
			pc.printf("MAX_TORQUE=%d\n",max_torque);
	
		//==CW_ANGLE_LIMIT,CCW_ANGLE_LIMIT==//
		cw_angel_lim=dxl_read_word(gMapAxisID[i],CW_ANGLE_LIMIT_L);
		txrx_result = dxl_get_result();
		if(txrx_result!=COMM_RXSUCCESS)
			pc.printf("Failed read CW_ANGLE_LIMIT error=%d\n",txrx_result);
		else	
			pc.printf("CW_ANGLE_LIMIT=%d,degree=%f\n",cw_angel_lim,cw_angel_lim*DEF_RATIO_PUS_TO_DEG);

		ccw_angle_lim=dxl_read_word(gMapAxisID[i],CCW_ANGLE_LIMIT_L);
		txrx_result = dxl_get_result();
		if(txrx_result!=COMM_RXSUCCESS)
			pc.printf("Failed Read CCW_ANGLE_LIMIT failed error=%d\n",txrx_result);
		else	
			pc.printf("CCW_ANGLE_LIMIT=%d,degree=%f\n",ccw_angle_lim,ccw_angle_lim*DEF_RATIO_PUS_TO_DEG);
		

		//==multi turn offset==//
		multi_turn_offset=dxl_read_word(gMapAxisID[i],MULTITURN_OFFSET);
		txrx_result = dxl_get_result();
		if(txrx_result!=COMM_RXSUCCESS)
			pc.printf("Failed Read MULTITURN_OFFSET failed error=%d\n",txrx_result);
		else	
			pc.printf("MULTITURN_OFFSET=%d\n",multi_turn_offset);
	}

	return 0;
}




int Read_pos(float *pos,unsigned char unit)
{
	int i=0;
	short int pulse=0;
	int rt=0;

	if(unit==DEF_UNIT_RAD)
	{
		for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
		{
			pulse = dxl_read_word(gMapAxisID[i], PRESENT_POS);
			if(dxl_get_result()!=COMM_RXSUCCESS)
			{
				rt=-gMapAxisNO[i];
				pos[i]=0xffff;
			}
			else
			{
				pulse-=gr2m_offset_pulse[i]; //mortor to robot offset =>minus offset
				pos[i]=pulse*DEF_RATIO_PUS_TO_RAD;
			}
		}
		
	}
	else if(unit==DEF_UNIT_DEG)
	{
		for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
		{
			pulse = dxl_read_word(gMapAxisID[i], PRESENT_POS);
			if(dxl_get_result()!=COMM_RXSUCCESS)
			{
				rt=-gMapAxisNO[i];	
				pos[i]=0xffff;
			}
			else
			{
				pulse-=gr2m_offset_pulse[i]; 
				pos[i]=pulse*DEF_RATIO_PUS_TO_DEG;
			}
		}
	}
	else if(unit==DEF_UNIT_PUS)
	{
		for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
		{
			pulse = dxl_read_word(gMapAxisID[i], PRESENT_POS);
			if(dxl_get_result()!=COMM_RXSUCCESS)
			{
				rt=-gMapAxisNO[i];
				pos[i]=0xffff;
			}
			else
			{
				pulse-=gr2m_offset_pulse[i]; 
				pos[i]=pulse;
			}
		}
		
	}
	else
	{
		//non offset value
		for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
		{
			pos[i] = dxl_read_word(gMapAxisID[i], PRESENT_POS);
			if(dxl_get_result()!=COMM_RXSUCCESS)
			{
				rt=-gMapAxisNO[i];
				pos[i]=0xffff;
			}
		}
	}
	
	return rt;
}




int Output_to_Dynamixel(const float *Ang_rad,const unsigned short int *velocity) 
{
	unsigned char i=0;

	//===================================================================//
	//==limit axis  if not zero ,the return value is the overlimit axis==//
	//===================================================================//
	for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
	{
		if( (Ang_rad[i] > grobot_lim_rad_H[i]) || (Ang_rad[i] < grobot_lim_rad_L[i]) )
		{
			DBGMSG(("AXIS%d over limit Ang_rad=%f,grobot_lim_rad_L=%f,grobot_lim_rad_H=%f\n",gMapAxisNO[i],Ang_rad[i],grobot_lim_rad_L[i],grobot_lim_rad_H[i]))
			return -gMapAxisNO[i];
		}
	}

	//===================================================//
	//==trnasformat to pulse and offset to motor domain==//
	//====================================================//
	short int Ang_pulse[MAX_AXIS_NUM]={0};
	for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
	{
		Ang_pulse[i]=(short int)(Ang_rad[i]*DEF_RATIO_RAD_TO_PUS);
		Ang_pulse[i]+=gr2m_offset_pulse[i];

		if( Ang_pulse[i] > DEF_JOINT_MODE_MAX_PULSE )//  0~4095
		{
			DBGMSG(("AXIS%d over range of mortor  Ang_pulse=%d,JOINT_MODE_MIN_PULSE=%d,JOINT_MODE_MAX_PULSE=%d\n",gMapAxisNO[i],Ang_pulse[i],DEF_JOINT_MODE_MIN_PULSE,DEF_JOINT_MODE_MAX_PULSE))
			return -gMapAxisNO[i];
		}
	}

	//================================//
	//==output to motor by syncpage===//
	//===============================//
	unsigned short int SyncPage1[21]=
	{ 
		ID_AXIS1,(unsigned short int)Ang_pulse[Index_AXIS1],velocity[Index_AXIS1], //ID,goal,velocity
		ID_AXIS2,(unsigned short int)Ang_pulse[Index_AXIS2],velocity[Index_AXIS2], 
		ID_AXIS3,(unsigned short int)Ang_pulse[Index_AXIS3],velocity[Index_AXIS3], 
		ID_AXIS4,(unsigned short int)Ang_pulse[Index_AXIS4],velocity[Index_AXIS4], 
		ID_AXIS5,(unsigned short int)Ang_pulse[Index_AXIS5],velocity[Index_AXIS5], 
		ID_AXIS6,(unsigned short int)Ang_pulse[Index_AXIS6],velocity[Index_AXIS6], 
		ID_AXIS7,(unsigned short int)Ang_pulse[Index_AXIS7],velocity[Index_AXIS7], 
	};

	
#if (DEBUG)
	for(i=Index_AXIS1;i<MAX_AXIS_NUM;i++)
		pc.printf("syncwrite AXIS%d pos=%d velocity=%d\n",gMapAxisNO[i],Ang_pulse[i],velocity[i]);
#endif

	syncWrite_u16base(GOAL_POSITION,2,SyncPage1,21);//byte syncWrite(byte start_addr, byte num_of_data, int *param, int array_length);
  
	return 0;
}