v1.5.8 use writeAngle() in moveToOps()

Author: altjz

HISTORY.md

@@ -1,9 +1,8 @@
-## [1.5.7] - 2016-05-07
+## [1.5.8] - 2016-05-07
 
 ### Fix
-
-- Update SERVO_OFFST_ADDRESS to MANUAL_OFFSET_ADDRESS
-- Update indent for syntax
+- Speed up Calibration, reduce delay
+- Use writeAngle() Function in MoveTo
 
 ### Changes
 

examples/Calibration/Calibration.ino

@@ -143,18 +143,18 @@ void linear_calibration_servo(byte servo_num)
                 }
 
                 if(i == 0) {
-                        delay(2000);
+                        delay(1000);
                 }
 
                 for (int l = 0; l<3; l++) {
                         l_angle_analog = analogRead(l_analog_pin);
-                        delay(100);
+                        delay(50);
                 }
 
                 arr_real[i] = kServoRangeIni + dot_i;
                 arr_input[i] = l_angle_analog;
 
-                delay(800);
+                delay(100);
 
         }
         arr_real[0] = kServoRangeIni;

uarm_library.cpp

@@ -54,6 +54,32 @@ void uArmClass::alert(byte times, byte runTime, byte stopTime)
     }
 }
 
+void uArmClass::writeAngle(double servo_rot_angle, double servo_left_angle, double servo_right_angle, double servo_hand_rot_angle)
+{
+  attachAll();
+  
+  if(servo_left_angle < 10) servo_left_angle = 10;
+  if(servo_left_angle > 120) servo_left_angle = 120;
+  if(servo_right_angle < 10) servo_right_angle = 10;
+  if(servo_right_angle > 110) servo_right_angle = 110;
+
+  if(servo_left_angle + servo_right_angle > 160) 
+  {
+    servo_right_angle = 160 - servo_left_angle;
+    return;
+  }
+  writeServoAngle(SERVO_ROT_NUM,servo_rot_angle,true);
+  writeServoAngle(SERVO_LEFT_NUM,servo_left_angle,true);
+  writeServoAngle(SERVO_RIGHT_NUM,servo_right_angle,true);
+  writeServoAngle(SERVO_HAND_ROT_NUM,servo_hand_rot_angle,true);
+  
+
+  // refresh logical servo angle cache
+  cur_rot = servo_rot_angle;
+  cur_left = servo_left_angle;
+  cur_right = servo_right_angle;
+  cur_hand = servo_hand_rot_angle;
+}
 
 /* Write the angle to Servo
 * servo_number - SERVO_ROT_NUM, SERVO_LEFT_NUM, SERVO_RIGHT_NUM, SERVO_HAND_ROT_NUM
@@ -487,12 +513,12 @@ void uArmClass::calXYZ()
                 for (byte i = 0; i < INTERP_INTVLS; i++)
                 {
 
-                    // writeAngle(rot_array[i], left_array[i], right_array[i], hand_array[i]);
-                    writeServoAngle(SERVO_ROT_NUM, rot_array[i], true);
+                     writeAngle(rot_array[i], left_array[i], right_array[i], hand_array[i]);
+                    //writeServoAngle(SERVO_ROT_NUM, rot_array[i], true);
                     // writeServoAngle(SERVO_LEFT_NUM, left_array[i],true);
                     // writeServoAngle(SERVO_RIGHT_NUM, right_array[i],true);
-                    writeLeftRightServoAngle(left_array[i], right_array[i], true);
-                    writeServoAngle(SERVO_HAND_ROT_NUM, hand_array[i],true);
+                    //writeLeftRightServoAngle(left_array[i], right_array[i], true);
+                    //writeServoAngle(SERVO_HAND_ROT_NUM, hand_array[i],true);
                     delay(time * 1000 / INTERP_INTVLS);
                 }
             } else if (path_type == PATH_LINEAR) {
@@ -516,25 +542,25 @@ void uArmClass::calXYZ()
                     // Serial.println(left);
                     // Serial.println(right);
                     // Serial.println();
-                    writeServoAngle(SERVO_ROT_NUM, rot,true);
+                    //writeServoAngle(SERVO_ROT_NUM, rot,true);
                     // writeServoAngle(SERVO_LEFT_NUM, tgt_left,true);
                     // writeServoAngle(SERVO_RIGHT_NUM, tgt_right,true);
-                    writeLeftRightServoAngle(left, right, true);
+                    //writeLeftRightServoAngle(left, right, true);
                     // if(enable_hand)
-                    writeServoAngle(SERVO_HAND_ROT_NUM, hand_array[i], true);
-                    // writeAngle(rot, left, right, hand_array[i]);
+                    //writeServoAngle(SERVO_HAND_ROT_NUM, hand_array[i], true);
+                    writeAngle(rot, left, right, hand_array[i]);
                     delay(time * 1000 / INTERP_INTVLS);
                 }
             }
         }
 
         // set final target position at end of interpolation or "atOnce"
-        writeServoAngle(SERVO_ROT_NUM, tgt_rot, true);
+        //writeServoAngle(SERVO_ROT_NUM, tgt_rot, true);
         // writeServoAngle(SERVO_LEFT_NUM, tgt_left, true);
         // writeServoAngle(SERVO_RIGHT_NUM, tgt_right, true);
-        writeLeftRightServoAngle(tgt_left, tgt_right, true);
-        writeServoAngle(SERVO_HAND_ROT_NUM, hand_angle, true);
-        // writeAngle(tgt_rot, tgt_left, tgt_right, hand_angle);
+        //writeLeftRightServoAngle(tgt_left, tgt_right, true);
+        //writeServoAngle(SERVO_HAND_ROT_NUM, hand_angle, true);
+         writeAngle(tgt_rot, tgt_left, tgt_right, hand_angle);
     }
 
     double uArmClass::calYonly(double theta_1, double theta_2, double theta_3)

uarm_library.h

@@ -22,7 +22,7 @@
 
 #define UARM_MAJOR_VERSION      1
 #define UARM_MINOR_VERSION      5
-#define UARM_BUGFIX             7
+#define UARM_BUGFIX             8
 
 #define SERVO_ROT_NUM           0
 #define SERVO_LEFT_NUM          1
@@ -155,7 +155,7 @@ class uArmClass
     }
 
     void writeStretch(double armStretch, double armHeight);
-
+    void writeAngle(double servo_rot_angle, double servo_left_angle, double servo_right_angle, double servo_hand_rot_angle);
 
     double getCalX() {
         return g_cal_x;