hsu han-lin / Mbed 2 deprecated DXL_SDK_Porting_Test

Still Test

Dependencies:   DXL_SDK_For_F446RE RobotControl_7Axis Matrix mbed

main.cpp@4:53ef39fbf9d9, 2017-02-08 (annotated)

Committer:
stanley1228
Date:
Wed Feb 08 00:32:55 2017 +0800
Revision:
4:53ef39fbf9d9
Parent:
3:8a9407817ba9
Child:
7:15f0494813f7
1.Add if(dxl_get_result()!=COMM_RXSUCCESS)  pos=-1;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
stanley1228 0:1780ffc33286 1 #include "mbed.h"
stanley1228 0:1780ffc33286 2 #include "dynamixel.h"
stanley1228 0:1780ffc33286 3
stanley1228 4:53ef39fbf9d9 4
stanley1228 4:53ef39fbf9d9 5
stanley1228 4:53ef39fbf9d9 6
stanley1228 4:53ef39fbf9d9 7 //stanley robot test define//
stanley1228 4:53ef39fbf9d9 8 enum{
stanley1228 4:53ef39fbf9d9 9 ID_AXIS1=1,
stanley1228 4:53ef39fbf9d9 10 ID_AXIS2,
stanley1228 4:53ef39fbf9d9 11 ID_AXIS3,
stanley1228 4:53ef39fbf9d9 12 ID_AXIS4,
stanley1228 4:53ef39fbf9d9 13 ID_AXIS5,
stanley1228 4:53ef39fbf9d9 14 ID_AXIS6,
stanley1228 4:53ef39fbf9d9 15 ID_AXIS7
stanley1228 4:53ef39fbf9d9 16 };
stanley1228 4:53ef39fbf9d9 17
stanley1228 4:53ef39fbf9d9 18 #define MAX_AXIS_NUM 7
stanley1228 4:53ef39fbf9d9 19
stanley1228 4:53ef39fbf9d9 20 #define AXIS1_PULSE_LIM_L (-910)
stanley1228 4:53ef39fbf9d9 21 #define AXIS1_PULSE_LIM_H 2048
stanley1228 4:53ef39fbf9d9 22 #define AXIS2_PULSE_LIM_L (-205)
stanley1228 4:53ef39fbf9d9 23 #define AXIS2_PULSE_LIM_H 2048
stanley1228 4:53ef39fbf9d9 24 #define AXIS3_PULSE_LIM_L (-2840)
stanley1228 4:53ef39fbf9d9 25 #define AXIS3_PULSE_LIM_H 1190
stanley1228 4:53ef39fbf9d9 26 #define AXIS4_PULSE_LIM_L 0//need to test
stanley1228 4:53ef39fbf9d9 27 #define AXIS4_PULSE_LIM_H 0//need to test
stanley1228 4:53ef39fbf9d9 28 #define AXIS5_PULSE_LIM_L (-2048)
stanley1228 4:53ef39fbf9d9 29 #define AXIS5_PULSE_LIM_H 1680
stanley1228 4:53ef39fbf9d9 30 #define AXIS6_PULSE_LIM_L (-680)
stanley1228 4:53ef39fbf9d9 31 #define AXIS6_PULSE_LIM_H 1024
stanley1228 4:53ef39fbf9d9 32 #define AXIS7_PULSE_LIM_L (-420)
stanley1228 4:53ef39fbf9d9 33 #define AXIS7_PULSE_LIM_H 420
stanley1228 4:53ef39fbf9d9 34
stanley1228 0:1780ffc33286 35
stanley1228 0:1780ffc33286 36 DigitalOut myled(LED1);
stanley1228 0:1780ffc33286 37 Serial pc(SERIAL_TX, SERIAL_RX);
stanley1228 0:1780ffc33286 38
stanley1228 0:1780ffc33286 39
stanley1228 0:1780ffc33286 40 int main()
stanley1228 0:1780ffc33286 41 {
stanley1228 0:1780ffc33286 42 int rt =0;
stanley1228 0:1780ffc33286 43
stanley1228 0:1780ffc33286 44 pc.printf("start\n",rt);
stanley1228 0:1780ffc33286 45
stanley1228 0:1780ffc33286 46 rt=dxl_initialize( 1, 1);
stanley1228 0:1780ffc33286 47 pc.printf("dxl_initialize rt=%d\n",rt);
stanley1228 0:1780ffc33286 48
stanley1228 0:1780ffc33286 49 myled = 0; // LED is ON
stanley1228 0:1780ffc33286 50
stanley1228 0:1780ffc33286 51 /**************************
stanley1228 0:1780ffc33286 52 *** Check communication ***
stanley1228 0:1780ffc33286 53 **************************/
stanley1228 2:fb9508744bc4 54 //dxl_write_byte(3,ADDRESS_LED,0);
stanley1228 0:1780ffc33286 55 //dxl_ping(3);
stanley1228 0:1780ffc33286 56
stanley1228 3:8a9407817ba9 57 short int pos=0; //16bit
stanley1228 0:1780ffc33286 58 while(1)
stanley1228 0:1780ffc33286 59 {
stanley1228 2:fb9508744bc4 60
stanley1228 3:8a9407817ba9 61 //pc.printf("before=%d\n",rt);
stanley1228 2:fb9508744bc4 62
stanley1228 4:53ef39fbf9d9 63
stanley1228 4:53ef39fbf9d9 64 //===Test move
stanley1228 2:fb9508744bc4 65 //dxl_write_word(3,GOAL_POSITION,400);
stanley1228 3:8a9407817ba9 66 //setPosition(3,2048,10);
stanley1228 3:8a9407817ba9 67 //pc.printf("after=%d\n",rt);
stanley1228 0:1780ffc33286 68 //myled = 1; // LED is ON
stanley1228 3:8a9407817ba9 69 //wait(4);
stanley1228 2:fb9508744bc4 70 //dxl_write_word(3,GOAL_POSITION,-400);
stanley1228 3:8a9407817ba9 71 //setPosition(3,-2048,10);
stanley1228 0:1780ffc33286 72 //myled = 0; // LED is ON
stanley1228 3:8a9407817ba9 73 //wait(4);
stanley1228 3:8a9407817ba9 74
stanley1228 4:53ef39fbf9d9 75
stanley1228 4:53ef39fbf9d9 76 //===Test read pos====//
stanley1228 4:53ef39fbf9d9 77 /*pos=dxl_read_word(3,PRESENT_POS);
stanley1228 3:8a9407817ba9 78 pc.printf("pos=%d\n",pos);
stanley1228 4:53ef39fbf9d9 79 wait_ms(200); */
stanley1228 3:8a9407817ba9 80
stanley1228 4:53ef39fbf9d9 81 //====Test read all pos===
stanley1228 4:53ef39fbf9d9 82 int i=0;
stanley1228 4:53ef39fbf9d9 83 for(i=ID_AXIS1;i<=ID_AXIS7;i++)
stanley1228 4:53ef39fbf9d9 84 {
stanley1228 4:53ef39fbf9d9 85 pos = dxl_read_word(i, PRESENT_POS);
stanley1228 4:53ef39fbf9d9 86 if(dxl_get_result()!=COMM_RXSUCCESS)
stanley1228 4:53ef39fbf9d9 87 pos=-1;
stanley1228 4:53ef39fbf9d9 88
stanley1228 4:53ef39fbf9d9 89 pc.printf("X%d=%d,",i,pos);
stanley1228 4:53ef39fbf9d9 90 }
stanley1228 4:53ef39fbf9d9 91 pc.printf("\n");
stanley1228 4:53ef39fbf9d9 92 wait_ms(500);
stanley1228 0:1780ffc33286 93 }
stanley1228 4:53ef39fbf9d9 94 }