Merge pull request #8 from dcorboy/refactor

A handful of changes including hand orientation and interpolation data handling

Author: songjunyi

uarm_library.cpp

@@ -18,7 +18,6 @@ int angleR;
 int angleL;
 int angleBottom;
 int l_movementTrigger = 0;
-double g_interpol_val_arr[50];
 
 uArmClass::uArmClass()
 {
@@ -386,7 +385,7 @@ void uArmClass::calAngles(double x, double y, double z)
 
 }
 
-void uArmClass::interpolation(double init_val, double final_val)
+void uArmClass::interpolation(double init_val, double final_val, double (&interpol_val_array)[INTERP_INTVL])
 {
   // by using the formula theta_t = l_a_0 + l_a_1 * t + l_a_2 * t^2 + l_a_3 * t^3
   // theta(0) = init_val; theta(t_f) = final_val
@@ -405,10 +404,10 @@ void uArmClass::interpolation(double init_val, double final_val)
   l_a_2 = (3 * (final_val - init_val)) / (l_time_total*l_time_total);
   l_a_3 = (-2 * (final_val - init_val)) / (l_time_total*l_time_total*l_time_total);
 
-  for (byte i = 0; i < 50; i=i+1)
+  for (byte i = 0; i < INTERP_INTVL; i=i+1)
   {
-    l_t_step = (l_time_total / 50.0) *i;
-    g_interpol_val_arr[i] = l_a_0 + l_a_1 * (l_t_step) + l_a_2 * (l_t_step *l_t_step ) + l_a_3 * (l_t_step *l_t_step *l_t_step);  
+    l_t_step = (l_time_total / (float)INTERP_INTVL) *i;
+    interpol_val_array[i] = l_a_0 + l_a_1 * (l_t_step) + l_a_2 * (l_t_step *l_t_step ) + l_a_3 * (l_t_step *l_t_step *l_t_step);  
   }
 }
 
@@ -442,40 +441,31 @@ void uArmClass::moveTo(double x, double y, double z)
   double l_current_x;
   double l_current_y;
   double l_current_z;
+  double l_current_hand;
 
-  double x_arr[50];
-  double y_arr[50];
-  double z_arr[50];
+  double x_arr[INTERP_INTVL];
+  double y_arr[INTERP_INTVL];
+  double z_arr[INTERP_INTVL];
 
   calXYZ();
   l_current_x = g_cal_x;
   l_current_y = g_cal_y;
   l_current_z = g_cal_z;
+  l_current_hand = readAngle(SERVO_HAND_ROT_NUM);
+  if (l_current_hand < 0) l_current_hand = 0;
 
-  interpolation(l_current_x, x);
-  for (byte i = 0; i < 50; i++){
-    x_arr[i] = g_interpol_val_arr[i];
-    
-  }
+  interpolation(l_current_x, x, x_arr);
 
-  interpolation(l_current_y, y);
-  for (byte i = 0; i < 50; i++){
-    y_arr[i] = g_interpol_val_arr[i];
-    
-  }
+  interpolation(l_current_y, y, y_arr);
 
-  interpolation(l_current_z, z); 
-  for (byte i = 0; i < 50; i++){
-    z_arr[i] = g_interpol_val_arr[i];
-    
-  }
+  interpolation(l_current_z, z, z_arr); 
 
-  for (byte i = 0; i < 50; i++)
+  for (byte i = 0; i < INTERP_INTVL; i++)
   {
     calAngles(x_arr[i],y_arr[i],z_arr[i]);
 
     l_movementTrigger = 1;
-    uarm.writeAngle(g_theta_1, g_theta_2, g_theta_3,0);
+    uarm.writeAngle(g_theta_1, g_theta_2, g_theta_3, l_current_hand);
 
     delay(40);
 
@@ -487,15 +477,17 @@ void uArmClass::moveTo(double x, double y, double z, int relative, double time_s
 {
   uarm.attachAll();
 
-  double x_arr[50];
-  double y_arr[50];
-  double z_arr[50];
+  double x_arr[INTERP_INTVL];
+  double y_arr[INTERP_INTVL];
+  double z_arr[INTERP_INTVL];
 
   calXYZ();
   double current_x = g_cal_x;
   double current_y = g_cal_y;
   double current_z = g_cal_z;
-  
+  double current_hand = readAngle(SERVO_HAND_ROT_NUM);
+  if (current_hand < 0) current_hand = 0;
+
   if ((relative !=0)&&(relative != 1))
   { 
     relative = 0;
@@ -508,37 +500,19 @@ void uArmClass::moveTo(double x, double y, double z, int relative, double time_s
 
   // }
 
-  interpolation(current_x, current_x*relative+x);
-
-  for (byte i = 0; i < 50; i++){
+  interpolation(current_x, current_x*relative+x, x_arr);
 
-    x_arr[i] = g_interpol_val_arr[i];
-    
-  }
-
-  interpolation(current_y, current_y*relative+y);
-  
-  for (byte i = 0; i < 50; i++){
-
-    y_arr[i] = g_interpol_val_arr[i];
-    
-  }
+  interpolation(current_y, current_y*relative+y, y_arr);
 
-  interpolation(current_z, current_z*relative+z); 
-  
-  for (byte i = 0; i < 50; i++){
+  interpolation(current_z, current_z*relative+z, z_arr); 
 
-    z_arr[i] = g_interpol_val_arr[i];
-    
-  }
-    
-  for (byte i = 0; i < 50; i++)
+  for (byte i = 0; i < INTERP_INTVL; i++)
   {
     calAngles(x_arr[i],y_arr[i],z_arr[i]);
     l_movementTrigger = 1;
-    uarm.writeAngle(g_theta_1, g_theta_2, g_theta_3,0);
+    uarm.writeAngle(g_theta_1, g_theta_2, g_theta_3, current_hand);
 
-    delay(time_spend*1000/50);
+    delay(time_spend*1000/INTERP_INTVL);
 
   }
 
@@ -549,9 +523,9 @@ void uArmClass::moveToAtOnce(double x, double y, double z, int relative, double
 {
   uarm.attachAll();
 
-  double x_arr[50];
-  double y_arr[50];
-  double z_arr[50];
+  double x_arr[INTERP_INTVL];
+  double y_arr[INTERP_INTVL];
+  double z_arr[INTERP_INTVL];
 
 
   calXYZ();
@@ -574,16 +548,18 @@ void uArmClass::moveToAtOnce(double x, double y, double z, int relative, double
 void uArmClass::moveTo(double x, double y, double z, int relative, double time_spend, int servo_4_relative, double servo_4_angle)
 {
   uarm.attachAll();
-  
-  double x_arr[50];
-  double y_arr[50];
-  double z_arr[50];
-  
+
+  double x_arr[INTERP_INTVL];
+  double y_arr[INTERP_INTVL];
+  double z_arr[INTERP_INTVL];
+  double h_arr[INTERP_INTVL];
 
   calXYZ();
   double current_x = g_cal_x;
   double current_y = g_cal_y;
   double current_z = g_cal_z;
+  double current_hand = readAngle(SERVO_HAND_ROT_NUM);
+  if (current_hand < 0) current_hand = 0;
 
   if ((relative !=0)&&(relative != 1))
   { 
@@ -600,40 +576,24 @@ void uArmClass::moveTo(double x, double y, double z, int relative, double time_s
     time_spend = abs(time_spend);
   }
 
-  interpolation(current_x, current_x*relative+x);
+  interpolation(current_x, current_x*relative+x, x_arr);
 
-  for (byte i = 0; i < 50; i++){
-
-    x_arr[i] = g_interpol_val_arr[i];
-    
-  }
-
-  interpolation(current_y, current_y*relative+y);
-  
-  for (byte i = 0; i < 50; i++){
-
-    y_arr[i] = g_interpol_val_arr[i];
-    
-  }
+  interpolation(current_y, current_y*relative+y, y_arr);
 
   if ( current_z*relative+z>25)
-    { interpolation(current_z, 25); }
+    { interpolation(current_z, 25, z_arr); }
   else
-    { interpolation(current_z, current_z*relative+z); }
-  
-  for (byte i = 0; i < 50; i++){
+    { interpolation(current_z, current_z*relative+z, z_arr); }
 
-    z_arr[i] = g_interpol_val_arr[i];
+  interpolation(current_hand, current_hand*servo_4_relative+servo_4_angle, h_arr);
 
-  }
-    
-  for (byte i = 0; i < 50; i++)
+  for (byte i = 0; i < INTERP_INTVL; i++)
   {
     calAngles(x_arr[i],y_arr[i],z_arr[i]);
     l_movementTrigger = 1;
-    uarm.writeAngle(g_theta_1, g_theta_2, g_theta_3, g_theta_1*servo_4_relative+servo_4_angle);
+    uarm.writeAngle(g_theta_1, g_theta_2, g_theta_3, h_arr[i]);
 
-    delay(time_spend*1000/50);
+    delay(time_spend*1000/INTERP_INTVL);
   }
 
 }
@@ -660,20 +620,20 @@ void uArmClass::drawCur(double length_1, double length_2, int angle, double time
   double current_x = g_cal_x;
   double current_y = g_cal_y;
   double current_z = g_cal_z;
-
-  interpolation(0, angle/MATH_TRANS); 
-
+  double interp_arr[INTERP_INTVL];
 
-  for (byte i = 0; i < 50; i++){
+  interpolation(0, angle/MATH_TRANS, interp_arr); 
+
+  for (byte i = 0; i < INTERP_INTVL; i++){
 
-    l_xp = length_1 * cos(g_interpol_val_arr[i]);
-    l_yp = length_2 * sin(g_interpol_val_arr[i]);
+    l_xp = length_1 * cos(interp_arr[i]);
+    l_yp = length_2 * sin(interp_arr[i]);
 
     calAngles( l_xp + current_x - length_1 , l_yp+ current_y , current_z);
     l_movementTrigger = 1;
     uarm.writeAngle(g_theta_1, g_theta_2, g_theta_3,0);
 
-    delay(time_spend*1000/50);
+    delay(time_spend*1000/INTERP_INTVL);
 
   }
 

uarm_library.h

@@ -45,6 +45,8 @@
 #define RELATIVE 1
 #define ABSOLUTE 0
 
+#define INTERP_INTVL     50
+
 #define TopOffset -1.5
 #define BottomOffset 1.5
 
@@ -102,6 +104,7 @@ class uArmClass
 	    double getCalX() {calXYZ(); return g_cal_x;}
 	    double getCalY() {calXYZ(); return g_cal_y;}
 	    double getCalZ() {calXYZ(); return g_cal_z;}
+      void getCalXYZ(double& x, double& y, double &z) {calXYZ(); x = g_cal_x; y = g_cal_y; z = g_cal_z;}
 
 	    void calAngles(double x, double y, double z);
 
@@ -110,7 +113,7 @@ class uArmClass
 
 	    void gripperCatch();
 	    void gripperRelease();
-		void interpolation(double init_val, double final_val);	
+		void interpolation(double init_val, double final_val, double (&interpol_val_array)[INTERP_INTVL]);
 		void pumpOn();
 		void pumpOff();
 protected: