![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Still Test
Dependencies: DXL_SDK_For_F446RE RobotControl_7Axis Matrix mbed
main.cpp
- Committer:
- stanley1228
- Date:
- 2017-02-08
- Revision:
- 8:37f5a7219fe6
- Parent:
- 7:15f0494813f7
- Child:
- 9:54710454ce60
File content as of revision 8:37f5a7219fe6:
#include "mbed.h" #include "dynamixel.h" //stanley robot test define// enum{ ID_AXIS1=1, ID_AXIS2, ID_AXIS3, ID_AXIS4, ID_AXIS5, ID_AXIS6, ID_AXIS7 }; #define MAX_AXIS_NUM ID_AXIS7 #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); int ROM_Setting() { int i=0; for(i=ID_AXIS1;i<=ID_AXIS7;i++) { //Set MAX_torgue dxl_write_word(i,MAX_TORQUE,55);// more safe //Set all to multi turn mode==// dxl_write_word(i,CW_ANGLE_LIMIT_L,0xfff); dxl_write_word(i,CCW_ANGLE_LIMIT_L,0xfff); //Set all to multi turn mode==// dxl_write_word(i,MULTITURN_OFFSET,-1024); } //==read and check ==// int txrx_result=0; short int max_torque=0; short int cw_angel_lim=0,ccw_angle_lim=0; short int multi_turn_offset=0; for(i=ID_AXIS1;i<=ID_AXIS7;i++) { pc.printf("===AXIS_%d===\n",i); //MAX_torgue max_torque = dxl_read_word(i, MAX_TORQUE); txrx_result = dxl_get_result(); if(txrx_result!=COMM_RXSUCCESS) pc.printf("Failed read MAX_TORQUE error=%d\n",txrx_result); else pc.printf("MAX_TORQUE=%d\n",max_torque); //CW_ANGLE_LIMIT,CCW_ANGLE_LIMIT==// cw_angel_lim=dxl_read_word(i,CW_ANGLE_LIMIT_L); txrx_result = dxl_get_result(); if(txrx_result!=COMM_RXSUCCESS) pc.printf("Failed read CW_ANGLE_LIMIT error=%d\n",txrx_result); else pc.printf("CW_ANGLE_LIMIT=%x\n",cw_angel_lim); ccw_angle_lim=dxl_read_word(i,CCW_ANGLE_LIMIT_L); txrx_result = dxl_get_result(); if(txrx_result!=COMM_RXSUCCESS) pc.printf("Failed Read CCW_ANGLE_LIMIT failed error=%d\n",txrx_result); else pc.printf("CCW_ANGLE_LIMIT=%x\n",ccw_angle_lim); //multi turn offset==// multi_turn_offset=dxl_read_word(i,MULTITURN_OFFSET); txrx_result = dxl_get_result(); if(txrx_result!=COMM_RXSUCCESS) pc.printf("Failed Read MULTITURN_OFFSET failed error=%d\n",txrx_result); else pc.printf("MULTITURN_OFFSET=%d\n",multi_turn_offset); } return 0; } int main() { int rt =0; 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); while(1) { //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); //myled = 1; // LED is ON //wait(4); //dxl_write_word(3,GOAL_POSITION,-400); //setPosition(3,-2048,10); //myled = 0; // LED is ON //wait(4); //===Test read pos====// /*pos=dxl_read_word(3,PRESENT_POS); pc.printf("pos=%d\n",pos); 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"); //====Test syncWrite_u16base=== static short int pos3=-100; pos3=(pos3==-100)? 100 :-100; short int SyncPage1[21]= { ID_AXIS1,100,5, ID_AXIS2,100,5, ID_AXIS3,100,5, ID_AXIS4,100,5, ID_AXIS5,100,5, ID_AXIS6,100,5, ID_AXIS7,100,5, }; wait(5); //short int SyncPage1[21]=//Test use //{ // 0x00,0x010,0x150, // 3 Dynamixels are move to position 512 // 0x01,0x020,0x360, // 0x02,0x030,0x170, // 0x03,0x220,0x380, // 0x05,0x123,0x121, // 0x06,0x234,0x143, // 0x07,0x145,0x167 //}; syncWrite_u16base(GOAL_POSITION, 2,SyncPage1,21);//start_addr, data_length, *param, param_length; } }