nuts_bolts.c

00001 /*
00002   nuts_bolts.c - Shared functions
00003   Part of Grbl
00004 
00005   Copyright (c) 2011-2016 Sungeun K. Jeon for Gnea Research LLC
00006   Copyright (c) 2009-2011 Simen Svale Skogsrud
00007 
00008   Grbl is free software: you can redistribute it and/or modify
00009   it under the terms of the GNU General Public License as published by
00010   the Free Software Foundation, either version 3 of the License, or
00011   (at your option) any later version.
00012 
00013   Grbl is distributed in the hope that it will be useful,
00014   but WITHOUT ANY WARRANTY; without even the implied warranty of
00015   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016   GNU General Public License for more details.
00017 
00018   You should have received a copy of the GNU General Public License
00019   along with Grbl.  If not, see <http://www.gnu.org/licenses/>.
00020 */
00021 
00022 #include "grbl.h"
00023 
00024 
00025 #define MAX_INT_DIGITS 8 // Maximum number of digits in int32 (and float)
00026 
00027 
00028 // Extracts a floating point value from a string. The following code is based loosely on
00029 // the avr-libc strtod() function by Michael Stumpf and Dmitry Xmelkov and many freely
00030 // available conversion method examples, but has been highly optimized for Grbl. For known
00031 // CNC applications, the typical decimal value is expected to be in the range of E0 to E-4.
00032 // Scientific notation is officially not supported by g-code, and the 'E' character may
00033 // be a g-code word on some CNC systems. So, 'E' notation will not be recognized.
00034 // NOTE: Thanks to Radu-Eosif Mihailescu for identifying the issues with using strtod().
00035 uint8_t read_float(char *line, uint8_t *char_counter, float *float_ptr)
00036 {
00037   char *ptr = line + *char_counter;
00038   unsigned char c;
00039 
00040   // Grab first character and increment pointer. No spaces assumed in line.
00041   c = *ptr++;
00042 
00043   // Capture initial positive/minus character
00044   bool isnegative = false;
00045   if (c == '-') {
00046     isnegative = true;
00047     c = *ptr++;
00048   } else if (c == '+') {
00049     c = *ptr++;
00050   }
00051 
00052   // Extract number into fast integer. Track decimal in terms of exponent value.
00053   uint32_t intval = 0;
00054   int8_t exp = 0;
00055   uint8_t ndigit = 0;
00056   bool isdecimal = false;
00057   while(1) {
00058     c -= '0';
00059     if (c <= 9) {
00060       ndigit++;
00061       if (ndigit <= MAX_INT_DIGITS) {
00062         if (isdecimal) { exp--; }
00063         intval = (((intval << 2) + intval) << 1) + c; // intval*10 + c
00064       } else {
00065         if (!(isdecimal)) { exp++; }  // Drop overflow digits
00066       }
00067     } else if (c == (('.'-'0') & 0xff)  &&  !(isdecimal)) {
00068       isdecimal = true;
00069     } else {
00070       break;
00071     }
00072     c = *ptr++;
00073   }
00074 
00075   // Return if no digits have been read.
00076   if (!ndigit) { return(false); };
00077 
00078   // Convert integer into floating point.
00079   float fval;
00080   fval = (float)intval;
00081 
00082   // Apply decimal. Should perform no more than two floating point multiplications for the
00083   // expected range of E0 to E-4.
00084   if (fval != 0) {
00085     while (exp <= -2) {
00086       fval *= 0.01f;
00087       exp += 2;
00088     }
00089     if (exp < 0) {
00090       fval *= 0.1f;
00091     } else if (exp > 0) {
00092       do {
00093         fval *= 10.0f;
00094       } while (--exp > 0);
00095     }
00096   }
00097 
00098   // Assign floating point value with correct sign.
00099   if (isnegative) {
00100     *float_ptr = -fval;
00101   } else {
00102     *float_ptr = fval;
00103   }
00104 
00105   *char_counter = ptr - line - 1; // Set char_counter to next statement
00106 
00107   return(true);
00108 }
00109 
00110 
00111 // Non-blocking delay function used for general operation and suspend features.
00112 void delay_sec(float seconds, uint8_t mode)
00113 {
00114     uint16_t i = (uint16_t)ceilf(1000 / DWELL_TIME_STEP*seconds);
00115     while (i-- > 0) {
00116         if (sys.abort) { return; }
00117         if (mode == DELAY_MODE_DWELL) {
00118             protocol_execute_realtime();
00119         } else { // DELAY_MODE_SYS_SUSPEND
00120           // Execute rt_system() only to avoid nesting suspend loops.
00121           protocol_exec_rt_system();
00122           if (sys.suspend & SUSPEND_RESTART_RETRACT) { return; } // Bail, if safety door reopens.
00123         }
00124         _delay_ms(DWELL_TIME_STEP); // Delay DWELL_TIME_STEP increment
00125     }
00126 }
00127 
00128 
00129 // Delays variable defined milliseconds. Compiler compatibility fix for _delay_ms(),
00130 // which only accepts constants in future compiler releases.
00131 void delay_ms(uint16_t ms)
00132 {
00133   while ( ms-- ) { _delay_ms(1); }
00134 }
00135 
00136 
00137 // Simple hypotenuse computation function.
00138 float hypot_f(float x, float y) { return(sqrtf(x*x + y*y)); }
00139 
00140 
00141 float convert_delta_vector_to_unit_vector(float *vector)
00142 {
00143   uint8_t idx;
00144   float magnitude = 0.0f;
00145   for (idx=0; idx<N_AXIS; idx++) {
00146     if (vector[idx] != 0.0f) {
00147       magnitude += vector[idx]*vector[idx];
00148     }
00149   }
00150   magnitude = sqrtf(magnitude);
00151   float inv_magnitude = 1.0f/magnitude;
00152   for (idx=0; idx<N_AXIS; idx++) { vector[idx] *= inv_magnitude; }
00153   return(magnitude);
00154 }
00155 
00156 
00157 float limit_value_by_axis_maximum(float *max_value, float *unit_vec)
00158 {
00159   uint8_t idx;
00160   float limit_value = SOME_LARGE_VALUE;
00161   for (idx=0; idx<N_AXIS; idx++) {
00162     if (unit_vec[idx] != 0) {  // Avoid divide by zero.
00163       limit_value = min(limit_value,fabsf(max_value[idx]/unit_vec[idx]));
00164     }
00165   }
00166   return(limit_value);
00167 }