remove CalXYZ from getCalX/Y/Z

Author: altjz

examples/GetXYZ/GetXYZ.ino

@@ -22,6 +22,7 @@ void loop() {
     uarm.detachAll();
     // assign robot coordinate to x,y,z.
     // unit is in centimeter
+    uarm.calXYZ();
     x = uarm.getCalX();  // assign robot x - axis coordinate to value x
     y = uarm.getCalY();
     z = uarm.getCalZ();

uarm_library.cpp

@@ -428,9 +428,9 @@ void uArmClass::calXYZ()
 {
 
   calXYZ(
-  uarm.readToAngle(analogRead(2),1,0),
-  uarm.readToAngle(analogRead(0),2,0),
-  uarm.readToAngle(analogRead(1),3,0));
+  uarm.readToAngle(analogRead(SERVO_ROT_ANALOG_PIN),SERVO_ROT_NUM,ABSOLUTE),
+  uarm.readToAngle(analogRead(SERVO_LEFT_ANALOG_PIN),SERVO_LEFT_NUM,ABSOLUTE),
+  uarm.readToAngle(analogRead(SERVO_RIGHT_ANALOG_PIN),SERVO_RIGHT_NUM,ABSOLUTE));
 
 }
 

uarm_library.h

@@ -79,80 +79,51 @@ class uArmClass
 	// void writeAngle(byte servo_rot_angle, byte servo_left_angle, byte servo_right_angle);
 	double readAngle(byte servo_num, byte trigger);
 
-	// const byte kAddrOffset      		= 90;
-	// const byte kAddrServo      			= 60;
-	// const byte kServoRotReadPin     	= 2;
-	// const byte kServoLeftReadPin    	= 0;
-	// const byte kServoRightReadPin    	= 1;
-	// const byte kServoHandRotReadPin     = 3;
-
 // Action control start
 
-	    void moveTo(double x, double y,double z);
-	    void moveTo(double x, double y,double z,int relative, double time);
-	    void moveTo(double x, double y,double z,int relative, double time_sepnd, double servo_4_angle);
-	    void moveTo(double x, double y, double z, int relative, double time_spend, int servo_4_relative, double servo_4_angle);
-	    void moveToAtOnce(double x, double y, double z, int relative, double servo_4_angle);
+	void moveTo(double x, double y,double z);
+	void moveTo(double x, double y,double z,int relative, double time);
+	void moveTo(double x, double y,double z,int relative, double time_sepnd, double servo_4_angle);
+	void moveTo(double x, double y, double z, int relative, double time_spend, int servo_4_relative, double servo_4_angle);
+	void moveToAtOnce(double x, double y, double z, int relative, double servo_4_angle);
 
-	    void drawCur(double length_1,double length_2,int angle, double time_spend);
-	    void drawRec(double length_1,double length_2,double time_spend_per_length);
+	void drawCur(double length_1,double length_2,int angle, double time_spend);
+	void drawRec(double length_1,double length_2,double time_spend_per_length);
 
-	    double getTheta1() const {return g_theta_1;}
-	    double getTheta2() const {return g_theta_2;}
-	    double getTheta3() const {return g_theta_3;}
+	double getTheta1() const {return g_theta_1;}
+	double getTheta2() const {return g_theta_2;}
+	double getTheta3() const {return g_theta_3;}
 
-	    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 getCalXYZ(double theta_1, double theta_2, double theta_3, double& x, double& y, double &z) {calXYZ(theta_1, theta_2, theta_3); x = g_cal_x; y = g_cal_y; z = g_cal_z;}
+	double getCalX() {return g_cal_x;}
+	double getCalY() {return g_cal_y;}
+	double getCalZ() {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 getCalXYZ(double theta_1, double theta_2, double theta_3, double& x, double& y, double &z) {calXYZ(theta_1, theta_2, theta_3); x = g_cal_x; y = g_cal_y; z = g_cal_z;}
 
-	    void calAngles(double x, double y, double z);
-      void getCalAngles(double x, double y, double z, double& theta_1, double& theta_2, double& theta_3) {calAngles(x, y, z); theta_1 = g_theta_1; theta_2 = g_theta_2; theta_3 = g_theta_3;}
+	void calAngles(double x, double y, double z);
+	void getCalAngles(double x, double y, double z, double& theta_1, double& theta_2, double& theta_3) {calAngles(x, y, z); theta_1 = g_theta_1; theta_2 = g_theta_2; theta_3 = g_theta_3;}
 
-	    void calXYZ(double theta_1, double theta_2, double theta_3);
-	    void calXYZ();
+	void calXYZ(double theta_1, double theta_2, double theta_3);
+	void calXYZ();
 
-	    void gripperCatch();
-	    void gripperRelease();
-		void interpolation(double init_val, double final_val, double (&interpol_val_array)[INTERP_INTVL]);
-		void pumpOn();
-		void pumpOff();
+	void gripperCatch();
+	void gripperRelease();
+	void interpolation(double init_val, double final_val, double (&interpol_val_array)[INTERP_INTVL]);
+	void pumpOn();
+	void pumpOff();
 protected:
-		// double getInterPolValueArray(int num) const {return g_interpol_val_arr[10];}
-		double calYonly(double theta_1, double theta_2, double theta_3);
-
-		// double g_x_in;
-		// double g_y_in;
-		// double g_z_in;
-		// double g_right_all;
-		// double g_sqrt_z_y;
-		double g_theta_1;
-		double g_theta_2;
-		double g_theta_3;
-		// double g_phi;
-		// double g_right_all_2;
-		// double g_sqrt_z_x;
-		double g_cal_x;
-		double g_cal_y;
-		double g_cal_z;
-
-		// double g_l3_1_2;
-		// double g_l4_1_2;
-		// double g_l5_2;
-
-		// double g_current_x;
-		// double g_current_y;
-		// double g_current_z;
-
-		
+	// double getInterPolValueArray(int num) const {return g_interpol_val_arr[10];}
+	double calYonly(double theta_1, double theta_2, double theta_3);
 
-		// double g_l3_1;
-		// double g_l4_1;
-		// double g_l5;
+	double g_theta_1;
+	double g_theta_2;
+	double g_theta_3;
 
+	double g_cal_x;
+	double g_cal_y;
+	double g_cal_z;
 
-		boolean g_gripper_reset;
+	boolean g_gripper_reset;
 // action control end
 private:
 	/*****************  Define variables  *****************/