Update 2020-07-10

README.md

@@ -1,10 +1,12 @@
 # Swift Pro Firmware V4.6.0 (Base on [Grbl v0.9j](https://github.com/grbl/grbl) )
 
 ----------
-## Update Summary for v4.6.0
-
-* Fixed air pump control delay problem
+## Update Summary for v4.7.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
 
 ## Caution
 #### The current firmware is not perfect and will be updated periodically

src/gcode.c

@@ -118,7 +118,6 @@ uint8_t gc_execute_line(char *line)
     if((letter < 'A') || (letter > 'Z')) { FAIL(STATUS_EXPECTED_COMMAND_LETTER); } // [Expected word letter]
     char_counter++;
     if (!read_float(line, &char_counter, &value)) { FAIL(STATUS_BAD_NUMBER_FORMAT); } // [Expected word value]
-
     // Convert values to smaller uint8 significand and mantissa values for parsing this word.
     // NOTE: Mantissa is multiplied by 100 to catch non-integer command values. This is more 
     // accurate than the NIST gcode requirement of x10 when used for commands, but not quite
@@ -371,7 +370,8 @@ uint8_t gc_execute_line(char *line)
           case 'T': word_bit = WORD_T; break; // gc.values.t = int_value;
           case 'X': word_bit = WORD_X; gc_block.values.xyz[X_AXIS] = value; axis_words |= (1<<X_AXIS); break;
           case 'Y': word_bit = WORD_Y; gc_block.values.xyz[Y_AXIS] = value; axis_words |= (1<<Y_AXIS); break;
-          case 'Z': word_bit = WORD_Z; gc_block.values.xyz[Z_AXIS] = value; axis_words |= (1<<Z_AXIS); break;
+          case 'Z': word_bit = WORD_Z; gc_block.values.xyz[Z_AXIS] = value; axis_words |= (1<<Z_AXIS); 
+						break;
           default: FAIL(STATUS_GCODE_UNSUPPORTED_COMMAND);
         } 
 
@@ -626,8 +626,8 @@ uint8_t gc_execute_line(char *line)
               if (gc_block.non_modal_command != NON_MODAL_ABSOLUTE_OVERRIDE) {
                 // Apply coordinate offsets based on distance mode.
                 if (gc_block.modal.distance == DISTANCE_MODE_ABSOLUTE) {
-                  gc_block.values.xyz[idx] += coordinate_data[idx] + gc_state.coord_offset[idx];
-                  if (idx == TOOL_LENGTH_OFFSET_AXIS) { gc_block.values.xyz[idx] += gc_state.tool_length_offset; }
+//                  gc_block.values.xyz[idx] += coordinate_data[idx] + gc_state.coord_offset[idx];
+                  if (idx == TOOL_LENGTH_OFFSET_AXIS) { gc_block.values.xyz[idx] += gc_state.tool_length_offset; }										
                 } else {  // Incremental mode
                   gc_block.values.xyz[idx] += gc_state.position[idx];
                 }
@@ -985,11 +985,11 @@ uint8_t gc_execute_line(char *line)
   if (gc_state.modal.motion != MOTION_MODE_NONE) {
     if (axis_command == AXIS_COMMAND_MOTION_MODE) {
       switch (gc_state.modal.motion) {
-        case MOTION_MODE_SEEK:
+        case MOTION_MODE_SEEK:;
           #ifdef USE_LINE_NUMBERS
             rtn_res = (uint8_t)mc_line(MOTION_MODE_SEEK, gc_block.values.xyz, gc_state.feed_rate, false, gc_state.line_number);
-          #else
-            rtn_res = (uint8_t)mc_line(MOTION_MODE_SEEK, gc_block.values.xyz, gc_state.feed_rate, false);
+          #else					
+            rtn_res = (uint8_t)mc_line(MOTION_MODE_SEEK, gc_block.values.xyz, gc_state.feed_rate, false);						
           #endif
           break;
         case MOTION_MODE_LINEAR:

src/motion_control.c

@@ -71,13 +71,13 @@
 	coord_effect2arm( &x, &y, &z );                               // calculate the arm current coord
 	coord_to_angle( x, y, z,																			
 									&final_angle[X_AXIS], &final_angle[Y_AXIS], &final_angle[Z_AXIS] ); // calculate final angle
-/*
-	char l_str[20], r_str[20], b_str[20];
-	dtostrf( final_angle[X_AXIS], 5, 4, l_str );
-	dtostrf( final_angle[Y_AXIS], 5, 4, r_str );
-	dtostrf( final_angle[Z_AXIS], 5, 4, b_str );
 
-	DB_PRINT_STR( "final_angle: %s, %s, %s\r\n", l_str, r_str, b_str );*/
+//	char l_str[20], r_str[20], b_str[20];
+//	dtostrf( final_angle[X_AXIS], 5, 4, l_str );
+//	dtostrf( final_angle[Y_AXIS], 5, 4, r_str );
+//	dtostrf( final_angle[Z_AXIS], 5, 4, b_str );
+
+//	DB_PRINT_STR( "final_angle: B%s L%s R%s\r\n", b_str, l_str, r_str );
 //	final_angle[X_AXIS] += 0.15;
 //	final_angle[Y_AXIS] -= 0.15;
 

src/report.c

@@ -86,10 +86,12 @@ void report_status_message(uint8_t status_code)
         case STATUS_GCODE_UNDEFINED_FEED_RATE:
         printPgmString(PSTR("Undefined feed rate")); break;
 
-				case STATUS_UARM_ERROR:
-					printString( "E21\n" );
-					break;
-
+		case STATUS_UARM_ERROR:
+			printString( "E21\n" );
+		break;
+		case STATUS_ENCODER_ERROR:
+			printString("E26\n");
+		break;			
         default:
           // Remaining g-code parser errors with error codes
           printPgmString(PSTR("Invalid gcode ID:"));

src/report.h

@@ -55,6 +55,7 @@
 #define STATUS_GCODE_G43_DYNAMIC_AXIS_ERROR 37
 
 #define STATUS_UARM_ERROR	50
+#define STATUS_ENCODER_ERROR 51
 
 // Define Grbl alarm codes.
 #define ALARM_HARD_LIMIT_ERROR 1

src/uarm_angle.c

@@ -17,17 +17,33 @@ uint16_t left_calibra_angle_map[64] 		= {0};		// <! angle 115.2~1.8
 uint16_t right_calibra_angle_map[61] 		= {0};    // <! angle -1.8~108
 
 
-
 // <! reference point angle
-#define POINT_A_ARML_ANGLE		(27.526186)
-#define POINT_A_ARMR_ANGLE		(111.687637)
-#define POINT_A_BASE_ANGLE		(90.0)
-#define POINT_B_ARML_ANGLE		(32.758930)
-#define POINT_B_ARMR_ANGLE		(88.980537)
-#define POINT_B_BASE_ANGLE		(90.0)
-#define POINT_C_ARML_ANGLE		(24.572023)
-#define POINT_C_ARMR_ANGLE		(62.595272)
-#define POINT_C_BASE_ANGLE		(90.0)
+#if defined(UARM_MINI)
+	#define POINT_A_ARML_ANGLE		(27.526186)
+	#define POINT_A_ARMR_ANGLE		(111.687637)
+	#define POINT_A_BASE_ANGLE		(90.0)
+	#define POINT_B_ARML_ANGLE		(20.0)
+	#define POINT_B_ARMR_ANGLE		(92.0)
+	#define POINT_B_BASE_ANGLE		(90.0)
+	#define POINT_C_ARML_ANGLE		(24.572023)
+	#define POINT_C_ARMR_ANGLE		(62.595272)
+	#define POINT_C_BASE_ANGLE		(90.0)
+#else
+	#define POINT_A_ARML_ANGLE		(27.526186)
+	#define POINT_A_ARMR_ANGLE		(111.687637)
+	#define POINT_A_BASE_ANGLE		(90.0)
+	#define POINT_B_ARML_ANGLE		(32.758930)
+	#define POINT_B_ARMR_ANGLE		(88.980537)
+	#define POINT_B_BASE_ANGLE		(90.0)
+	#define POINT_C_ARML_ANGLE		(24.572023)
+	#define POINT_C_ARMR_ANGLE		(62.595272)
+	#define POINT_C_BASE_ANGLE		(90.0)
+#endif
+
+
+
+
+
 
 
 
@@ -61,6 +77,59 @@ static uint16_t read_angle_reg_value(enum angle_channel_e channel){
 	return aver_value;
 }
 
+static uint16_t read_encoder_status_value(enum angle_channel_e channel)
+{
+	uint16_t temp_value = 0;
+	temp_value = (iic_read_byte(channel,(0x36<<1),0x0B)) & 0x18;
+	//temp_value = ((uint16_t)iic_read_byte(channel,(0x36<<1),0x05))<<8 |iic_read_byte(channel,(0x36<<1),0x06);
+	return temp_value;
+}
+
+bool check_encoder(enum angle_channel_e channel)
+{
+	uint16_t mang=0;
+	switch(channel)
+	{
+		case CHANNEL_ARML:
+			mang = read_encoder_status_value(CHANNEL_ARML);
+			//uart_printf( "left mang  : %d\r\n", mang );
+			if(mang == 24)
+			{
+				return true;
+			}
+			else 
+			{
+				return false;
+			}
+			break;
+		case CHANNEL_ARMR:
+			mang = read_encoder_status_value(CHANNEL_ARMR);
+			//uart_printf( "right mang  : %d\r\n", mang );
+			if(mang == 24)
+			{
+				return true;
+			}
+			else 
+			{
+				return false;
+			}
+			break;	
+		case CHANNEL_BASE:
+			mang = read_encoder_status_value(CHANNEL_BASE);
+			//uart_printf( "base mang  : %d\r\n", mang );
+			if(mang == 24)
+			{
+				return true;
+			}
+			else 
+			{
+				return false;
+			}
+			break;	
+
+	}
+}
+
 float calculate_current_angle(enum angle_channel_e channel){
 	uint16_t angle_reg_value = read_angle_reg_value(channel);
 	int long offset = 0;
@@ -381,28 +450,6 @@ void angle_sensor_init(void){
 	read_angle_reference_param();	// <! read reference param from eeprom 
 
 	read_angle_calibra_map();
-//	for( int i=0; i<sizeof(base_calibra_angle_map)/sizeof(uint16_t); i++ ){
-//		DB_PRINT_STR("%d : %d\r\n ", i, base_calibra_angle_map[i]);
-//		delay_ms(10);
-//	}
-
-//	for( int i=0; i<sizeof(left_calibra_angle_map)/sizeof(uint16_t); i++ ){
-//		DB_PRINT_STR("%d : %d\r\n ", i, left_calibra_angle_map[i]);
-//		delay_ms(10);
-//	}
-
-//	for( int i=0; i<sizeof(right_calibra_angle_map)/sizeof(uint16_t); i++ ){
-//		DB_PRINT_STR("%d : %d\r\n ", i, right_calibra_angle_map[i]);
-//		delay_ms(10);
-//	}
-
-
-//	uarm.init_arml_angle = calculate_current_angle(CHANNEL_ARML); 	// <! calculate init angle
-//	uarm.init_armr_angle = calculate_current_angle(CHANNEL_ARMR);
-//	uarm.init_base_angle = calculate_current_angle(CHANNEL_BASE);
-
-
-
 }
 
 

src/uarm_angle.h

@@ -18,6 +18,7 @@ void get_angle_reg_value(uint16_t *value);
 
 void single_point_reference(void);
 bool atuo_angle_calibra(void);
+bool check_encoder(enum angle_channel_e channel);
 
 
 #endif

src/uarm_common.h

@@ -16,6 +16,12 @@
 
 //#define UARM_MINI
 //#define UARM_2500
+#define DATA_TYPE_BYTE          1
+#define DATA_TYPE_INTEGER       2
+#define DATA_TYPE_FLOAT         4
+
+#define EXTERNAL_EEPROM_USER_ADDRESS 0xa0
+#define EXTERNAL_EEPROM_SYS_ADDRESS 0xa2
 
 
 #define EEPROM_ADDR_ANGLE_REFER		 				820U
@@ -44,7 +50,34 @@ enum uarm_work_mode_e {
 	WORK_MODE_TEST,
 };
 
-#if defined(UARM_2500)
+
+#if	defined(UARM_MINI)
+
+#define DEFAULT_NORMAL_HEIGHT 	55.0
+#define DEFAULT_NORMAL_FRONT		45.0
+
+#define DEFAULT_LASER_HEIGHT		0.0
+#define DEFAULT_LASER_FRONT 		0.0
+
+#define DEFAULT_3DPRINT_HEIGHT	0.0
+#define DEFAULT_3DPRINT_FRONT 	0.0
+
+#define DEFAULT_PEN_HEIGHT			0.0
+#define DEFAULT_PEN_FRONT 			0.0	
+
+#define DEFAULT_STEP_FLAT_HEIGHT		0.0
+#define DEFAULT_STEP_FLAT_FRONT 		0.0
+
+#define DEFAULT_STEP_STANDARD_HEIGHT		0.0
+#define DEFAULT_STEP_STANDARD_FRONT 		0.0
+
+#define DEFAULT_ROUND_PEN_HEIGHT		0.0
+#define DEFAULT_ROUND_PEN_FRONT 		0.0
+
+#define DEFAULT_TEST_HEIGHT 		0.0
+#define DEFAULT_TEST_FRONT			0.0
+
+#elif defined(UARM_2500)
 	#define DEFAULT_NORMAL_HEIGHT 	74.55
 	#define DEFAULT_NORMAL_FRONT		56.65
 
@@ -69,6 +102,7 @@ enum uarm_work_mode_e {
 	#define DEFAULT_TEST_HEIGHT 		25.49
 	#define DEFAULT_TEST_FRONT			44.5
 
+
 #else
 	#define DEFAULT_NORMAL_HEIGHT		74.55
 	#define DEFAULT_NORMAL_FRONT		56.65

src/uarm_coord_convert.c

@@ -1,25 +1,26 @@
 #include "uarm_coord_convert.h"
 
 #if defined(UARM_MINI)
-	#define ARM_A 		 (142.07)// Lower arm length
-	#define ARM_B 		 (142.07)// Upper arm length
+	#define ARM_A 		 (157.0)// Lower arm length
+	#define ARM_B 		 (157.0)// Upper arm length
 	#define ARM_2AB 	 (2*ARM_A*ARM_B)// 2*A*B
 	#define ARM_A2		 (ARM_A*ARM_A)// A^2
 	#define ARM_B2		 (ARM_B*ARM_B)// B^2
 	#define ARM_A2B2	 (ARM_A2+ARM_B2)// A^2+B^2
-	#define Z_BASIC 	 (108.8)
+	#define Z_BASIC 	 (116.55)
 	#define length_center_to_origin 	(13.2)
 	#define gearbox_ratio 		(4.5)
-	#define micro_steps 			(64.0)
+	#define micro_steps 			(32.0)
 	#define steps_per_angle 	(micro_steps/1.8*gearbox_ratio)
-	#define ARMA_MAX_ANGLE					(135.6)
+	#define ARMA_MAX_ANGLE					(100.0)
 	#define ARMA_MIN_ANGLE					(0)
-	#define ARMB_MAX_ANGLE					(119.9)
+	#define ARMB_MAX_ANGLE					(110.0)
 	#define ARMB_MIN_ANGLE					(0) 
 	#define BASE_MAX_ANGLE					(90)
 	#define BASE_MIN_ANGLE					(-90)
 	#define ARMA_ARMB_MAX_ANGLE 		(151)
 	#define ARMA_ARMB_MIN_ANGLE 		(10)
+
 #elif defined(UARM_2500)
 	#define ARM_A 		 (250.0)// Lower arm length
 	#define ARM_B 		 (250.0)// Upper arm length