![](/media/cache/group/Diana_su_nero.jpg.50x50_q85.jpg)
To be tested
Dependencies: Servo AX12_final MX106_not_working comunication_1
Diff: main.cpp
- Revision:
- 25:1aa10432cda2
- Parent:
- 24:adb6bac314d7
diff -r adb6bac314d7 -r 1aa10432cda2 main.cpp --- a/main.cpp Tue Sep 03 16:29:39 2019 +0000 +++ b/main.cpp Thu Sep 05 20:26:40 2019 +0000 @@ -43,7 +43,7 @@ DigitalOut dyn_enable(PC_11); int dxl_id[]={1,2,3,4}; JOINT joint_id[]={WRIST1,WRIST2,WRIST3,CAMERA1,CAMERA2}; -int dxl_speed[]={50,25,50,50}; +int dxl_speed[]={25,12,25,25}; float dxl_gear[]={3,2,3,1}; float dxl_present_position[] = {0,0,0,0}; float dxl_goal_position[] = {0,0,0,0}; @@ -255,6 +255,11 @@ } } + + #if VERBOSE + printf("DYNAMIXEL SET GOAL POSITION\n\r"); + #endif + w_1.setGoal((dxl_goal_position[0]-dxl_offset[0])); wait(SLEEP); w_2.setGoal((dxl_goal_position[1]-dxl_offset[1])); @@ -264,7 +269,11 @@ a_1.setGoal((dxl_goal_position[3]-dxl_offset[3])); wait(SLEEP); camera=camera_pose; - + + #if VERBOSE + printf("DYNAMIXEL PRESENT POSITION\n\r"); + #endif + dxl_present_position[0]=w_1.getPosition(); wait(SLEEP); dxl_present_position[1]=w_2.getPosition(); @@ -277,7 +286,9 @@ #if VERBOSE printf("DYNAMIXEL PRESENT POSITION %f %f %f %f\n\r", dxl_present_position[0],dxl_present_position[1], dxl_present_position[2] ,dxl_present_position[3] ); #endif - + #if VERBOSE + printf("DYNAMIXEL CURRENT\n\r"); + #endif dxl_current[0]=w_1.getCurrent(); wait(SLEEP); dxl_current[1]=w_2.getCurrent(); @@ -289,7 +300,9 @@ #if VERBOSE printf("DYNAMIXEL CURRENT %f %f %f %f\n\r", dxl_current[0],dxl_current[1], dxl_current[2] ,dxl_current[3] ); #endif - + #if VERBOSE + printf("DYNAMIXEL TORQUE\n\r"); + #endif dxl_torque[0]=dxl_current[0]*0.875; dxl_torque[1]=dxl_current[1]*0.875; dxl_torque[2]=dxl_current[2]*0.875; @@ -297,7 +310,9 @@ #if VERBOSE printf("DYNAMIXEL TORQUE %f %f %f %f\n\r", dxl_torque[0],dxl_torque[1], dxl_torque[2] ,dxl_torque[3] ); #endif - + #if VERBOSE + printf("DYNAMIXEL TEMP\n\r"); + #endif dxl_temperature[0]=w_1.getTemp(); wait(SLEEP); dxl_temperature[1]=w_2.getTemp(); @@ -368,6 +383,9 @@ #endif + #if VERBOSE + printf("DYNAMIXEL TORQUE MAX\n\r"); + #endif dxl_max_torque[0]=w_1.getTorqueMax();