For developers fixed point

lib/Marlin/Configuration.h

@@ -148,7 +148,7 @@
 
 // Optional custom name for your RepStrap or other custom machine
 // Displayed in the LCD "Ready" message
-//#define CUSTOM_MACHINE_NAME "3D Printer"
+#define CUSTOM_MACHINE_NAME "UArm Swift Pro"
 
 // Define this to set a unique identifier for this printer, (Used by some programs to differentiate between machines)
 // You can use an online service to generate a random UUID. (eg http://www.uuidgenerator.net/version4)
@@ -251,7 +251,7 @@
 #define TEMP_SENSOR_1 0
 #define TEMP_SENSOR_2 0
 #define TEMP_SENSOR_3 0
-#define TEMP_SENSOR_BED 0
+#define TEMP_SENSOR_BED 1
 
 // This makes temp sensor 1 a redundant sensor for sensor 0. If the temperatures difference between these sensors is to high the print will be aborted.
 //#define TEMP_SENSOR_1_AS_REDUNDANT
@@ -396,7 +396,7 @@
  */
 
 #define THERMAL_PROTECTION_HOTENDS // Enable thermal protection for all extruders
-//#define THERMAL_PROTECTION_BED     // Enable thermal protection for the heated bed
+#define THERMAL_PROTECTION_BED     // Enable thermal protection for the heated bed
 
 //===========================================================================
 //============================= Mechanical Settings =========================
@@ -837,7 +837,7 @@
 //
 // G20/G21 Inch mode support
 //
-//#define INCH_MODE_SUPPORT
+#define INCH_MODE_SUPPORT
 
 //
 // M149 Set temperature units support
@@ -1008,7 +1008,7 @@
 // IMPORTANT NOTE: The U8glib library is required for Full Graphic Display!
 //                 https://github.com/olikraus/U8glib_Arduino
 //
-//#define ULTRA_LCD   // Character based
+#define ULTRA_LCD   // Character based
 //#define DOGLCD      // Full graphics display
 
 //
@@ -1228,6 +1228,10 @@
 //
 //#define LCM1602
 
+// Grove controller
+
+#define LCD_GROVE_RGB
+
 //
 // PANELOLU2 LCD with status LEDs,
 // separate encoder and click inputs.

lib/Marlin/Marlin_main.cpp

@@ -8416,6 +8416,10 @@ void process_next_command() {
 	case 2120:
 		uarm_gcode_M2120();
 		break;
+
+	case 2121:
+		uarm_gcode_M2121();
+		break;
 
 	case 2122:
 		uarm_gcode_M2122();

lib/Marlin/cos_fix.c

@@ -0,0 +1,119 @@
+/*
+ * cos_fix.c: Fixed-point cos() function.
+ *
+ * Compile for AVR:
+ *      avr-gcc -c -mmcu=avr5 -Os -Wall -Wextra cos_fix.c
+ *
+ * Compile for AVR with no ASM code:
+ *      avr-gcc -DNO_ASM -c -mmcu=avr5 -Os -Wall -Wextra cos_fix.c
+ *
+ * Compile test program:
+ *      gcc -DRUN_TEST -O -Wall -Wextra cos_fix.c -o cos_fix
+ *
+ * Usage (test program):
+ *      ./cos_fix > cos_fix.tsv
+ */
+
+#include "cos_fix.h"
+
+#define FIXED(x, n) ((uint16_t)((float)(x) * ((uint32_t)1 << (n)) + .5))
+
+#if !defined(NO_ASM)
+# if defined(__AVR_HAVE_MUL__) && defined(__AVR_HAVE_MOVW__)
+#  define USE_AVR_ASM
+# endif
+#endif
+
+/*
+ * Fixed point multiplication.
+ *
+ * Multiply two fixed point numbers in u16,16 (unsigned 0.16) format.
+ * Returns result in the same format.
+ * Rounds to nearest, ties rounded up.
+ */
+static uint16_t mul_fix_u16(uint16_t x, uint16_t y)
+{
+    uint16_t result;
+
+#ifdef USE_AVR_ASM
+    /* Optimized ASM version. */
+    asm volatile(
+                 "mul  %B1, %B2\n\t"
+                 "movw %A0, r0\n\t"
+                 "ldi  r19, 0x80\n\t"
+                 "clr  r18\n\t"
+                 "mul  %A1, %A2\n\t"
+                 "add  r19, r1\n\t"
+                 "adc  %A0, r18\n\t"
+                 "adc  %B0, r18\n\t"
+                 "mul  %B1, %A2\n\t"
+                 "add  r19, r0\n\t"
+                 "adc  %A0, r1\n\t"
+                 "adc  %B0, r18\n\t"
+                 "mul  %A1, %B2\n\t"
+                 "add  r19, r0\n\t"
+                 "adc  %A0, r1\n\t"
+                 "adc  %B0, r18\n\t"
+                 "clr  r1"
+                 : "=&r" (result)
+                 : "r" (x), "r" (y)
+                 : "r18", "r19"
+                 );
+#else
+    /* Generic C version. Compiles to inefficient 32 bit code. */
+    result = ((uint32_t) x * y + 0x8000) >> 16;
+#endif
+    return result;
+}
+
+/*
+ * Cheap and rough fixed point multiplication: multiply only the high
+ * bytes of the operands, return 16 bit result.
+ *
+ * For some reason, the equivalent macro compiles to inefficient code.
+ * This compiles to 3 instructions (mul a,b; movw res,r0; clr r1).
+ */
+static uint16_t mul_high_bytes(uint16_t x, uint16_t y)
+{
+    return (uint8_t)(x >> 8) * (uint8_t)(y >> 8);
+}
+
+/*
+ * Fixed point cos() function: sixth degree polynomial approximation.
+ *
+ * argument is in units of 2*M_PI/2^16.
+ * result is in units of 1/2^14 (range = [-2^14 : 2^14]).
+ *
+ * Uses the approximation
+ *      cos(M_PI/2*x) ~ P(x^2), with
+ *      P(u) = (1 - u) * (1 - u * (0.23352 - 0.019531 * u))
+ * for x in [0 : 1]. Max error = 9.53e-5
+ */
+int16_t cos_fix(uint16_t x)
+{
+    uint16_t y, s;
+    uint8_t i = (x >> 8) & 0xc0;  // quadrant information
+    x = (x & 0x3fff) << 1;        // .15
+    if (i & 0x40) x = FIXED(1, 15) - x;
+    x = mul_fix_u16(x, x) << 1;   // .15
+    y = FIXED(1, 15) - x;         // .15
+    s = FIXED(0.23361, 16) - mul_high_bytes(FIXED(0.019531, 17), x); // .16
+    s = FIXED(1, 15) - mul_fix_u16(x, s);  // .15
+    s = mul_fix_u16(y, s);        // .14
+    return (i == 0x40 || i == 0x80) ? -s : s;
+}
+
+#ifdef RUN_TEST
+
+#include <stdio.h>
+
+/* Print out a table of values for checking accuracy. */
+int main(void)
+{
+    uint32_t x;
+    for (x = 0; x < (1UL << 16); x++)
+        printf("%u\t%d\t%g\n", (uint16_t) x, cos_fix(x));
+    return 0;
+}
+
+#endif

lib/Marlin/cos_fix.h

@@ -0,0 +1,36 @@
+/*
+ * cos_fix.h: Fixed-point cos() and sin() functions, based on a sixth
+ * degree polynomial approximation.
+ *
+ * argument is in units of 2*M_PI/2^16.
+ * result is in units of 1/2^14 (range = [-2^14 : 2^14]).
+ *
+ * The cosine function uses an even-polynomial approximation of
+ * cos(M_PI/2*x) for x in [0:1], and symmetries when x is outside [0:1].
+ * Sine is defined as sin(x) = cos(3*M_PI/2+x).
+ */
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+    /*
+     * Sixth degree polynomial:
+     *      cos(M_PI/2*x) ~ (1 - x^2)*(1 - x^2*(0.23352 - 0.019531*x^2))
+     * for x in [0:1]. Max error = 9.53e-5
+     */
+    int16_t cos_fix(uint16_t x);
+
+    /*
+     * Fixed point sin().
+     */
+    static inline int16_t sin_fix(uint16_t x)
+    {
+        return cos_fix(0xc000 + x);
+    }
+
+#ifdef __cplusplus
+}
+#endif

lib/Marlin/pins_Swift.h

@@ -108,13 +108,13 @@
 //#define PS_ON_PIN          12	// PS POWER 
 #define TEMP_0_PIN         13   // ANALOG NUMBERING
 #define TEMP_1_PIN         -1	// 14   // ANALOG NUMBERING
-#define TEMP_BED_PIN       -1 	//15   // ANALOG NUMBERING
+#define TEMP_BED_PIN       15 	//15   // ANALOG NUMBERING
 
 #define HEATER_0_PIN     9
 
 
 #define FAN_PIN        	8
-#define HEATER_BED_PIN 	-1
+#define HEATER_BED_PIN 	39
 
 #define VALVE_EN		4
 //#define PUMP_EN	//PG4

lib/Marlin/uArmAPI.cpp

@@ -9,6 +9,9 @@
 
 #include "uArmAPI.h" 
 #include "Grovergb_lcd.h"
+#include <FixedPoints.h>
+#include <FixedPointsCommon.h>
+#include "cos_fix.h"
 
 static float front_end_offset = 0.0;
 static float height_offset = 0.0;
@@ -652,16 +655,38 @@ void clear_acceleration_flag()
 
 unsigned char getXYZFromAngle(float& x, float& y, float& z, float rot, float left, float right)
 {
-	// 閿熸枻鎷稾Y骞抽敓鏂ゆ嫹閿熼叺闃熷府鎷烽敓鏂ゆ嫹�?	
-
-
-	double stretch = MATH_LOWER_ARM * cos(left / MATH_TRANS) + MATH_UPPER_ARM * cos(right / MATH_TRANS) + MATH_L2 + front_end_offset;
-
-	// 閿熸枻鎷穁閿熸枻鎷烽敓閰甸槦甯嫹閿熸枻鎷烽�?
-	double height = MATH_LOWER_ARM * sin(left / MATH_TRANS) - MATH_UPPER_ARM * sin(right / MATH_TRANS) + MATH_L1;
-	y = -stretch * cos(rot / MATH_TRANS);
-	x = stretch * sin(rot / MATH_TRANS);
-	z = height - height_offset;
+    SQ15x16 sqLowerArm = MATH_LOWER_ARM;
+    SQ15x16 sqTrans = 0.017453286279274;
+    SQ15x16 sqUpperArm = MATH_UPPER_ARM;
+    SQ15x16 sqL1 = MATH_L1;
+    SQ15x16 sqL2 = MATH_L2;
+
+    SQ15x16 leftfp = left;
+    SQ15x16 rightfp = right;
+    SQ15x16 rotfp  = rot;
+
+    SQ15x16 lowerCos = cos_fix(static_cast<float>(leftfp * sqTrans));
+    SQ15x16 lowerArmCalc = sqLowerArm * lowerCos;
+
+    SQ15x16 upperCos = cos_fix(static_cast<float>(rightfp * sqTrans));
+    SQ15x16 upperArmCalc = sqUpperArm * upperCos;
+    SQ15x16 stretchfp = lowerArmCalc + upperArmCalc;
+    stretchfp = stretchfp + sqL2 + front_end_offset;
+
+    SQ15x16 lowerSin = sin_fix(static_cast<float>(leftfp * sqTrans));
+    lowerArmCalc = sqLowerArm * lowerSin;
+    SQ15x16 upperSin = sin_fix(static_cast<float>(rightfp * sqTrans));
+    upperArmCalc = sqUpperArm * upperSin;
+
+    SQ15x16 heightfp = lowerArmCalc - upperArmCalc;
+    heightfp = heightfp + sqL1;
+
+    SQ15x16 cosRot = cos_fix(static_cast<float>(rotfp * sqTrans));
+    SQ15x16 sinRot = sin_fix(static_cast<float>(rotfp * sqTrans));
+
+    x = static_cast<float>(stretchfp * sinRot);
+    y = static_cast<float>(-stretchfp * cosRot);
+    z = static_cast<float>(heightfp - height_offset);
 
 	return 0;    
 }