Diff: main.cpp

Revision:

9:54710454ce60

Parent:

8:37f5a7219fe6

Child:

10:328cc5179ffb

--- a/main.cpp	Wed Feb 08 15:50:53 2017 +0800
+++ b/main.cpp	Thu Feb 09 17:18:40 2017 +0800
@@ -32,7 +32,10 @@
 #define AXIS7_PULSE_LIM_L (-420)
 #define AXIS7_PULSE_LIM_H 420
 
+#define DEF_PI (3.1415)
 
+
+//==Pin define==//
 DigitalOut myled(LED1);
 Serial pc(SERIAL_TX, SERIAL_RX);
 
@@ -101,28 +104,108 @@
 	return 0;
 }
 
+int Output_to_Dynamixel(float *Ang_rad)
+{
+  //========================//
+  //==trnasformat to pulse==//
+  //========================//
+  short int Ang_pulse[MAX_AXIS_NUM]={0};
+  short int i=0;
+  for(i=0;i<MAX_AXIS_NUM;i++)
+  {
+    Ang_pulse[i]=(int)(Ang_rad[i]*57.3*11.378); //radian => degree => pulse
+  }
+
+  //===================================================================//
+  //==limit axis  if not zero ,the return value is the overlimit axis==//
+  //===================================================================//
+	static int Axis_Pulse_lim_H[MAX_AXIS_NUM]=
+	{    
+	AXIS1_PULSE_LIM_H,
+	AXIS2_PULSE_LIM_H,
+	AXIS3_PULSE_LIM_H,
+	AXIS4_PULSE_LIM_H,
+	AXIS5_PULSE_LIM_H,
+	AXIS6_PULSE_LIM_H,
+	AXIS7_PULSE_LIM_H};
+    
+	static int Axis_Pulse_lim_L[MAX_AXIS_NUM]=
+	{    
+	AXIS1_PULSE_LIM_L,
+	AXIS2_PULSE_LIM_L,
+	AXIS3_PULSE_LIM_L,
+	AXIS4_PULSE_LIM_L,
+	AXIS5_PULSE_LIM_L,
+	AXIS6_PULSE_LIM_L,
+	AXIS7_PULSE_LIM_L};  
+
+	for(i=0;i<MAX_AXIS_NUM;i++)
+	{
+		if( (Ang_pulse[i] > Axis_Pulse_lim_H[i]) || (Ang_pulse[i] < Axis_Pulse_lim_L[i]) )
+		{
+			pc.printf("over limit Ang_pulse[%d]=%d,Axis_Pulse_lim_H[%d]=%d,Axis_Pulse_lim_L[%d]=%d\n",i,Ang_pulse[i],i,Axis_Pulse_lim_H[i],i,Axis_Pulse_lim_L[i]);
+			return i+1;
+		}
+	}
+
+	//================================//
+	//==output to motor by syncpage===//
+	//===============================//
+	short int velocity[MAX_AXIS_NUM]={10,10,10,10,30,30,30};
+  
+	short int SyncPage1[21]=
+	{ 
+		ID_AXIS1,Ang_pulse[0],velocity[0], //ID,goal,velocity
+		ID_AXIS2,Ang_pulse[1],velocity[1], 
+		ID_AXIS3,Ang_pulse[2],velocity[2], 
+		ID_AXIS4,Ang_pulse[3],velocity[3], 
+		ID_AXIS5,Ang_pulse[4],velocity[4], 
+		ID_AXIS6,Ang_pulse[5],velocity[5], 
+		ID_AXIS7,Ang_pulse[6],velocity[6], 
+	};
+   
+	for(i=0;i<MAX_AXIS_NUM;i++)
+		pc.printf("Ang_pulse[%d]=%d velocity[%d]=%d\n",i,Ang_pulse[i],i,velocity[i]);
+
+
+
+	syncWrite_u16base(GOAL_POSITION,2,SyncPage1,21);//byte syncWrite(byte start_addr, byte num_of_data, int *param, int array_length);
+  
+	return 0;
+}
+
 int main()
 {
-   int rt =0;
+	int rt =0;
    
+	//==================//
+	//==dxl_initialize==//
+	//===================//
     pc.printf("start\n",rt);
-  
     rt=dxl_initialize( 1, 1);
     pc.printf("dxl_initialize rt=%d\n",rt);
 
-	short int pos=0;
 	
+	//=========================//
 	//==ROM parameter setting==//
+	//=========================//
 	//rt=ROM_Setting();
 	
   
-	myled = 0; // LED is ON
-    /**************************
-    *** Check communication ***
-    **************************/
-    //dxl_write_byte(3,ADDRESS_LED,0);
-    //dxl_ping(3);
+	//myled = 0; // LED is ON
    
+	//==Test Output_to_Dynamixel==//
+	float Ang_rad[MAX_AXIS_NUM]={0};
+	Ang_rad[4]=90*DEF_PI/180;
+	Ang_rad[5]=90*DEF_PI/180;
+	Ang_rad[6]=30*DEF_PI/180;
+		
+	//dxl_write_word(ID_AXIS5,MAX_TORQUE,55);
+	//dxl_write_word(ID_AXIS5,TORQUE_LIMIT,512);
+
+	rt=Output_to_Dynamixel(Ang_rad);
+	pc.printf("Output_to_Dynamixel rt=%d",rt);
+	
     while(1)
     {
 		//pc.printf("before=%d\n",rt);
@@ -146,6 +229,7 @@
 		wait_ms(200);    */
 
 		//====Test read all pos===
+		static short int pos=0;
 		int i=0;
 		for(i=ID_AXIS1;i<=ID_AXIS7;i++)
 		{
@@ -158,9 +242,11 @@
 		pc.printf("\n");
 		
 
+		
+
 
 		//====Test syncWrite_u16base===
-		static short int pos3=-100;
+		/*static short int pos3=-100;
 		pos3=(pos3==-100)? 100 :-100;
 
 		short int SyncPage1[21]=
@@ -174,7 +260,7 @@
 			ID_AXIS7,100,5,
 		};
 
-		wait(5);    
+		wait(0.5);    */
 		//short int SyncPage1[21]=//Test use
 		//{ 
 		//	0x00,0x010,0x150, // 3 Dynamixels are move to position 512
@@ -187,7 +273,7 @@
 		//};
 
 
-		syncWrite_u16base(GOAL_POSITION, 2,SyncPage1,21);//start_addr, data_length, *param, param_length;
+		//syncWrite_u16base(GOAL_POSITION, 2,SyncPage1,21);//start_addr, data_length, *param, param_length;
 	
 	}
 }