To be tested
Dependencies: Servo AX12_final MX106_not_working comunication_1
Revision 25:1aa10432cda2, committed 2019-09-05
- Comitter:
- gidiana
- Date:
- Thu Sep 05 20:26:40 2019 +0000
- Parent:
- 24:adb6bac314d7
- Commit message:
- revision and more verbosity
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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();