Fix angle limit error

uArm-Developer · Aug 10, 2020 · 8a243e2 · 8a243e2
1 parent be08b9a
commit 8a243e2
Show file tree

Hide file tree

Showing 7 changed files with 27 additions and 8,101 deletions.
diff --git a/README.md b/README.md
@@ -1,12 +1,9 @@
-# Swift Pro Firmware V4.7.0 (Base on [Grbl v0.9j](https://github.com/grbl/grbl) )
+# Swift Pro Firmware V4.8.0 (Base on [Grbl v0.9j](https://github.com/grbl/grbl) )
 
 ----------
-## Update Summary for v4.7.0
+## Update Summary for v4.8.0
 
-* Add M2211 cmd to read external EEPROM
-* Add M2212 cmd to write external EEPROM
-* Add P2244 cmd to get the communication status of AS5600
-* Fix read bug of work mode
+* Fix angle limit error
 
 ## Caution
 #### The current firmware is not perfect and will be updated periodically

diff --git a/doc/uArm Swift Pro Protocol V4.0.5_cn .pdf b/doc/uArm Swift Pro Protocol V4.0.5_cn .pdf
diff --git a/doc/uArm Swift Pro Protocol V4.0.5_en .pdf b/doc/uArm Swift Pro Protocol V4.0.5_en .pdf
diff --git a/hex/uArm3Plus_V4.5.0_release_20190924.hex b/hex/uArm3Plus_V4.5.0_release_20190924.hex
diff --git a/hex/uArmPro_V4.7.0_release_20200710.hex b/hex/uArmPro_V4.7.0_release_20200710.hex
diff --git a/src/uarm_coord_convert.c b/src/uarm_coord_convert.c
@@ -54,9 +54,9 @@
 	#define micro_steps       (32.0)
 	#define steps_per_angle   (micro_steps/1.8*gearbox_ratio)
 	#define ARMA_MAX_ANGLE      		(135.6)
-	#define ARMA_MIN_ANGLE      		(0)
+	#define ARMA_MIN_ANGLE      		(-4.0)
 	#define ARMB_MAX_ANGLE      		(119.9)
-	#define ARMB_MIN_ANGLE      		(0)	
+	#define ARMB_MIN_ANGLE      		(-4.0)	
 	#define BASE_MAX_ANGLE					(90)
 	#define BASE_MIN_ANGLE					(-90)
 	#define ARMA_ARMB_MAX_ANGLE    	(151)
@@ -124,10 +124,10 @@ void angle_to_coord(float anglea, float angleb, float anglec, float *x, float *y
 void coord_to_angle(float x, float y, float z, float *anglea, float *angleb, float *anglec){		
 
 
-/*	DB_PRINT_STR("coord xyz:");
-	DB_PRINT_FLOAT(x);DB_PRINT_STR(" ");
-	DB_PRINT_FLOAT(y);DB_PRINT_STR(" ");
-	DB_PRINT_FLOAT(z);DB_PRINT_STR("\r\n"); 	*/
+//	DB_PRINT_STR("coord xyz:");
+//	DB_PRINT_FLOAT(x);DB_PRINT_STR(" ");
+//	DB_PRINT_FLOAT(y);DB_PRINT_STR(" ");
+//	DB_PRINT_FLOAT(z);DB_PRINT_STR("\r\n"); 
 
 
 
@@ -140,13 +140,23 @@ void coord_to_angle(float x, float y, float z, float *anglea, float *angleb, flo
 
 
   *anglea = acos((_stretch * xxx - _height * sqrt(4.0 * ARM_A2 * xx - xxx * xxx)) / (xx * 2.0 * ARM_A)) * RAD_TO_DEG;
-
   // get angle B
  	xxx = ARM_B2 -ARM_A2 + xx;
   *angleb = acos((_stretch * xxx + _height * sqrt(4.0 * ARM_B2 * xx -xxx * xxx)) / (xx * 2.0 * ARM_B)) * RAD_TO_DEG;
   if( _height > (ARM_A *sin(*anglea/RAD_TO_DEG)) ){
     *angleb = -*angleb;
   }
+
+//	DB_PRINT_FLOAT(fabs(z));DB_PRINT_STR(" ");
+//	DB_PRINT_FLOAT(ARM_B *sin(*angleb/RAD_TO_DEG));DB_PRINT_STR(" ");
+//	DB_PRINT_FLOAT(z);DB_PRINT_STR("\r\n"); 
+
+
+	if( fabs(_height)>(ARM_B *sin(*angleb/RAD_TO_DEG)) && _height<0.0  ){
+		*anglea = -*anglea;
+	}
+
+
 
   // get the base rotation angle
 
@@ -163,10 +173,10 @@ void coord_to_angle(float x, float y, float z, float *anglea, float *angleb, flo
 	}	
 
 
-/*	DB_PRINT_STR("coord abc:");
-	DB_PRINT_FLOAT(*anglea);DB_PRINT_STR(" ");
-	DB_PRINT_FLOAT(*angleb);DB_PRINT_STR(" ");
-	DB_PRINT_FLOAT(*anglec);DB_PRINT_STR("\r\n");*/
+//	DB_PRINT_STR("coord abc:");
+//	DB_PRINT_FLOAT(*anglea);DB_PRINT_STR(" ");
+//	DB_PRINT_FLOAT(*angleb);DB_PRINT_STR(" ");
+//	DB_PRINT_FLOAT(*anglec);DB_PRINT_STR("\r\n");
 }
 
 void get_current_step(float anglea, float angleb, float anglec, float *x_step, float *y_step, float *z_step){

diff --git a/src/uarm_swift.h b/src/uarm_swift.h
@@ -6,16 +6,16 @@
 
 
 #if defined(UARM_MINI)
-	#define DEVICE_NAME						"SwiftProMini"
+	#define DEVICE_NAME						"uArmMini"
 #elif	defined(UARM_2500)
 	#define DEVICE_NAME						"uArm3Plus"
 #else
 	#define DEVICE_NAME						"SwiftPro"
 #endif
 
 #define HARDWARE_VERSION			hardware_version
-#define SOFTWARE_VERSION			"V4.7.0"
-#define API_VERSION						"V4.0.4"
+#define SOFTWARE_VERSION			"V4.8.0"
+#define API_VERSION						"V4.0.5"
 #define BLE_UUID							bt_mac_addr