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

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();