![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Still Test
Dependencies: DXL_SDK_For_F446RE RobotControl_7Axis Matrix mbed
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 +}