diff --git a/lib/Marlin/Configuration.h b/lib/Marlin/Configuration.h index c2ac019..3a1fdf5 100644 --- a/lib/Marlin/Configuration.h +++ b/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. diff --git a/lib/Marlin/Marlin_main.cpp b/lib/Marlin/Marlin_main.cpp index de5ca59..648a0d5 100644 --- a/lib/Marlin/Marlin_main.cpp +++ b/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(); diff --git a/lib/Marlin/cos_fix.c b/lib/Marlin/cos_fix.c new file mode 100644 index 0000000..7c06155 --- /dev/null +++ b/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 + +/* 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 diff --git a/lib/Marlin/cos_fix.h b/lib/Marlin/cos_fix.h new file mode 100644 index 0000000..6216bc9 --- /dev/null +++ b/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 + +#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 diff --git a/lib/Marlin/pins_Swift.h b/lib/Marlin/pins_Swift.h index ca58c2a..aa52ba6 100644 --- a/lib/Marlin/pins_Swift.h +++ b/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 diff --git a/lib/Marlin/uArmAPI.cpp b/lib/Marlin/uArmAPI.cpp index e865918..1cf9fd4 100644 --- a/lib/Marlin/uArmAPI.cpp +++ b/lib/Marlin/uArmAPI.cpp @@ -9,6 +9,9 @@ #include "uArmAPI.h" #include "Grovergb_lcd.h" +#include +#include +#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(leftfp * sqTrans)); + SQ15x16 lowerArmCalc = sqLowerArm * lowerCos; + + SQ15x16 upperCos = cos_fix(static_cast(rightfp * sqTrans)); + SQ15x16 upperArmCalc = sqUpperArm * upperCos; + SQ15x16 stretchfp = lowerArmCalc + upperArmCalc; + stretchfp = stretchfp + sqL2 + front_end_offset; + + SQ15x16 lowerSin = sin_fix(static_cast(leftfp * sqTrans)); + lowerArmCalc = sqLowerArm * lowerSin; + SQ15x16 upperSin = sin_fix(static_cast(rightfp * sqTrans)); + upperArmCalc = sqUpperArm * upperSin; + + SQ15x16 heightfp = lowerArmCalc - upperArmCalc; + heightfp = heightfp + sqL1; + + SQ15x16 cosRot = cos_fix(static_cast(rotfp * sqTrans)); + SQ15x16 sinRot = sin_fix(static_cast(rotfp * sqTrans)); + + x = static_cast(stretchfp * sinRot); + y = static_cast(-stretchfp * cosRot); + z = static_cast(heightfp - height_offset); return 0; } diff --git a/lib/Marlin/uArmCalibration.cpp b/lib/Marlin/uArmCalibration.cpp index 2c266b7..241a624 100644 --- a/lib/Marlin/uArmCalibration.cpp +++ b/lib/Marlin/uArmCalibration.cpp @@ -9,6 +9,8 @@ #include "uArmCalibration.h" #include "macros.h" +#include +#include uint16_t reference_angle_value[NUM_AXIS] = {95, 1429, 1998, 0}; @@ -21,6 +23,8 @@ float reference_angle_B[NUM_AXIS] = {90.0, 33.138634, 88.795792, 0.0}; float (*reference_angle_p)[NUM_AXIS] = &reference_angle; +SQ15x16 angleConv = 0.087890625; + void init_reference_angle_value() { @@ -118,6 +122,146 @@ uint16_t get_current_angle_adc(uint8_t index) } float get_current_angle(uint8_t index) +{ + if (index > E_AXIS) + return 0; + + if (index == E_AXIS) + return get_current_angle_adc(index); + + //float angle = 0.0; + SQ15x16 angle = 0.0; + + uint16_t value[5] = {0.0}; + uint16_t max_value = 0; + uint16_t min_value = 0xffff; + uint32_t sum_value = 0; + bool invalid = false; + + do + { + invalid = false; + sum_value = 0; + max_value = 0; + min_value = 0xffff; + + for (int i = 0 ; i < 5; i++) + { + value[i] = get_current_angle_adc(index); + //debugPrint("value[%d] = %d\r\n", i, value[i]); + + if (max_value < value[i]) + max_value = value[i]; + + if (min_value > value[i]) + min_value = value[i]; + + sum_value += value[i]; + + } + + if (max_value - min_value > 20) + invalid = true; + }while(invalid); + + uint16_t cur_value = (sum_value - max_value - min_value) / 3;//get_current_angle_adc(index); + + //debugPrint("cur_value = %d\r\n", cur_value); + + + //uint16_t cur_value = get_current_angle_adc(index); + uint16_t diff = 0; + + if (cur_value > reference_angle_value[index]) + { + diff = cur_value - reference_angle_value[index]; + + SQ15x16 fpDiff = diff; + SQ15x16 angleDiff = fpDiff * angleConv; + SQ15x16 refAngle = (*reference_angle_p)[index]; + angle = static_cast(refAngle + angleDiff); + } + else if (cur_value < reference_angle_value[index]) + { + diff = reference_angle_value[index] - cur_value; + SQ15x16 fpDiff = diff; + SQ15x16 angleDiff = fpDiff * angleConv; + SQ15x16 refAngle = (*reference_angle_p)[index]; + angle = static_cast(refAngle - angleDiff); + } + else + { + angle = (*reference_angle_p)[index]; + } + + //debugPrint("cur_value = %d\r\n", cur_value); + //debugPrint("reference_value = %d\r\n", reference_angle_value[index]); + + //debugPrint("angle = %f\r\n", angle); + + float origin_angle = static_cast(angle); + float min = 0, max = 0; + + if (angle > 360) + { + angle -= 360; + } + + if (angle < 0) + { + angle += 360; + } + //debugPrint("angle2 = %f\r\n", angle); + + switch (index) + { + + case X_AXIS: + min = 0; + max = 180; + break; + + case Y_AXIS: + min = LOWER_ARM_MIN_ANGLE; + max = LOWER_ARM_MAX_ANGLE; + break; + + case Z_AXIS: + min = UPPER_ARM_MIN_ANGLE; + max = UPPER_ARM_MAX_ANGLE; + break; + + } + + if (angle >= min && angle <= max) + { + return static_cast(angle); + } + else if (diff > 2048) + { + if (origin_angle < 0) + { + return max; + } + else + { + return min; + } + } + else + { + if (origin_angle < 0) + { + return min; + } + else + { + return max; + } + } +} + +float get_current_angle2(uint8_t index) { if (index > E_AXIS) return 0; @@ -141,7 +285,7 @@ float get_current_angle(uint8_t index) max_value = 0; min_value = 0xffff; - for (int i = 0 ; i < 5; i++) + for (int i = 0 ; i < 1; i++) { value[i] = get_current_angle_adc(index); //debugPrint("value[%d] = %d\r\n", i, value[i]); @@ -161,8 +305,9 @@ float get_current_angle(uint8_t index) invalid = true; }while(invalid); - uint16_t cur_value = (sum_value - max_value - min_value) / 3;//get_current_angle_adc(index); + //uint16_t cur_value = (sum_value - max_value - min_value) / 3;//get_current_angle_adc(index); + uint16_t cur_value= sum_value; //debugPrint("cur_value = %d\r\n", cur_value); @@ -204,7 +349,8 @@ float get_current_angle(uint8_t index) //debugPrint("angle2 = %f\r\n", angle); switch (index) - { + { + case X_AXIS: min = 0; max = 180; diff --git a/lib/Marlin/uArmCalibration.h b/lib/Marlin/uArmCalibration.h index d8ecf30..be95174 100644 --- a/lib/Marlin/uArmCalibration.h +++ b/lib/Marlin/uArmCalibration.h @@ -20,6 +20,7 @@ #include "Z_IIC.h" float get_current_angle(uint8_t index); +float get_current_angle2(uint8_t index); void init_reference_angle_value(); void update_reference_angle_value(uint16_t value[NUM_AXIS]); void update_reference_angle_value_B(uint16_t value[NUM_AXIS]); diff --git a/lib/Marlin/uArmSwift.cpp b/lib/Marlin/uArmSwift.cpp index e7d9b1d..8ebaf6d 100644 --- a/lib/Marlin/uArmSwift.cpp +++ b/lib/Marlin/uArmSwift.cpp @@ -15,6 +15,10 @@ #include "Grovergb_lcd.h" #include "uArmGrove2.h" +#include +#include +#include "cos_fix.h" + // CAUTION: E_AXIS means FrontEnd Servo not extruder0 //float current_angle[NUM_AXIS] = { 0.0 }; @@ -26,9 +30,7 @@ extern float destination[NUM_AXIS]; bool block_running = false; -unsigned long tickStartTime = millis(); // get timestamp; - - +unsigned long tickStartTime = millis(); // get timestamp; uArmButton button_menu; uArmButton button_play; @@ -193,18 +195,40 @@ void swift_run() unsigned char getXYZFromAngleOrigin(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; - - // 閿熸枻鎷穁閿熸枻鎷烽敓閰甸槦甯嫹閿熸枻鎷烽? - double height = MATH_LOWER_ARM * sin(left / MATH_TRANS) - MATH_UPPER_ARM * sin(right / MATH_TRANS) + MATH_L1; - x = stretch * cos(rot / MATH_TRANS); - y = stretch * sin(rot / MATH_TRANS); - z = height; - - return 0; + 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(leftfp * sqTrans)); + SQ15x16 lowerArmCalc = sqLowerArm * lowerCos; + + SQ15x16 upperCos = cos_fix(static_cast(rightfp * sqTrans)); + SQ15x16 upperArmCalc = sqUpperArm * upperCos; + SQ15x16 stretchfp = lowerArmCalc + upperArmCalc; + stretchfp = stretchfp + sqL2; + + SQ15x16 lowerSin = sin_fix(static_cast(leftfp * sqTrans)); + lowerArmCalc = sqLowerArm * lowerSin; + SQ15x16 upperSin = sin_fix(static_cast(rightfp * sqTrans)); + upperArmCalc = sqUpperArm * upperSin; + + SQ15x16 heightfp = lowerArmCalc - upperArmCalc; + heightfp = heightfp + sqL1; + + SQ15x16 cosRot = cos_fix(static_cast(rotfp * sqTrans)); + SQ15x16 sinRot = sin_fix(static_cast(rotfp * sqTrans)); + + x = static_cast(stretchfp * cosRot); + y = static_cast(stretchfp * sinRot); + z = static_cast(heightfp); + + return 0; } float get_current_height() @@ -317,8 +341,6 @@ void uarm_gcode_G0() if (get_user_mode() == USER_MODE_LASER) { - - debugPrint("laser off\r\n"); @@ -399,6 +421,38 @@ void reportPos() reportString(result); } +void reportPos2() // report servo angles +{ + char result[128]; + //@3 X154.714 Y194.915 Z10.217\n + //msprintf(result, "@3 X%f Y%f Z%f\r\n", ); + float angle[NUM_AXIS]; + float pos[NUM_AXIS]; + + if (!isPowerPlugIn()) + { + MYSERIAL.println("No Power Connected!"); + return ; + } + + for (int i = 0; i < NUM_AXIS; i++) + { + angle[i] = get_current_angle2(i); + } + + msprintf(result,"@3 Ax%f, Ay%f, Az%f, Ae%f\r\n", angle[X_AXIS], angle[Y_AXIS], angle[Z_AXIS], angle[E_AXIS]); + + // get current pos + // getXYZFromAngle(pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], angle[X_AXIS], angle[Y_AXIS], angle[Z_AXIS]); + + // debugPrint("cur_pos: %f, %f, %f\r\n", pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS]); + + // msprintf(result, "@3 X%f Y%f Z%f R%f\r\n", pos[X_AXIS], pos[Y_AXIS], pos[Z_AXIS], angle[3]); + + reportString(result); +} + + void rotate_frontend_motor() { float angle = 0; @@ -468,6 +522,27 @@ void uarm_gcode_M2120() } } +void uarm_gcode_M2121() +{ + float interval = 0; + if (code_seen('V')) + { + interval = code_value_float(); + + interval *= 1000; + + if (interval == 0) + { + removeReportService(3); + } + else + { + addReportService(3, interval, reportPos2); + } + } +} + + void uarm_gcode_M2122() { uint8_t value = 0; diff --git a/lib/Marlin/uArmSwift.h b/lib/Marlin/uArmSwift.h index 424e5ce..a7f72d1 100644 --- a/lib/Marlin/uArmSwift.h +++ b/lib/Marlin/uArmSwift.h @@ -58,6 +58,7 @@ void uarm_gcode_G1(); void uarm_gcode_M2000(); void uarm_gcode_M2120(); +void uarm_gcode_M2121(); void uarm_gcode_M2122(); uint8_t uarm_gcode_M2200(char reply[]); diff --git a/lib/Marlin/ultralcd_impl_HD44780.h b/lib/Marlin/ultralcd_impl_HD44780.h index ab120c2..14a6f19 100644 --- a/lib/Marlin/ultralcd_impl_HD44780.h +++ b/lib/Marlin/ultralcd_impl_HD44780.h @@ -166,6 +166,11 @@ extern volatile uint8_t buttons; //an extended version of the last checked butt #include #define LCD_CLASS LiquidCrystal_I2C LCD_CLASS lcd(0x27, 2, 1, 0, 4, 5, 6, 7, 3, POSITIVE); +#elif ENABLED(LCD_GROVE_RGB) + #define LCD_CLASS Grovergb_lcd + #include + #include "Grovergb_lcd.h" + LCD_CLASS lcd; #else // Standard directly connected LCD implementations #include diff --git a/update.log b/update.log index d55bba9..2762458 100644 --- a/update.log +++ b/update.log @@ -1,3 +1,13 @@ +v3.2.0 20180213 M2121 command added which provides reduced averaging and accuracy but improves speed and keeps the arm movement undisturbed. +also the new command reports angular positions only + +v3.2.0 20180205 +Heated bed support - thermistor input into A15, digital heater bed switch on D39 +Support for G20/G21 code for Slic3r +UArm grove lcd module support +Ensure that you remove the Base LCD Library and replace with this: +https://bitbucket.org/fmalpartida/new-liquidcrystal/downloads/Newliquidcrystal_1.3.5.zip + v3.2.0 20171025 new stable version release.