Diff: main.cpp

Revision:

4:53ef39fbf9d9

Parent:

3:8a9407817ba9

Child:

7:15f0494813f7

--- a/main.cpp	Wed Feb 08 00:11:41 2017 +0800
+++ b/main.cpp	Wed Feb 08 00:32:55 2017 +0800
@@ -1,8 +1,37 @@
 #include "mbed.h"
 #include "dynamixel.h"
 
-#define ADDRESS_LED 0x19
-#define GOAL_POSITION     30
+
+
+
+//stanley robot test define//
+enum{
+  ID_AXIS1=1,
+  ID_AXIS2,
+  ID_AXIS3,
+  ID_AXIS4,
+  ID_AXIS5,
+  ID_AXIS6,
+  ID_AXIS7
+};
+
+#define MAX_AXIS_NUM     7
+
+#define AXIS1_PULSE_LIM_L (-910)
+#define AXIS1_PULSE_LIM_H 2048
+#define AXIS2_PULSE_LIM_L (-205)
+#define AXIS2_PULSE_LIM_H 2048
+#define AXIS3_PULSE_LIM_L (-2840)
+#define AXIS3_PULSE_LIM_H 1190
+#define AXIS4_PULSE_LIM_L 0//need to test
+#define AXIS4_PULSE_LIM_H 0//need to test
+#define AXIS5_PULSE_LIM_L (-2048)
+#define AXIS5_PULSE_LIM_H 1680
+#define AXIS6_PULSE_LIM_L (-680)
+#define AXIS6_PULSE_LIM_H 1024
+#define AXIS7_PULSE_LIM_L (-420)
+#define AXIS7_PULSE_LIM_H 420
+
 
 DigitalOut myled(LED1);
 Serial pc(SERIAL_TX, SERIAL_RX);
@@ -17,11 +46,6 @@
     rt=dxl_initialize( 1, 1);
     pc.printf("dxl_initialize rt=%d\n",rt);
 
-	int nnn=-100;
-	pc.printf("nnn=%d\n",nnn);
-
-	nnn=-1000;
-	pc.printf("nnn=%d\n",nnn);
     myled = 0; // LED is ON
     
     /**************************
@@ -36,6 +60,8 @@
         
 		//pc.printf("before=%d\n",rt);
     
+
+		//===Test move
         //dxl_write_word(3,GOAL_POSITION,400);
         //setPosition(3,2048,10);
          //pc.printf("after=%d\n",rt);
@@ -46,10 +72,23 @@
         //myled = 0; // LED is ON
         //wait(4);
 		
-		pos=dxl_read_word(3,PRESENT_POS);
+
+		//===Test read pos====//
+		/*pos=dxl_read_word(3,PRESENT_POS);
 		pc.printf("pos=%d\n",pos);
-		wait_ms(200);
+		wait_ms(200);    */
 
-        
+		//====Test read all pos===
+		int i=0;
+		for(i=ID_AXIS1;i<=ID_AXIS7;i++)
+		{
+		  pos = dxl_read_word(i, PRESENT_POS);
+		  if(dxl_get_result()!=COMM_RXSUCCESS)
+			pos=-1;
+
+		  pc.printf("X%d=%d,",i,pos);
+		}
+		pc.printf("\n");
+		wait_ms(500);    
     }
-}
\ No newline at end of file
+}