hsu han-lin / Mbed 2 deprecated DXL_SDK_Porting_Test

Still Test

Dependencies:   DXL_SDK_For_F446RE RobotControl_7Axis Matrix mbed

main.cpp@7:15f0494813f7, 2017-02-08 (annotated)

Committer:
stanley1228
Date:
Wed Feb 08 12:27:32 2017 +0800
Revision:
7:15f0494813f7
Parent:
4:53ef39fbf9d9
Child:
8:37f5a7219fe6
sync write test seem ok need more test

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 7:15f0494813f7 57 short int pos=0; //16bit
stanley1228 0:1780ffc33286 58 while(1)
stanley1228 0:1780ffc33286 59 {
stanley1228 3:8a9407817ba9 60 //pc.printf("before=%d\n",rt);
stanley1228 2:fb9508744bc4 61
stanley1228 4:53ef39fbf9d9 62
stanley1228 4:53ef39fbf9d9 63 //===Test move
stanley1228 2:fb9508744bc4 64 //dxl_write_word(3,GOAL_POSITION,400);
stanley1228 3:8a9407817ba9 65 //setPosition(3,2048,10);
stanley1228 3:8a9407817ba9 66 //pc.printf("after=%d\n",rt);
stanley1228 0:1780ffc33286 67 //myled = 1; // LED is ON
stanley1228 3:8a9407817ba9 68 //wait(4);
stanley1228 2:fb9508744bc4 69 //dxl_write_word(3,GOAL_POSITION,-400);
stanley1228 3:8a9407817ba9 70 //setPosition(3,-2048,10);
stanley1228 0:1780ffc33286 71 //myled = 0; // LED is ON
stanley1228 3:8a9407817ba9 72 //wait(4);
stanley1228 3:8a9407817ba9 73
stanley1228 4:53ef39fbf9d9 74
stanley1228 4:53ef39fbf9d9 75 //===Test read pos====//
stanley1228 4:53ef39fbf9d9 76 /*pos=dxl_read_word(3,PRESENT_POS);
stanley1228 3:8a9407817ba9 77 pc.printf("pos=%d\n",pos);
stanley1228 4:53ef39fbf9d9 78 wait_ms(200); */
stanley1228 3:8a9407817ba9 79
stanley1228 4:53ef39fbf9d9 80 //====Test read all pos===
stanley1228 4:53ef39fbf9d9 81 int i=0;
stanley1228 4:53ef39fbf9d9 82 for(i=ID_AXIS1;i<=ID_AXIS7;i++)
stanley1228 4:53ef39fbf9d9 83 {
stanley1228 4:53ef39fbf9d9 84 pos = dxl_read_word(i, PRESENT_POS);
stanley1228 4:53ef39fbf9d9 85 if(dxl_get_result()!=COMM_RXSUCCESS)
stanley1228 4:53ef39fbf9d9 86 pos=-1;
stanley1228 4:53ef39fbf9d9 87
stanley1228 4:53ef39fbf9d9 88 pc.printf("X%d=%d,",i,pos);
stanley1228 4:53ef39fbf9d9 89 }
stanley1228 4:53ef39fbf9d9 90 pc.printf("\n");
stanley1228 7:15f0494813f7 91
stanley1228 7:15f0494813f7 92
stanley1228 7:15f0494813f7 93
stanley1228 7:15f0494813f7 94 //====Test syncWrite_u16base===
stanley1228 7:15f0494813f7 95 static short int pos3=-100;
stanley1228 7:15f0494813f7 96 pos3=(pos3==-100)? 100 :-100;
stanley1228 7:15f0494813f7 97
stanley1228 7:15f0494813f7 98 short int SyncPage1[21]=
stanley1228 7:15f0494813f7 99 {
stanley1228 7:15f0494813f7 100 ID_AXIS1,100,5,
stanley1228 7:15f0494813f7 101 ID_AXIS2,100,5,
stanley1228 7:15f0494813f7 102 ID_AXIS3,100,5,
stanley1228 7:15f0494813f7 103 ID_AXIS4,100,5,
stanley1228 7:15f0494813f7 104 ID_AXIS5,100,5,
stanley1228 7:15f0494813f7 105 ID_AXIS6,100,5,
stanley1228 7:15f0494813f7 106 ID_AXIS7,100,5,
stanley1228 7:15f0494813f7 107 };
stanley1228 7:15f0494813f7 108
stanley1228 7:15f0494813f7 109 wait(2);
stanley1228 7:15f0494813f7 110 //short int SyncPage1[21]=//Test use
stanley1228 7:15f0494813f7 111 //{
stanley1228 7:15f0494813f7 112 // 0x00,0x010,0x150, // 3 Dynamixels are move to position 512
stanley1228 7:15f0494813f7 113 // 0x01,0x020,0x360,
stanley1228 7:15f0494813f7 114 // 0x02,0x030,0x170,
stanley1228 7:15f0494813f7 115 // 0x03,0x220,0x380,
stanley1228 7:15f0494813f7 116 // 0x05,0x123,0x121,
stanley1228 7:15f0494813f7 117 // 0x06,0x234,0x143,
stanley1228 7:15f0494813f7 118 // 0x07,0x145,0x167
stanley1228 7:15f0494813f7 119 //};
stanley1228 7:15f0494813f7 120
stanley1228 7:15f0494813f7 121
stanley1228 7:15f0494813f7 122 syncWrite_u16base(GOAL_POSITION, 2,SyncPage1,21);//start_addr, data_length, *param, param_length;
stanley1228 7:15f0494813f7 123
stanley1228 7:15f0494813f7 124 }
stanley1228 4:53ef39fbf9d9 125 }