[Arduino] ELS do tokarki

Nie wiesz jak przetłumaczyć kod z innego języka? Napisz. Postaram/postaramy się pomóc.
C-->Bascom, Arduino-->Bascom. Nie bój żaby zapytać :D
ODPOWIEDZ
Awatar użytkownika
Jacek
Posty: 384
Rejestracja: 25 kwie 2016, 19:14

[Arduino] ELS do tokarki

Post autor: Jacek » 27 lut 2022, 11:41

Witam potrzebuje pomocy w przetłumaczeniu programu napisanego w Arduino na Bascom AVR

AtomicELS.ino
  1. // Copyright (c) 2020 Jon Bryan
  2. //
  3. // Permission is hereby granted, free of charge, to any person obtaining a copy
  4. // of this software and associated documentation files (the "Software"), to deal
  5. // in the Software without restriction, including without limitation the rights
  6. // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  7. // copies of the Software, and to permit persons to whom the Software is
  8. // furnished to do so, subject to the following conditions:
  9. //
  10. // The above copyright notice and this permission notice shall be included in all
  11. // copies or substantial portions of the Software.
  12. //
  13. // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  14. // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  15. // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  16. // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  17. // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  18. // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  19. // SOFTWARE.
  20.  
  21.  
  22.  
  23. //================================================================================
  24. //
  25. //  ELECTRONIC LEADSCREW
  26. //  Jon R. Bryan
  27. //  2020
  28. //
  29. //  This runs on an Arduino Mega (ATmega2560).
  30. //
  31. //  The target lathe in this case is a 1946 10" Logan Model 200, Serial #38108
  32. //
  33. //  The spindle encoder, which is mounted to the upper arm of the banjo, tucked in under
  34. //  the reversing lever, and driven 1:1 by a modified 60-tooth timing pulley mounted
  35. //  in place of the stud gear via a 6mm wide 220-2GT belt, outputs 800 counts per revolution.
  36. //  The CL57Y stepper controller/driver from omc-stepperonline.com is set to 400 counts per
  37. //  revolution, and controls a 3.5Nm Nema 24 motor mounted under the lower banjo arm.
  38. //  The stepper drives the leadscrew through 8:1 (15t/120t) HTD3M timing pulleys and
  39. //  a 16mm HTD420-3M belt (for 3200 steps per leadscrew revolution).
  40. //  Everything fits (barely) with no permanent modifications to the lathe.
  41. //  The leadscrew has a pitch of 8tpi (25600 steps per inch).
  42. //  The carriage feeds at a rate of 39.0625 microinches per step.
  43. //  It takes 256 steps to move the carriage or cross feed 0.010".
  44. //
  45. //  Pin change interrupts are used for spindle quadrature decoding and counting.
  46. //  A timer/counter in PWM mode outputs the appropriate number of leadscrew steps per spindle tick.
  47. //  Leadscrew steps are counted by a timer/counter overflow interrupt.
  48. //
  49. //  This will handle spindle speeds over 1500rpm up to one step for every spindle tick.
  50. //  With 800-count spindle encoder resolution that is an interrupt every 50us (20khz).
  51. //  The maximum rate that the stepper controller can handle is 200khz.
  52. //  The absolute theoretical maximum would be 10 step pulses per spindle count,
  53. //  which would translate to a nonsensical 0.250" per revolution feed at 1500rpm.
  54. //  As implemented, the output period is adjusted and the maximum spindle RPM recalculated
  55. //  for each feed rate to keep the maximum lead screw rate approximately constant
  56. //  over the lathe's speed range, resulting in a speed limit of 70rpm at the maximum
  57. //  rate of 4tpi (56rpm at 6.5mm).
  58. //
  59. //  A "Nextion" color LCD touchscreen, a 20ppr encoder knob and a
  60. //  MOM-OFF-MOM toggle switch provide the user interface.
  61. //  A great deal of functionality is bound up in the Nextion
  62. //  display programming, so refer to the Nextion configuration.
  63. //  There is no error checking in the Nextion communications, so there's a bit of risk there.
  64. //  The display is configured to return data only on failures.
  65. //  No problem as long as there's never a glitch.  An error-checking wrapper would be possible?
  66. //
  67. //================================================================================
  68.  
  69. #include "configuration.h"
  70. #include "tables.h"
  71.  
  72.  
  73. //Fixed synchronization bug 04/21/2020:jrb
  74. //Added RPM blanking according to feed rate 04/22/2020:jrb
  75.  
  76.  
  77. //================================================================================
  78. // Setup
  79. //================================================================================
  80.  
  81. void setup(void) {
  82.  
  83.   //Configure the input pins
  84.   pinMode(SPINDLE_A, INPUT);          //Spindle encoder quadrature inputs
  85.   pinMode(SPINDLE_B, INPUT);          //Put 2k pullups on these inputs to avoid spurious interrupts
  86.   pinMode(ALARM, INPUT_PULLUP);       //ALM+ open collector output from controller (not implemented yet)
  87.   pinMode(LEFT_MOM, INPUT_PULLUP);    //MOM-OFF-MOM toggle switch for controlling feed direction
  88.   pinMode(RIGHT_MOM, INPUT_PULLUP);
  89.  
  90.   Serial.begin(38400);               //Keep the default port for debugging
  91.   Serial2.begin(38400);               //Use USART2 for the Nextion display
  92.  
  93.   nextionInit();                      //Initialize the Nextion display
  94.   nextionLeft();                      //Defaults to feeding toward the headstock
  95.  
  96.   feedSelect(inch_feed);              //Default to INCH mode
  97.  
  98.   feedFill(inch[pitchFind("  40")].steps);  //Initialize the lookup table for 40tpi (1 step per spindle tick).
  99.  
  100.   tc3Init();                          //Initialize the timers
  101.   tc4Init();
  102.  
  103.   pcint4Enab();                       //Enable timer interrupt
  104.  
  105.   tc3Enab();                          //Enable timers
  106.   tc4Enab();
  107.  
  108.   zeroSet(!waitSerial2);              //Zero the leadscrew and clear the limits.
  109.  
  110.   //Falling edges are sharper, and generally provide better noise margin
  111.   attachInterrupt(digitalPinToInterrupt(KNOB_A), knob, FALLING);
  112.   attachInterrupt(digitalPinToInterrupt(SPINDLE_A), spindle, FALLING);
  113.  
  114.   steps = 0;
  115. }
  116.  
  117.  
  118.  
  119. //================================================================================
  120. // Loop
  121. //================================================================================
  122.  
  123. void loop(void) {
  124.  
  125.   int i;
  126.  
  127.   for (i = 0; i < 4; i++) {
  128.     knobCheck();        //Has the encoder knob been turned?
  129.     toggleCheck();      //Feed switch activated?
  130.     nextionCheck();     //Has the display sent anything?
  131.     nextionDirection(); //Correctly reflect the feed direction
  132.     nextionLead();      //Display the leadscrew position.
  133.     delay(25);          //No point in updating too fast
  134.   }
  135.  
  136.   nextionRPM();         //Display less often so the flicker isn't so distracting.
  137.  
  138. }
  139.  
  140.  
  141. /**************************************************************
  142. ***************************************************************
  143. ***                                                         ***
  144. ***   Functions that communicate with the Nextion display   ***
  145. ***                                                         ***
  146. ***************************************************************
  147. **************************************************************/
  148.  
  149. void nextionInit(void) {
  150.   //Initialize the state of the display buttons, etc.
  151.  
  152.   //I dislike the "magic numbers" for the colors, but I'm not enough of a
  153.   //C programmer to work around them without more effort than it seems to be worth here.
  154.   //F() puts the string constants in flash memory, at least.
  155.  
  156.   //The display design is done in the Nextion editor.
  157.   //Refer to the Nextion .HMI display configuration file for the object names and id's.
  158.  
  159.   Serial2.print(F("page start\xFF\xFF\xFF"));
  160.   Serial2.print(F("inch_btn.pco=WHITE\xFF\xFF\xFF"));               //INCH text white
  161.   Serial2.print(F("metric_btn.pco=BLACK\xFF\xFF\xFF"));             //METRIC black
  162.   Serial2.print(F("diam_btn.pco=BLACK\xFF\xFF\xFF"));               // etc...
  163.   Serial2.print(F("module_btn.pco=BLACK\xFF\xFF\xFF"));
  164.   Serial2.print(F("inch_btn.bco=1024\xFF\xFF\xFF"));                //INCH background toggle
  165.   Serial2.print(F("inch_btn.bco=44415\xFF\xFF\xFF"));               //44415 = pale blue
  166.   Serial2.print(F("shoulder.left_lim.txt=\"-----\"\xFF\xFF\xFF"));
  167.   Serial2.print(F("shoulder.right_lim.txt=\"-----\"\xFF\xFF\xFF"));
  168.   Serial2.print(F("shoulder.left_lim.bco=33816\xFF\xFF\xFF"));      //33816 = dark blue
  169.   Serial2.print(F("shoulder.right_lim.bco=33816\xFF\xFF\xFF"));     //33816 = dark blue
  170.   //Serial2.print(F("thup=1\xFF\xFF\xFF"));                         //Auto wake on touch
  171.   //Serial2.print(F("thsp=600\xFF\xFF\xFF"));                       //Sleep on no touch after 10 minutes
  172. }
  173.  
  174. void nextionRPM(void) {
  175.   // Sends the spindle rpm value to the display.
  176.  
  177.   char str[30];
  178.   static unsigned int spin;
  179.   int rpm;
  180.  
  181.   //If stopped, spin_rate is set to 0xFFFF by TC3 overflow interrupt.
  182.   if ((spin = spinAvg(spin_rate)) < RPM20) {
  183.     if (fault) {
  184.       //A fault means that an encoder interrupt occurred before all the steps were output,
  185.       //indicating that the lathe is running too fast for the feed rate.
  186.       //Make RPM background red.  Might need more for a color-blind operator.
  187.       sprintf(str, "%s%s", "rpm.bco=RED", NX_END);
  188.       Serial2.print(str);
  189.     }
  190.     rpm = T3CPM / spin / SCPR;
  191.     sprintf(str, "%s%d%s", "rpm.txt=\"", rpm, QTNX_END);
  192.   } else {
  193.     sprintf(str, "%s%s", "rpm.txt=\"0", QTNX_END);
  194.   }
  195.   Serial2.print(str);
  196. }
  197.  
  198. void nextionLead(void) {
  199.   //Output the leadscrew position to the display
  200.  
  201.   char leadstr[20];
  202.   char str[50];
  203.  
  204.   sprintf(str, "%s%s%s", "shoulder.leadscrew.txt=\"", leadStr(leadscrew, leadstr), QTNX_END);
  205.   Serial2.print(str);
  206. }
  207.  
  208. void nextionCheck(void) {
  209.   //Handler for touch events from the display
  210.  
  211.   char buf[20];
  212.  
  213.   //Things get a little complicated here.
  214.   //Wait for an event from the Nextion display.
  215.   //The mode buttons only send a "press" event.
  216.   //The left/right and limit buttons also send a "release" event
  217.   //which is used in the called functions to implement delays and spin checks.
  218.   if (Serial2.available()) {
  219.     Serial2.readBytes(buf, 7);
  220.     //The mode-select ID's 1 through 4 appear on all three Nextion pages.
  221.     //It took some care to make sure that the ID's were the same on each page.
  222.     //ID's 5 and 6 are the left and right feed select "arrows", 7 is the "BACK" button,
  223.     //and 8, 9 and 10 are the "LEFT", "ZERO" and "RIGHT" limit buttons.
  224.     //            touch              page 0            page 1            page 2             press
  225.     if (buf[0] == 0x65 && (buf[1] == 0x00 || buf[1] == 0x01 || buf[1] == 0x02) && buf[3] == 0x01) {
  226.       //Figure out which button press came from the touchscreen:
  227.       switch (buf[2]) {   // Component ID
  228.         case inch_btn:
  229.           feed_mode = inch_feed;
  230.           break;
  231.         case metric_btn:
  232.           feed_mode = metric_feed;
  233.           break;
  234.         case diametral_btn:
  235.           feed_mode = diametral_feed;
  236.           break;
  237.         case module_btn:
  238.           feed_mode = module_feed;
  239.           break;
  240.         case left_btn:
  241.           //Left arrow
  242.           jogCheck(true);
  243.           break;
  244.         case right_btn:
  245.           //Right arrow
  246.           jogCheck(false);
  247.           break;
  248.         case lset_btn:
  249.           leftSet();
  250.           break;
  251.         case zset_btn:
  252.           zeroSet(waitSerial2);
  253.           break;
  254.         case rset_btn:
  255.           rightSet();
  256.           break;
  257.         case lclr_btn:
  258.           leftClr(waitSerial2);
  259.           break;
  260.         case rclr_btn:
  261.           rightClr(waitSerial2);
  262.           break;
  263.       }
  264.     }
  265.     feedSelect(feed_mode);
  266.   }
  267. }
  268.  
  269. void leftSet(void) {
  270.   //Set the left limit and update the display
  271.  
  272.   char lead[10];
  273.   char str[30];
  274.  
  275.   left_limited = true;
  276.   left_limit = leadscrew;
  277.   sprintf(str, "%s%s%s", "shoulder.left_lim.bco=", nxDGREEN, NX_END);
  278.   Serial2.print(str);
  279.   sprintf(str, "%s%s%s", "shoulder.left_lim.txt=\"", leadStr(left_limit, lead), QTNX_END);
  280.   Serial2.print(str);
  281. }
  282.  
  283. void rightSet(void) {
  284.   //Set the right limit and update the display
  285.  
  286.   char lead[10];
  287.   char str[30];
  288.  
  289.   right_limited = true;
  290.   right_limit = leadscrew;
  291.   sprintf(str, "%s%s%s", "shoulder.right_lim.bco=", nxDGREEN, NX_END);
  292.   Serial2.print(str);
  293.   sprintf(str, "%s%s%s", "shoulder.right_lim.txt=\"", leadStr(right_limit, lead), QTNX_END);
  294.   Serial2.print(str);
  295. }
  296.  
  297. void leftClr(bool wait) {
  298.   //Clear the limit and update the display
  299.  
  300.   if (spin_rate != SPINDLE_STOPPED) {
  301.     //Refuse if the spindle is turning, for safety's sake
  302.     Serial2.print(F("shoulder.lclr_btn.bco2=RED\xFF\xFF\xFF"));
  303.   } else {
  304.     left_limited = false;
  305.     Serial2.print(F("shoulder.left_lim.bco=33816\xFF\xFF\xFF"));
  306.     Serial2.print(F("shoulder.left_lim.txt=\"-----\"\xFF\xFF\xFF"));
  307.   }
  308.   if (wait) {
  309.     while (!Serial2.available()); //Wait for the release event
  310.     Serial2.print(F("shoulder.lclr_btn.bco2=1024\xFF\xFF\xFF"));
  311.   }
  312. }
  313.  
  314. void rightClr(bool wait) {
  315.   //Clear the limit and update the display
  316.  
  317.   if (spin_rate != SPINDLE_STOPPED) {
  318.     //Refuse if the spindle is turning, for safety's sake
  319.     Serial2.print(F("shoulder.rclr_btn.bco2=RED\xFF\xFF\xFF"));
  320.   } else {
  321.     right_limited = false;
  322.     Serial2.print(F("shoulder.right_lim.bco=33816\xFF\xFF\xFF"));
  323.     Serial2.print(F("shoulder.right_lim.txt=\"-----\"\xFF\xFF\xFF"));
  324.   }
  325.   if (wait) {
  326.     while (!Serial2.available()); //Wait for the release event
  327.     Serial2.print(F("shoulder.rclr_btn.bco2=1024\xFF\xFF\xFF"));
  328.   }
  329. }
  330.  
  331. void zeroSet(bool wait) {
  332.   //Zero the leadscrew and clear the limits.
  333.   //Optionally wait for a button release event from the display.
  334.  
  335.   if (spin_rate != SPINDLE_STOPPED) {
  336.     //Refuse if the spindle is turning, for safety's sake,
  337.     //because having the spindle suddenly start moving could be bad.
  338.     Serial2.print(F("shoulder.zset_btn.bco2=RED\xFF\xFF\xFF"));
  339.   } else {
  340.     spin_count = 0;
  341.     leadscrew = 0L;
  342.     left_limit = 0L;
  343.     right_limit = 0L;
  344.     leftClr(!waitSerial2);
  345.     rightClr(!waitSerial2);
  346.   }
  347.   if (wait) {
  348.     while (!Serial2.available()); //Wait for the release event
  349.     Serial2.print(F("shoulder.zset_btn.bco2=1024\xFF\xFF\xFF"));
  350.   }
  351. }
  352.  
  353. void jogCheck(bool feed) {
  354.   //Decide whether it's safe to jog
  355.  
  356.   unsigned long timer = millis();
  357.   long lead;
  358.  
  359.   while (!Serial2.available()) {
  360.     //The button hasn't been released.
  361.     //Don't jog unless the Nextion button is held for 500ms.
  362.     //This needs to be extended to also jog from the toggle switch.
  363.     if ((millis() - timer) > 500UL) {
  364.       if (spin_rate != SPINDLE_STOPPED) {
  365.         //Refuse if the spindle is turning.
  366.         //I'm considering allowing creeping up on a shoulder and nudging the limit. It's on the list.
  367.         noJog(feed);
  368.       } else {
  369.         //Save the leadscrew position
  370.         lead = leadscrew;
  371.         //Spin the leadscrew
  372.         detachInterrupt(digitalPinToInterrupt(SPINDLE_A));    //No spindle interrupts
  373.         nextionJog(feed);
  374.         //Compensate the leadscrew value by the distance jogged
  375.         jogAdjust(leadscrew - lead);
  376.         attachInterrupt(digitalPinToInterrupt(SPINDLE_A), spindle, FALLING);
  377.       }
  378.     }
  379.   }
  380.   feed_left = feed;
  381. }
  382.  
  383. void nextionRamp(void) {
  384.   //Accelerate until the loop finishes at ICR4_MIN or the button is released
  385.   //This needs a little more work to handle parameter changes better
  386.  
  387.   unsigned int accel = ICR4_ACCEL;
  388.   unsigned int i, j;
  389.  
  390.   //Smoothly accelerate
  391.   for ( j = ICR4_MAX ; j != ICR4_MIN ; /* decrement j index in inner loop */ ) {
  392.     for ( i = 32 ; i > 0 ; i--, j -= accel ) {
  393.       _icr4 = j;  //ICR4 updates from this in the interrupt handler for smoothness
  394.       if ( Serial2.available() ) break ;  //Direction button release message received
  395.       nextionLead();
  396.       delay( 50 );  //Determines the time to ramp up
  397.     }
  398.     if ( Serial2.available() ) break ;
  399.     accel = accel / 2;
  400.   }
  401.   while ( !Serial2.available() ) {
  402.     //The jog button hasn't been released
  403.     //Still jogging at max speed, but need to keep the position updated
  404.     nextionLead();
  405.     delay( 25 );
  406.   }
  407. }
  408.  
  409. void noJog(bool feed) {
  410.   //Change the direction button background RED
  411.  
  412.   char str[30];
  413.  
  414.   //Turn left or right button RED
  415.   //Then wait until the button is released
  416.   if (feed) {
  417.     //You're on a page with a left_btn, so no need for the page name
  418.     sprintf(str, "%s%s%s", "left_btn.bco2=", "RED", NX_END);
  419.     Serial2.print(str);
  420.     while (!Serial2.available());
  421.     sprintf(str, "%s%s%s", "left_btn.bco2=", nxDGREEN, NX_END);
  422.     Serial2.print(str);
  423.   } else {
  424.     sprintf(str, "%s%s", "right_btn.bco2=RED", NX_END);
  425.     Serial2.print(str);
  426.     while (!Serial2.available());
  427.     sprintf(str, "%s%s%s", "right_btn.bco2=", nxDGREEN, NX_END);
  428.     Serial2.print(str);
  429.   }
  430. }
  431.  
  432. void nextionFeed(FEED_TABLE *table, int i) {
  433.   //Display the rate and pitch values
  434.  
  435.   char nxstr[50];
  436.   char rate[20];
  437.  
  438.   //Changing the rate always clears the fault
  439.   fault = false;
  440.  
  441.   sprintf(nxstr, "%s%s%s", "rpm.bco=", nxDBLUE, NX_END);
  442.   Serial2.print(nxstr);
  443.  
  444.   sprintf(rate, "%s%s", table[i].rate, QTNX_END);
  445.  
  446.   //Unfortunately, this has to be sent to all three pages
  447.   sprintf(nxstr, "%s%s", "start.rate.txt=\"", rate);
  448.   Serial2.print(nxstr);
  449.   sprintf(nxstr, "%s%s", "shoulder.rate.txt=\"", rate);
  450.   Serial2.print(nxstr);
  451.   sprintf(nxstr, "%s%s", "setup.rate.txt=\"", rate);
  452.   Serial2.print(nxstr);
  453.  
  454.   sprintf(rate, "%s%s", table[i].pitch, QTNX_END);
  455.  
  456.   sprintf(nxstr, "%s%s", "start.pitch.txt=\"", rate);
  457.   Serial2.print(nxstr);
  458.   sprintf(nxstr, "%s%s", "shoulder.pitch.txt=\"", rate);
  459.   Serial2.print(nxstr);
  460.   sprintf(nxstr, "%s%s", "setup.pitch.txt=\"", rate);
  461.   Serial2.print(nxstr);
  462. }
  463.  
  464. void nextionLeft(void) {
  465.   //"Click" the left direction button
  466.  
  467.   Serial2.print(F("click left_btn,1\xFF\xFF\xFF"));
  468.   Serial2.print(F("click left_btn,0\xFF\xFF\xFF"));
  469. }
  470.  
  471. void nextionRight(void) {
  472.   //Click the right direction button
  473.  
  474.   Serial2.print(F("click right_btn,1\xFF\xFF\xFF"));
  475.   Serial2.print(F("click right_btn,0\xFF\xFF\xFF"));
  476. }
  477.  
  478. void nextionDirection(void) {
  479.   //Update the display left/right feed buttons when feed changes direction.
  480.   //Selected feed direction stays "sticky".
  481.  
  482.   static bool last_spin;
  483.  
  484.   if (last_spin != spin_ccw) {
  485.     if (FEEDING_LEFT)
  486.       nextionLeft();
  487.     else
  488.       nextionRight();
  489.   }
  490.   last_spin = spin_ccw;
  491. }
  492.  
  493. void nextionUseRPM(void) {
  494.   //Blank the RPM values on the SETUP screen that are too high for the current feed rate
  495.   //"Too high" is defined as driving the stepper faster than 1500rpm.
  496.  
  497.   int i;
  498.  
  499.   for ( i=0 ; i<12 ; i++ ) {
  500.     if (steps_per_rev * (long)rpm_table[i] >= STEPPER_LIMIT) {
  501.       rpmHide(i);
  502.     } else {
  503.       rpmShow(i);
  504.     }
  505.   }
  506. }
  507.  
  508. void rpmHide(int i) {
  509.   char str[30];
  510.  
  511.   sprintf(str, "%s%s%s%s", "setup.", nxrpmID[i], ".pco=33816", NX_END);
  512.   Serial2.print(str);
  513. }
  514.  
  515. void rpmShow(int i) {
  516.   char str[30];
  517.  
  518.   sprintf(str, "%s%s%s%s", "setup.", nxrpmID[i], ".pco=WHITE", NX_END);
  519.   Serial2.print(str);
  520. }
  521.  
  522.  
  523. /**************************************
  524. ***************************************
  525. ***                                 ***
  526. ***        Support Functions        ***
  527. ***                                 ***
  528. ***************************************
  529. **************************************/
  530.  
  531. int pitchFind(const char *pitch) {
  532.   //Search the feed table for the pitch string and return the index.
  533.   //I got tired of doing this manually every time I changed the table.
  534.  
  535.   int i;
  536.  
  537.   for (i = 0 ; i < INCHES ; i++) {
  538.     if (strcmp(inch[i].pitch, pitch) == 0) return i;
  539.   }
  540. }
  541.  
  542. int rateFind(const char *rate){
  543.   //Search the feed table for the rate string and return the index.
  544.  
  545.   int i;
  546.  
  547.   for (i = 0 ; i < METRICS ; i++) {
  548.     if (strcmp(metric[i].rate, rate) == 0) return i;
  549.   }
  550. }
  551.  
  552. void feedSelect(int fmode) {
  553.   //Fill the step lookup table according to mode and feed rate, and update the display.
  554.  
  555.   //Default to 40tpi, 0.5mm, and lowest diametral and module.
  556.   static int i[4] = {pitchFind("  40"), rateFind("  0.5 "), 0, 0};
  557.  
  558.   switch (fmode) {
  559.     case inch_feed:
  560.       i[fmode] = knobCount(i[fmode], INCHES);   //Update and remember the rate
  561.       feedFill(inch[i[fmode]].steps);           //Update the lookup table
  562.       //Serial.println(inch[i[fmode]].steps);
  563.       nextionFeed(inch, i[fmode]);              //Update the display
  564.       nextionUseRPM();
  565.       break;
  566.     case metric_feed:
  567.       i[fmode] = knobCount(i[fmode], METRICS);
  568.       feedFill(metric[i[fmode]].steps);
  569.       //Serial.println(metric[i[fmode]].steps);
  570.       nextionFeed(metric, i[fmode]);
  571.       nextionUseRPM();
  572.       break;
  573.     case diametral_feed:
  574.       i[fmode] = knobCount(i[fmode], DIAMETRALS);
  575.       feedFill(diametral[i[fmode]].steps);
  576.       //Serial.println(diametral[i[fmode]].steps);
  577.       nextionFeed(diametral, i[fmode]);
  578.       nextionUseRPM();
  579.       break;
  580.     case module_feed:
  581.       i[fmode] = knobCount(i[fmode], MODULES);
  582.       feedFill(module[i[fmode]].steps);
  583.       //Serial.println(module[i[fmode]].steps);
  584.       nextionFeed(module, i[fmode]);
  585.       nextionUseRPM();
  586.       break;
  587.   }
  588. }
  589.  
  590. char *leadStr(long lead, char *leadstr) {
  591.   //Format and return the leadscrew position string
  592.  
  593.   char sign;
  594.   int whole, fraction;
  595.  
  596.   if (lead < 0)
  597.     sign = '-';
  598.   else
  599.     sign = ' ';
  600.   if (feed_mode == inch_feed || feed_mode == diametral_feed) {
  601.     //Format in inches
  602.     whole = abs(lead) / LSPI;
  603.     fraction = (abs(lead) * 10 / LSPM10) % 1000;          //Scale to 0.001"
  604.     sprintf(leadstr, "%c%d.%03d", sign, whole, fraction );
  605.   } else {
  606.     //Format in millimeters with extra precision
  607.     whole = abs(lead) * 10 / LSPMM10;  //Lead is a long, so the long calculation is cast to int
  608.     fraction = (abs(lead) * 10 % LSPMM10) * 100 / LSPMM10;  //Scale to .01mm
  609.     sprintf(leadstr, "%c%d.%02d", sign, whole, fraction );
  610.   }
  611.   return (leadstr);
  612. }
  613.  
  614. void feedFill(int steps_per) {
  615.   //Distribute the steps evenly around one spindle rotation
  616.  
  617.   int i;
  618.   int spsc;           //Steps per spindle count
  619.   int rem;            //Remainder
  620.   int sum = 0;        //For accumulating remainders
  621.  
  622.   steps_per_rev = steps_per;
  623.  
  624.   spsc = steps_per / SCPR;
  625.   rem = steps_per % SCPR;
  626.  
  627.   max_steps = spsc;
  628.  
  629.   if (rem)
  630.     max_steps++;
  631.  
  632.   for (i = 0; i < SCPR; i++) {
  633.     step_table[i] = spsc;
  634.     sum += rem;
  635.     //Sum the remainders and carry the one
  636.     if (sum >= SCPR) {
  637.       step_table[i]++;
  638.       sum -= SCPR;
  639.     }
  640.   }
  641.   pwmPeriodSet();
  642. }
  643.  
  644. int spinAvg(unsigned int rate) {
  645.   //Average 16 spindle rate readings for RPM calculation.
  646.  
  647.   static unsigned int spin[16];
  648.   unsigned long sum = 0L;
  649.   static int i = 0;
  650.   int j;
  651.  
  652.   spin[i++] = rate;
  653.   if (i == 16)
  654.     i = 0;
  655.   for (j = 0; j < 16; j++)
  656.     sum += spin[j];
  657.   return sum / 16;
  658. }
  659.  
  660. void spinModulus(bool ccw) {
  661.   //Keep track of the spindle position for synchronization
  662.   //Counts between 0 and SCPR-1
  663.  
  664.   if (ccw) {
  665.     if (spin_count < (SCPR - 1)) {
  666.       spin_count++;
  667.     } else {
  668.       spin_count = 0;
  669.     }
  670.   } else {
  671.     if (spin_count > 0) {
  672.       spin_count--;
  673.     } else {
  674.       spin_count = SCPR - 1;
  675.     }
  676.   }
  677. }
  678.  
  679. void jogAdjust(long jog_count) {
  680.   //Adjust spin_count as if the leadscrew movement had been
  681.   //generated by the spindle turning rather than jogging.
  682.  
  683.   int jog_adjust;
  684.  
  685.   //A few counts can get lost to truncation on each jog, but saving them would be complicated.
  686.   //Because jog_count is long, the calculation is performed with long precision.
  687.   //Jogging toward the head produces a negative jog count and jog_adjust value.
  688.  
  689.   jog_adjust = (jog_count * SCPR / steps_per_rev) % SCPR;
  690.  
  691.   if ((spin_count -= jog_adjust) < 0) {
  692.     spin_count += SCPR;
  693.   }
  694. }
  695.  
  696. void nextionJog(bool feed) {
  697.   //While the direction button is held, advance the leadscrew with smooth acceleration
  698.  
  699.   //Save the timer registers
  700.   volatile int icr4 = ICR4;
  701.   volatile int tcnt4 = TCNT4;
  702.  
  703.   jogging = true;
  704.  
  705.   pwmOn(feed);
  706.  
  707.   nextionRamp();
  708.  
  709.   pwmOff();
  710.   jogging = false;
  711.  
  712.   TCNT4 = tcnt4;    //Restore the timer registers
  713.   ICR4 = icr4;
  714. }
  715.  
  716. void knobCheck(void) {
  717.   //Check for encoder knob ticks
  718.  
  719.   if (knob_count) {
  720.     feedSelect(feed_mode);
  721.     nextionUseRPM();
  722.   }
  723. }
  724.  
  725. int knobCount(int i, int table_size) {
  726.   //Knob count doesn't really matter, only direction and table limits
  727.  
  728.   if (knob_count > 0) {
  729.     if (i < table_size - 1)
  730.       ++i;
  731.   } else if (knob_count < 0) {
  732.     if (i > 0)
  733.       --i;
  734.   }
  735.   knob_count = 0;
  736.   return i;
  737. }
  738.  
  739. void toggleCheck(void) {
  740.   //Check the state of the feed direction switch (active low).
  741.  
  742.   //I intend to add the jog function to the toggle switch, but haven't gotten to it yet.
  743.  
  744.   unsigned long timer = millis();
  745.   long lead;
  746.  
  747.   volatile int this_left = digitalRead(LEFT_MOM);
  748.   volatile int this_right = digitalRead(RIGHT_MOM);
  749.   static int last_left;
  750.   static int last_right;
  751.  
  752.   if (FEEDING_LEFT && (this_right == LOW)) {   //No need to do anything unless direction changes
  753.     if (last_right == LOW) {   //Very simple debounce
  754.       if (spin_ccw) {
  755.         feed_left = false;
  756.       } else {
  757.         feed_left = true;
  758.       }
  759.       nextionRight();
  760.     }
  761.   } else if (!FEEDING_LEFT && (this_left == LOW)) {
  762.     if (last_left == LOW) {
  763.       if (spin_ccw) {
  764.         feed_left = true;
  765.       } else {
  766.         feed_left = false;
  767.       }
  768.       nextionLeft();
  769.     }
  770.   }
  771.  
  772.   last_left = this_left;
  773.   last_right = this_right;
  774. }
  775.  
  776.  
  777. /********************************************************
  778. *********************************************************
  779. ***                                                   ***
  780. ***              REGISTER MANIPULATION                ***
  781. ***                                                   ***
  782. *********************************************************
  783. ********************************************************/
  784.  
  785. void pcint4Enab(void) {
  786.   //Interrupt on PB4 (Pin 21) falling edge from the knob
  787.  
  788.   EICRB |= _BV(ISC41);
  789.   EIMSK |= _BV(INT4);
  790. }
  791.  
  792. void tc3Init(void) {
  793.   // Configure TC3 for checking spindle speed
  794.  
  795.   TCCR3A  = _BV(COM3A0);    //Because the high byte doesn't increment in '00' mode
  796.   TCCR3B  = _BV(CS30);      //Run TCNT3 at 16Mhz to get better RPM resolution at high speed
  797.   tc3.low = TCNT3;          //Initialize to the current value of the counter
  798. }
  799.  
  800. void tc3Enab(void) {
  801.   //Enable TC3 overflow interrupt
  802.  
  803.   TIMSK3  = _BV(TOIE3);
  804. }
  805.  
  806. void tc4Init(void) {
  807.   //TC4 generates the step pulses to drive the leadscrew
  808.  
  809.   ICR4  = STP_MIN;                                    //Period defaults to minimum
  810.   OCR4A = PUL_MIN;                                    //Pulse width is constant
  811.   TCCR4B = _BV(WGM43) | _BV(WGM42);                   //Set Fast PWM Mode 14 with the clock off
  812.   PORTH |= _BV(DIR_N) | _BV(PUL_N);                   //Set high or it glitches low when enabled
  813.   DDRH  |= _BV(DDH4) | _BV(DDH3);                     //Set timer pins to output
  814.   TCCR4A =  _BV(WGM41) | _BV(COM4A1) | _BV(COM4A0);   //Set on compare match
  815.   TCNT4 = period_list[max_steps] - PUL_MIN;           //Preload the (stopped) counter for immediate pulse
  816. }
  817.  
  818. void tc4Enab(void) {
  819.   //Enable TC4 overflow interrupt
  820.  
  821.   TIMSK4 = _BV(OCIE4A);
  822. }
  823.  
  824. void pwmOn(bool feed) {
  825.   //Fast PWM Mode 14, inverting
  826.  
  827.   //Start by setting the direction bit
  828.   if (feed) {
  829.     PORTH |= _BV(DIR_N);
  830.   } else {
  831.     PORTH &= ~_BV(DIR_N);
  832.   }
  833.   //TOP in ICR4A, start the 2Mhz clock
  834.   TCCR4B = _BV(WGM43) | _BV(WGM42) | _BV(CS41);
  835. }
  836.  
  837. void pwmOff(void) {
  838.   //Just turn off the clock
  839.  
  840.   TCCR4B = _BV(WGM43) | _BV(WGM42);
  841. }
  842.  
  843. void pwmPeriodSet(void) {
  844.   //Set the period value according to the number of steps per spindle tick.
  845.   //Update ICR4 with interrupts off since it's not double-buffered.
  846.  
  847.   noInterrupts();
  848.   ICR4 = period_list[max_steps];
  849.   interrupts();
  850. }
  851.  
  852.  
  853. /********************************************************
  854. *********************************************************
  855. ***                                                   ***
  856. ***              INTERRUPT HANDLING                   ***
  857. ***                                                   ***
  858. *********************************************************
  859. ********************************************************/
  860.  
  861. void spindle(void) {
  862.   //This interrupt is called on falling edges of SPINDLE_A
  863.  
  864.   bool feeding_left;
  865.   static bool last_feed = feed_left;  //Just for the first time
  866.  
  867.   // First check if all steps have been sent from the last time.
  868.   // If steps are left over, the spindle is going too fast for the feed rate.
  869.   // A fault here will cause the displayed RPM to turn red, but nothing stops.
  870.   // Things can get strange with the driver if the difference is too extreme.
  871.   // Several times it has seemed to trip something so that both driver status LED's went out.
  872.   // Turning it off for a couple of minutes has reset it so far, so thermal protection?
  873.  
  874.   if (steps != 0)
  875.     fault = true;
  876.  
  877.   // SPINDLE_A brought us here, now determine the direction it's turning.
  878.   // The lathe's reverse lever position isn't known.  I keep it in the latched-down position.
  879.   // Adding a microswitch to the encoder bracket to sense it might be a future enhancement.
  880.  
  881.   // I do the simplest possible decoding of direction by simply looking at the state of phase B,
  882.   // which was much simpler and faster than full quadrature decoding.
  883.   // I started off with a 200 pulse-per-revolution rotary encoder and full decoding,
  884.   // but switched to an 800p/r encoder and this scheme for the equivalent resolution.
  885.   // Full decoding may have better noise immunity, but I haven't had any problems with this
  886.   // since adding 2k external pull-up resistors to the Arduino inputs to speed up the rising edges.
  887.   // The Arduino's internal pull-ups were just too big and slow.
  888.  
  889.   if (digitalRead(SPINDLE_B)) {   //Spindle is turning CCW if B is high
  890.     spinModulus(spin_ccw = true);
  891.   } else {
  892.     spinModulus(spin_ccw = false);
  893.   }
  894.  
  895.   if (feed_left != last_feed) {
  896.     //Changed direction, so remember sync count
  897.     synced = false;
  898.     //Keep the sync value between 0 and SCPR-1
  899.     if (spin_count) {   //Non-zero
  900.       sync_count = SCPR - spin_count;
  901.     } else {
  902.       sync_count = 0;
  903.     }
  904.   }
  905.  
  906.   //Save a few clock cycles later
  907.   feeding_left = (spin_ccw && feed_left) || (!spin_ccw && !feed_left);
  908.  
  909.   if (feeding_left) {
  910.     if (synced) {
  911.       //Is feeding left possible?
  912.       if ( !left_limited || (leadscrew > left_limit) ) {
  913.         //Turn on PWM if steps is non-zero.
  914.         if (steps = step_table[spin_count]) {
  915.           pwmOn(feeding_left);
  916.         }
  917.       } else {
  918.         //The left limit was just reached
  919.         synced = false;
  920.         if (spin_count) {
  921.           lsync_count = SCPR - spin_count;
  922.         } else {
  923.           lsync_count = 0;
  924.         }
  925.       }
  926.     } else if (right_limited && (leadscrew >= right_limit)) {
  927.       //Hit the right limit last time, so wait for sync before going left
  928.       if (spin_count == rsync_count) {
  929.         synced = true;
  930.       }
  931.     } else if (!left_limited || (leadscrew > left_limit)) {
  932.       //It was an on-the-fly direction change
  933.       if (spin_count == sync_count) {
  934.         synced = true;
  935.       }
  936.     }
  937.   } else {
  938.     if (synced) {
  939.       if ( !right_limited || (leadscrew < right_limit) ) {
  940.         if (steps = step_table[spin_count]) {
  941.           pwmOn(feeding_left);
  942.         }
  943.       } else {
  944.         synced = false;
  945.         if (spin_count) {
  946.           rsync_count = SCPR - spin_count;
  947.         } else {
  948.           rsync_count = 0;
  949.         }
  950.       }
  951.     } else if (left_limited && (leadscrew <= left_limit)) {
  952.       if (spin_count == lsync_count) {
  953.         synced = true;
  954.       }
  955.     } else if (!right_limited || (leadscrew < right_limit)) {
  956.       if (spin_count == sync_count) {
  957.         synced = true;
  958.       }
  959.     }
  960.   }
  961.  
  962.   last_feed = feed_left;
  963.  
  964.   tc3.low = TCNT3;  //tc3.high is handled by the counter overflow interrupt
  965.   spin_rate = tc3.count - last_tc3;
  966.   last_tc3 = tc3.count;
  967. }
  968.  
  969. void knob(void) {
  970.   //This interrupt is called by KNOB_A
  971.   //It really just keeps track of the direction of the click.
  972.  
  973.   if (digitalRead(KNOB_B)) {
  974.     knob_count-- ;
  975.   } else {
  976.     knob_count++ ;
  977.   }
  978. }
  979.  
  980. ISR(TIMER3_OVF_vect) {
  981.   // TIMER3 provides the master clock for determining spindle speed.
  982.   // This interrupt counts 16-bit timer overflows to extend the precision.
  983.   // It also provides a convenient place to check whether the spindle is moving.
  984.  
  985.   tc3.count += 0x10000;
  986.  
  987.   // Detect that the spindle has stopped
  988.   if (spin_rate == last_spin)
  989.     spin_rate = SPINDLE_STOPPED;
  990.  
  991.   last_spin = spin_rate;
  992. }
  993.  
  994. ISR(TIMER4_COMPA_vect) {
  995.   // This stops the PWM if all the steps for a spindle tick have been output,
  996.   // resets the counter for the next spindle tick according to feed rate,
  997.   // and keeps track of the carriage movement.
  998.  
  999.   static int i;
  1000.  
  1001.   if (!jogging) {
  1002.     if (--steps == 0) {   //Get out fast if there's another step coming
  1003.       pwmOff();
  1004.  
  1005.       //The clock is stopped, so TCNT4 is no longer incrementing.
  1006.       //Now, preload TCNT4 so that the first step pulse will be
  1007.       //output immediately when the clock is turned on by pwmOn().
  1008.       //This provides more margin after the last step is output.
  1009.       TCNT4 = period_list[max_steps] - PUL_MIN;
  1010.  
  1011.       //When feeding, the granularity of the leadscrew value
  1012.       //is determined by counts per spindle tick.
  1013.       if (FEEDING_LEFT) {
  1014.         leadscrew -= step_table[spin_count];
  1015.       } else {
  1016.         leadscrew += step_table[spin_count];
  1017.       }
  1018.     }
  1019.   } else {
  1020.     //Synchronously update ICR4 here for smoother acceleration
  1021.     ICR4 = _icr4;
  1022.     //When jogging, the leadscrew is incremented/decremented by one
  1023.     if (PORTH & _BV(DIR_N)) {   //Cheat a little here and read the bit directly
  1024.       --leadscrew;
  1025.     } else {
  1026.       ++leadscrew;
  1027.     }
  1028.   }
  1029. }
  1030.  
configuration.h
  1.  
  2. // Copyright (c) 2020 Jon Bryan
  3. //
  4. // Permission is hereby granted, free of charge, to any person obtaining a copy
  5. // of this software and associated documentation files (the "Software"), to deal
  6. // in the Software without restriction, including without limitation the rights
  7. // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  8. // copies of the Software, and to permit persons to whom the Software is
  9. // furnished to do so, subject to the following conditions:
  10. //
  11. // The above copyright notice and this permission notice shall be included in all
  12. // copies or substantial portions of the Software.
  13. //
  14. // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  15. // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  16. // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  17. // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  18. // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  19. // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  20. // SOFTWARE.
  21.  
  22.  
  23. #ifndef __CONFIGURATION_H
  24. #define __CONFIGURATION_H
  25.  
  26.  
  27.  
  28. //Added RPM blanking 04/22/2020:jrb
  29.  
  30. //================================================================================
  31. // Output Pins
  32. //================================================================================
  33.  
  34. #define PUL_N         PORTH3  //Stepper PUL- is on Pin 6, active low, with PUL+ tied to +5V
  35. #define DIR_N         PORTH4  //Stepper DIR- is on Pin 7, active low, with DIR+ tied to +5V
  36.  
  37.  
  38.  
  39. //================================================================================
  40. // Nextion miscellaneous
  41. //================================================================================
  42.  
  43. #define nxPBLUE "44415"   //Pale blue
  44. #define nxDBLUE "33816"   //Dark blue
  45. #define nxDGREEN "1024"   //Dark green
  46.  
  47. //Nextion display closing quote and three-character terminator
  48. #define QTNX_END "\"\xFF\xFF\xFF"
  49. //Without the closing quote
  50. #define NX_END "\xFF\xFF\xFF"
  51.  
  52. //Component ID's for the Nextion "buttons"
  53. enum nextionID {
  54.   inch_btn = 1,
  55.   metric_btn,
  56.   diametral_btn,
  57.   module_btn,
  58.   left_btn,
  59.   right_btn,
  60.   back_btn,
  61.   lset_btn,
  62.   zset_btn,
  63.   rset_btn,
  64.   lclr_btn = 27,    //Too much trouble to edit the display to keep them all in sequence
  65.   rclr_btn
  66. };
  67.  
  68. //ID's for the RPM text boxes
  69. //These are placed in the display with the ID's in order of descending RPM
  70. char nxrpmID[12][4] {
  71.   "t3", "t4",  "t5",  "t6",  "t7",  "t8",
  72.   "t9", "t10", "t11", "t12", "t13", "t14"
  73. };
  74.  
  75.  
  76. //================================================================================
  77. //Input Pins
  78. //================================================================================
  79.  
  80. // Wired to PE4/INT4 and PE5/INT5 on Mega2560 but Arduino remaps it for compatibility
  81. #define SPINDLE_B   2   //Encoder Phase B pin (PORTE4) pulled up with 2k
  82. #define SPINDLE_A   3   //Encoder Phase A pin (PORTE5) pulled up with 2k
  83.  
  84. #define LEFT_MOM    18  //Direction toggle switch
  85. #define RIGHT_MOM   19  //Direction toggle switch
  86. #define KNOB_B      20  //Knob Phase B pin (PORTD1) through 1k/0.1uf RC filter
  87. #define KNOB_A      21  //Knob Phase A pin (PORTD0) through 1k/0.1uf RC filter
  88. #define ALARM       13  //Microstepper controller ALARM open collector (not yet implemented)
  89.  
  90.  
  91.  
  92. //================================================================================
  93. // Scaler magic numbers
  94. //================================================================================
  95.  
  96. #define SCPR        1000//800     //Spindle encoder Counts Per Revolution
  97. #define MICROSTEPS  400     //Driver microsteps per revolution
  98. #define STEP_RATIO  8       //Stepper:Leadscrew ratio
  99. #define LTPI        8       //Leadscrew Threads Per Inch
  100. #define LSPI        25600   //Leadscrew Steps Per Inch (LTPI*MICROSTEPS*STEP_RATIO)
  101. #define LSPM10      256     //Leadscrew Steps Per Mil (0.001") * 10
  102. #define LSPMM10     10079   //Leadscrew Steps Per MilliMeter (LSPI / 25.4 * 10)
  103.  
  104.  
  105.  
  106. //================================================================================
  107. // Timing
  108. //================================================================================
  109.  
  110. #define STP_MIN   60      //30us period minimum to accommodate jitter (2Mhz clock)
  111. #define PUL_MIN   6       //3us pulse minimum for stepper drive
  112. #define RPM20     60000   //16Mhz clock ticks at 20rpm
  113.  
  114. // Timer 3 Counts Per Minute for calculating spindle RPM
  115. #define T3CPM     (16000000L * 60L)     //TC3 counts per minute, 16Mhz * 60 seconds
  116.  
  117.  
  118.  
  119. //================================================================================
  120. // Timer Parameters for Jogging
  121. //================================================================================
  122.  
  123. #define ICR4_MAX    0x8000    //Slow enough to adjust by thousandths
  124. #define ICR4_MIN    0x100     //Any faster can cause the microstepper to error out
  125. #define ICR4_ACCEL  0x200     //Initial increment for acceleration
  126.  
  127.  
  128.  
  129. //================================================================================
  130. // "Slowest possible" value to indicate that the spindle is stopped
  131. //================================================================================
  132.  
  133. #define SPINDLE_STOPPED   0xFFFF    //Set when the spindle isn't moving
  134.  
  135.  
  136.  
  137. //================================================================================
  138. // Steps per minute to spin the stepper motor at a "conservative" 1500rpm.
  139. // That doesn't mean that you can't get away with running a little faster.
  140. // I settled on 1500rpm because it provides some margin, and people will
  141. // always push the speed limit, don't you know. The torque curve on the
  142. // 3.5Nm Nema 24 motor that I'm using is plotted up to 2000rpm.
  143. // It's really starting to whistle at that speed, though.
  144. //================================================================================
  145.  
  146. #define STEPPER_LIMIT     600000L
  147.  
  148.  
  149.  
  150. //================================================================================
  151. // Flags
  152. //================================================================================
  153.  
  154. bool feed_left = true;        //Feed toward the headstock
  155. bool spin_ccw = true;         //Spindle direction
  156. bool fault = false;           //Step overrun flag
  157. bool jogging = false;         //Flag to control timer interrupt behavior
  158. bool right_limited = false;   //Flag to control feed limits
  159. bool left_limited = false;    //Flag to control feed limits
  160. bool synced = true;           //Flag for synchronizing the leadscrew with the spindle
  161. bool waitSerial2 = true;      //For zeroSet()
  162.  
  163. int knob_count;               //Knob counts (really just direction)
  164.  
  165. int spin_count = 0;           //Value from 0-799 (counts per rev)
  166.  
  167. int sync_count = 0;           //On-the-fly spindle count for synchronizing
  168. int lsync_count = 0;          //Spindle count upon reaching the left limit
  169. int rsync_count = 0;          //Likewise for the right
  170.  
  171. int steps_per_rev;            //Steps per spindle revolution for the current pitch
  172.  
  173. long leadscrew;               //Leadscrew counts
  174. long left_limit = 0L;         //Leadscrew value for left limit when enabled
  175. long right_limit = 0L;        //Leadscrew value for right limit
  176.  
  177. unsigned int spin_rate = SPINDLE_STOPPED;
  178. unsigned int last_spin;
  179.  
  180.  
  181.  
  182. //================================================================================
  183. // Feed direction logic
  184. //================================================================================
  185.  
  186. #define FEEDING_LEFT  ((spin_ccw && feed_left) || (!spin_ccw && !feed_left))
  187.  
  188.  
  189.  
  190. //================================================================================
  191. // Feed modes
  192. //================================================================================
  193.  
  194. enum {
  195.   inch_feed,
  196.   metric_feed,
  197.   diametral_feed,
  198.   module_feed
  199. } feed_mode = inch_feed ;
  200.  
  201.  
  202.  
  203. //================================================================================
  204. // Timers
  205. //================================================================================
  206.  
  207. union {
  208.   // Used to extend the resolution of TC3
  209.   unsigned long count;
  210.   unsigned int low, high;
  211. } tc3 ;
  212.  
  213. unsigned long last_tc3 = 0;   //This is used to calculate spindle RPM.
  214.  
  215. unsigned int last_step = 0;   //For determining pulse rate
  216.  
  217. unsigned int _icr4;           //Buffered value for ICR4 used during jogging
  218.  
  219. byte steps;                   //Steps per spindle tick
  220. byte step_table[SCPR];        //Lookup table for precalculated feed rate
  221. int max_steps;                //Maximum steps per spindle tick, used in several ways
  222.  
  223. //The measured spindle speeds on my lathe with a 1720rpm motor
  224. unsigned int rpm_table[12] { 1430, 812, 648, 463, 368, 238, 210, 135, 108, 77, 61, 34 };
  225.  
  226. //        "Official" values: 1450, 780, 620, 420, 334, 244, 179, 131, 104, 70, 56, 30
  227.  
  228. // The highest-speed entry in the Nextion display has an ID of "t3",
  229. // and the lowest is "t14", and they are in order from highest to lowest RPM.
  230. // To blank the highest entry send "setup.t3.pco=33816" to turn the text blue,
  231. // then to unblank it send "setup.t3.pco=WHITE" to turn it white again.
  232.  
  233. // This chart is reorganized from the Model 200 manual to reflect the physical belt position.
  234. // Eventually I want this chart to display/highlight RPM's that are valid for the feed rate.
  235. //
  236. // --------------------------------------------------------------------|
  237. // MOTOR    |                   SPINDLE BELT POSITION                  |
  238. // BELT     |----------------------------------------------------------|
  239. // POSITION |     DIRECT BELT DRIVE       |       BACK GEAR DRIVE      |
  240. // ---------|-----------------------------|----------------------------|
  241. //   HIGH   |  1450   |   780   |   420   |   244   |   131   |   70   |
  242. // ---------|---------|---------|---------|---------|---------|--------|
  243. //   LOW    |   620   |   334   |   179   |   104   |    56   |   30   |
  244. // --------------------------------------------------------------------|
  245.  
  246.  
  247. //For completeness I tried to include every tap pitch available from McMaster-Carr,
  248. //and every pitch that I could identify in pictures of old lathes.
  249. //I even included metric "British Association" pitches once I knew about them.
  250. //The maximum feed rate in each mode is limited to about 8000 steps per rev.
  251. //There are practical limits on spindle speed for given feed rates.
  252. //It worked well to scale the RPM speed steps directly as the steps per spindle tick increases.
  253. //A rate of 0.025" per rev moves the carriage 0.6" per second at 1450rpm, a fast clip.
  254. //That's the 1:1 rate, when a step is output for every spindle tick.
  255. //A 0.050" feed at 780rpm is moving 0.65" per second, 0.075" at 620rpm is 0.775" etc.
  256. //At the max inch rate of 0.25" per rev at 70rpm, that's fallen off to about 0.29" per rev.
  257. //That seems sensible, since the forces go up with the deeper corresponding cut.
  258. //We'll see how it works out in practice.
  259.  
  260. //I experimented with dynamically varying the step period, but it just got too fussy.
  261. //Maximum steps per spindle tick is 11, a carriage movement of 0.00043" (fine enough for me).
  262. // Period is roughly scaled to feed rate, for smoothing and to mitigate resonances.
  263. // 2Mhz / ((RPM*SCPR)/60) / max_steps
  264. //int period_list[12] { STP_MIN, STP_MIN, 96, 81, 90, 90, 69, 120, 143, 160, 214, 244 };
  265. // 80% of 2Mhz / ((RPM*SCPR)/60) / max_steps
  266.  
  267. //So after all that I wound up looking at it on the oscilloscope and picking arbitrary values.
  268. //Maybe I'll revisit this at a later date:
  269. int period_list[12] {
  270.   STP_MIN, STP_MIN, STP_MIN, STP_MIN, STP_MIN, STP_MIN,
  271.   STP_MIN, STP_MIN,      83,     104,     155,     194 };
  272.  
  273.  
  274. #endif // __CONFIGURATION_H
  275.  
tables.h
  1.  
  2. // Copyright (c) 2020 Jon Bryan
  3. //
  4. // Permission is hereby granted, free of charge, to any person obtaining a copy
  5. // of this software and associated documentation files (the "Software"), to deal
  6. // in the Software without restriction, including without limitation the rights
  7. // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  8. // copies of the Software, and to permit persons to whom the Software is
  9. // furnished to do so, subject to the following conditions:
  10. //
  11. // The above copyright notice and this permission notice shall be included in all
  12. // copies or substantial portions of the Software.
  13. //
  14. // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  15. // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  16. // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  17. // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  18. // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  19. // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
  20. // SOFTWARE.
  21.  
  22.  
  23. #ifndef __TABLES_H
  24. #define __TABLES_H
  25.  
  26.  
  27.  
  28. // Change Log:
  29. //  Added British Association threads to the Metric table 01/28/2020:JRB
  30. //  Added British "Cycle Engineering Institute" 62tpi 04/04/2020:JRB
  31.  
  32. //================================================================================
  33. // Lookup tables
  34. //================================================================================
  35.  
  36. struct FEED_TABLE {
  37.   unsigned int steps;     //Encoder steps per spindle revolution for a given pitch //Kroki enkodera na obrót wrzeciona dla danego skoku
  38.   char rate[7];           //Feed rate in inches or millimeters //Szybkość posuwu w calach lub milimetrach
  39.   char pitch[5];          //Threads per inch or special designations like "10BA" //Gwinty na cal lub specjalne oznaczenia, takie jak „10BA”
  40. };
  41.  
  42. // INCH mode steps per revolution = leadscrew steps per inch / pitch //Kroki w trybie CALOWYM na obrót = kroki śruby pociągowej na cal / skok
  43. #define INCH_STEPS(tpi) ((unsigned int) round((float)LSPI/(tpi)))
  44.  
  45. // METRIC mode steps per revolution = rate_mm * leadscrew steps per inch / 25.4 //Metryczne kroki na obrót = stopa_mm * kroki śruby pociągowej na cal / 25,4
  46. #define MM_STEPS(mm) ((unsigned int) round((float)(mm)*LSPI/25.4))
  47.  
  48. // DIAMETRAL mode steps per revolution = pi * leadscrew steps per inch / diametral pitch
  49. #define DIAM_STEPS(dpi) ((unsigned int) round(PI*(float)LSPI/(dpi)))
  50.  
  51. // MODULE mode steps per revolution = pi * rate_mm * LSPI / 25.4
  52. #define MOD_STEPS(mm) ((unsigned int) round(PI*(float)(mm)*LSPI/25.4))
  53.  
  54. //***********************************************************************************
  55. // Following are the lookup tables for the number of steps per spindle tick.        *
  56. // Steps per spindle tick for a given rate/pitch are precalculated for efficiency.  *
  57. //***********************************************************************************
  58.  
  59. const int INCHES = 76;    //Number of entries in the inch feed table
  60.  
  61. FEED_TABLE inch[INCHES] = {
  62.   {INCH_STEPS(2000),  "0.0005", "----"}, /* 0  */
  63.   {INCH_STEPS(1000),  "0.001 ", "----"}, /* 1  */
  64.   {INCH_STEPS(667),   "0.0015", "----"}, /* 2  */
  65.   {INCH_STEPS(500),   "0.002 ", "----"}, /* 3  */
  66.   {INCH_STEPS(400),   "0.0025", "----"}, /* 4  */
  67.   {INCH_STEPS(333),   "0.003 ", "----"}, /* 5  */
  68.   {INCH_STEPS(286),   "0.0035", "----"}, /* 6  */
  69.   {INCH_STEPS(250),   "0.004 ", "----"}, /* 7  */
  70.   {INCH_STEPS(240),   "0.0042", " 240"}, /* 8  */
  71.   {INCH_STEPS(224),   "0.0045", " 224"}, /* 9  */
  72.   {INCH_STEPS(216),   "0.0046", " 216"}, /* 10 */
  73.   {INCH_STEPS(208),   "0.0048", " 208"}, /* 11 */
  74.   {INCH_STEPS(192),   "0.0052", " 192"}, /* 12 */
  75.   {INCH_STEPS(184),   "0.0054", " 184"}, /* 13 */
  76.   {INCH_STEPS(176),   "0.0057", " 176"}, /* 14 */
  77.   {INCH_STEPS(160),   "0.0062", " 160"}, /* 15 */
  78.   {INCH_STEPS(144),   "0.0069", " 144"}, /* 16 */
  79.   {INCH_STEPS(128),   "0.0078", " 128"}, /* 17 */
  80.   {INCH_STEPS(120),   "0.0083", " 120"}, /* 18 */
  81.   {INCH_STEPS(112),   "0.0089", " 112"}, /* 19 */
  82.   {INCH_STEPS(108),   "0.0093", " 108"}, /* 20 */
  83.   {INCH_STEPS(104),   "0.0096", " 104"}, /* 21 */
  84.   {INCH_STEPS(100),   "0.010 ", " 100"}, /* 22 */
  85.   {INCH_STEPS(96),    "0.0104", "  96"}, /* 23 */
  86.   {INCH_STEPS(92),    "0.0109", "  92"}, /* 24 */
  87.   {INCH_STEPS(90),    "0.0111", "  90"}, /* 25 */
  88.   {INCH_STEPS(88),    "0.0114", "  88"}, /* 26 */
  89.   {INCH_STEPS(80),    "0.0125", "  80"}, /* 27 */
  90.   {INCH_STEPS(72),    "0.0139", "  72"}, /* 28 */
  91.   {INCH_STEPS(70),    "0.0143", "  70"}, /* 29 */
  92.   {INCH_STEPS(64),    "0.0156", "  64"}, /* 30 */
  93.   {INCH_STEPS(62),    "0.0161", "  62"}, /* 31 */     //CEI thread (old spoke nipples mostly, I guess)
  94.   {INCH_STEPS(60),    "0.0167", "  60"}, /* 32 */
  95.   {INCH_STEPS(56),    "0.0179", "  56"}, /* 33 */
  96.   {INCH_STEPS(54),    "0.0185", "  54"}, /* 34 */
  97.   {INCH_STEPS(52),    "0.0192", "  52"}, /* 35 */
  98.   {INCH_STEPS(50),    "0.020 ", "  50"}, /* 36 */
  99.   {INCH_STEPS(48),    "0.0208", "  48"}, /* 37 */
  100.   {INCH_STEPS(46),    "0.0217", "  46"}, /* 38 */
  101.   {INCH_STEPS(44),    "0.0227", "  44"}, /* 39 */
  102.   {INCH_STEPS(40),    "0.025 ", "  40"}, /* 40 */
  103.   {INCH_STEPS(36),    "0.0278", "  36"}, /* 41 */
  104.   {INCH_STEPS(32),    "0.0312", "  32"}, /* 42 */
  105.   {INCH_STEPS(30),    "0.0333", "  30"}, /* 43 */
  106.   {INCH_STEPS(28),    "0.0357", "  28"}, /* 44 */
  107.   {INCH_STEPS(27),    "0.037 ", "  27"}, /* 45 */
  108.   {INCH_STEPS(26),    "0.0385", "  26"}, /* 46 */
  109.   {INCH_STEPS(25),    "0.040 ", "  25"}, /* 47 */
  110.   {INCH_STEPS(24),    "0.0417", "  24"}, /* 48 */
  111.   {INCH_STEPS(23),    "0.0434", "  23"}, /* 49 */
  112.   {INCH_STEPS(22),    "0.0454", "  22"}, /* 50 */
  113.   {INCH_STEPS(20),    "0.050 ", "  20"}, /* 51 */
  114.   {INCH_STEPS(19),    "0.0526", "  19"}, /* 52 */      // British Standard Pipe or "G" thread
  115.   {INCH_STEPS(18),    "0.0555", "  18"}, /* 53 */
  116.   {INCH_STEPS(17.5),  "0.0571", " 17\xBD"}, /* 54 */   // xBD is the character for "1/2"
  117.   {INCH_STEPS(16),    "0.0625", "  16"}, /* 55 */
  118.   {INCH_STEPS(15),    "0.0667", "  15"}, /* 56 */
  119.   {INCH_STEPS(14),    "0.0714", "  14"}, /* 57 */
  120.   {INCH_STEPS(13.5),  "0.0741", " 13\xBD"}, /* 58 */
  121.   {INCH_STEPS(13),    "0.0769", "  13"   }, /* 59 */
  122.   {INCH_STEPS(12),    "0.0833", "  12"   }, /* 60 */
  123.   {INCH_STEPS(11.5),  "0.087 ", " 11\xBD"}, /* 61 */
  124.   {INCH_STEPS(11),    "0.0909", "  11"   }, /* 62 */
  125.   {INCH_STEPS(10),    "0.100 ", "  10"   }, /* 63 */
  126.   {INCH_STEPS(9),     "0.1111", "   9"   }, /* 64 */
  127.   {INCH_STEPS(8),     "0.125 ", "   8"   }, /* 65 */
  128.   {INCH_STEPS(7.5),   "0.1333", "  7\xBD"}, /* 66 */
  129.   {INCH_STEPS(7),     "0.1429", "   7"   }, /* 67 */
  130.   {INCH_STEPS(6.75),  "0.1481", "  6\xBE"}, /* 68 */   // xBE is the character for "3/4"
  131.   {INCH_STEPS(6.5),   "0.1538", "  6\xBD"}, /* 69 */
  132.   {INCH_STEPS(6),     "0.1667", "   6"   }, /* 70 */
  133.   {INCH_STEPS(5.75),  "0.1739", "  5\xBE"}, /* 71 */
  134.   {INCH_STEPS(5.5),   "0.1818", "  5\xBD"}, /* 72 */
  135.   {INCH_STEPS(5),     "0.200 ", "   5"   }, /* 73 */
  136.   {INCH_STEPS(4.5),   "0.2222", "  4\xBD"}, /* 74 */
  137.   {INCH_STEPS(4),     "0.250 ", "   4"   }  /* 75 */
  138. };
  139.  
  140. const int METRICS = 56 ;
  141.  
  142. FEED_TABLE metric[METRICS] = {
  143.   {MM_STEPS(0.01),  "  0.01", "----"}, /* 0  */
  144.   {MM_STEPS(0.02),  "  0.02", "----"}, /* 1  */
  145.   {MM_STEPS(0.03),  "  0.03", "----"}, /* 2  */
  146.   {MM_STEPS(0.04),  "  0.04", "----"}, /* 3  */
  147.   {MM_STEPS(0.05),  "  0.05", "----"}, /* 4  */
  148.   {MM_STEPS(0.06),  "  0.06", "----"}, /* 5  */
  149.   {MM_STEPS(0.07),  "  0.07", "----"}, /* 6  */
  150.   {MM_STEPS(0.08),  "  0.08", "----"}, /* 7  */
  151.   {MM_STEPS(0.09),  "  0.09", "----"}, /* 8  */
  152.   {MM_STEPS(0.10),  "  0.1 ", "----"}, /* 9  */
  153.   {MM_STEPS(0.12),  "  0.12", "----"}, /* 10 */
  154.   {MM_STEPS(0.15),  "  0.15", "----"}, /* 11 */
  155.   {MM_STEPS(0.20),  "  0.2 ", "----"}, /* 12 */
  156.   {MM_STEPS(0.225), " 0.225", "----"}, /* 13 */
  157.   {MM_STEPS(0.25),  "  0.25", "----"}, /* 14 */
  158.   {MM_STEPS(0.30),  "  0.3 ", "----"}, /* 15 */
  159.   {MM_STEPS(0.35),  "  0.35", "10BA"}, /* 16 */   // British Association Thread
  160.   {MM_STEPS(0.39),  "  0.39", " 9BA"}, /* 17 */
  161.   {MM_STEPS(0.40),  "  0.4 ", "----"}, /* 18 */
  162.   {MM_STEPS(0.43),  "  0.43", " 8BA"}, /* 19 */
  163.   {MM_STEPS(0.45),  "  0.45", "----"}, /* 20 */
  164.   {MM_STEPS(0.48),  "  0.48", " 7BA"}, /* 21 */
  165.   {MM_STEPS(0.50),  "  0.5 ", "----"}, /* 22 */
  166.   {MM_STEPS(0.53),  "  0.53", " 6BA"}, /* 23 */
  167.   {MM_STEPS(0.55),  "  0.55", "----"}, /* 24 */
  168.   {MM_STEPS(0.59),  "  0.59", " 5BA"}, /* 25 */
  169.   {MM_STEPS(0.60),  "  0.6 ", "----"}, /* 26 */
  170.   {MM_STEPS(0.65),  "  0.65", "----"}, /* 27 */
  171.   {MM_STEPS(0.66),  "  0.66", " 4BA"}, /* 28 */
  172.   {MM_STEPS(0.70),  "  0.7 ", "----"}, /* 29 */
  173.   {MM_STEPS(0.73),  "  0.73", " 3BA"}, /* 30 */
  174.   {MM_STEPS(0.75),  "  0.75", "----"}, /* 31 */
  175.   {MM_STEPS(0.80),  "  0.8 ", "----"}, /* 32 */
  176.   {MM_STEPS(0.81),  "  0.81", " 2BA"}, /* 33 */
  177.   {MM_STEPS(0.90),  "  0.9 ", " 1BA"}, /* 34 */
  178.   {MM_STEPS(1.00),  "  1.0 ", " 0BA"}, /* 35 */
  179.   {MM_STEPS(1.10),  "  1.1 ", "----"}, /* 36 */
  180.   {MM_STEPS(1.20),  "  1.2 ", "----"}, /* 37 */
  181.   {MM_STEPS(1.25),  "  1.25", "----"}, /* 38 */
  182.   {MM_STEPS(1.30),  "  1.3 ", "----"}, /* 39 */
  183.   {MM_STEPS(1.40),  "  1.4 ", "----"}, /* 40 */
  184.   {MM_STEPS(1.50),  "  1.5 ", "----"}, /* 41 */
  185.   {MM_STEPS(1.75),  "  1.75", "----"}, /* 42 */
  186.   {MM_STEPS(2.00),  "  2.0 ", "----"}, /* 43 */
  187.   {MM_STEPS(2.25),  "  2.25", "----"}, /* 44 */
  188.   {MM_STEPS(2.50),  "  2.5 ", "----"}, /* 45 */
  189.   {MM_STEPS(2.75),  "  2.75", "----"}, /* 46 */
  190.   {MM_STEPS(3.00),  "  3.0 ", "----"}, /* 47 */
  191.   {MM_STEPS(3.25),  "  3.25", "----"}, /* 48 */
  192.   {MM_STEPS(3.50),  "  3.5 ", "----"}, /* 49 */
  193.   {MM_STEPS(4.00),  "  4.0 ", "----"}, /* 50 */
  194.   {MM_STEPS(4.50),  "  4.5 ", "----"}, /* 51 */
  195.   {MM_STEPS(5.00),  "  5.0 ", "----"}, /* 52 */
  196.   {MM_STEPS(5.50),  "  5.5 ", "----"}, /* 53 */
  197.   {MM_STEPS(6.00),  "  6.0 ", "----"}, /* 54 */
  198.   {MM_STEPS(6.50),  "  6.5 ", "----"}  /* 55 */
  199. };
  200.  
  201. const int DIAMETRALS = 38;
  202.  
  203. FEED_TABLE diametral[DIAMETRALS] = {
  204.   {DIAM_STEPS(120),  "0.0262", " 120"}, /* 0  */
  205.   {DIAM_STEPS(112),  "0.0280", " 112"}, /* 1  */
  206.   {DIAM_STEPS(108),  "0.0291", " 108"}, /* 2  */
  207.   {DIAM_STEPS(104),  "0.0302", " 104"}, /* 3  */
  208.   {DIAM_STEPS( 96),  "0.0327", "  96"}, /* 4  */
  209.   {DIAM_STEPS( 92),  "0.0341", "  92"}, /* 5  */
  210.   {DIAM_STEPS( 88),  "0.0357", "  88"}, /* 6  */
  211.   {DIAM_STEPS( 80),  "0.0393", "  80"}, /* 7  */
  212.   {DIAM_STEPS( 76),  "0.0413", "  76"}, /* 8  */
  213.   {DIAM_STEPS( 72),  "0.0436", "  72"}, /* 9  */
  214.   {DIAM_STEPS( 64),  "0.0491", "  64"}, /* 10 */
  215.   {DIAM_STEPS( 60),  "0.0524", "  60"}, /* 11 */
  216.   {DIAM_STEPS( 56),  "0.0561", "  56"}, /* 12 */
  217.   {DIAM_STEPS( 54),  "0.0582", "  54"}, /* 13 */
  218.   {DIAM_STEPS( 52),  "0.0604", "  52"}, /* 14 */
  219.   {DIAM_STEPS( 48),  "0.0654", "  48"}, /* 15 */
  220.   {DIAM_STEPS( 46),  "0.0683", "  46"}, /* 16 */
  221.   {DIAM_STEPS( 44),  "0.0714", "  44"}, /* 17 */
  222.   {DIAM_STEPS( 40),  "0.0785", "  40"}, /* 18 */
  223.   {DIAM_STEPS( 38),  "0.0827", "  38"}, /* 19 */
  224.   {DIAM_STEPS( 36),  "0.0873", "  36"}, /* 20 */
  225.   {DIAM_STEPS( 32),  "0.0912", "  32"}, /* 21 */
  226.   {DIAM_STEPS( 30),  "0.1041", "  30"}, /* 22 */
  227.   {DIAM_STEPS( 28),  "0.1122", "  28"}, /* 23 */
  228.   {DIAM_STEPS( 27),  "0.1164", "  27"}, /* 24 */
  229.   {DIAM_STEPS( 26),  "0.1208", "  26"}, /* 25 */
  230.   {DIAM_STEPS( 24),  "0.1309", "  24"}, /* 26 */
  231.   {DIAM_STEPS( 23),  "0.1366", "  23"}, /* 27 */
  232.   {DIAM_STEPS( 22),  "0.1428", "  22"}, /* 28 */
  233.   {DIAM_STEPS( 20),  "0.1571", "  20"}, /* 29 */
  234.   {DIAM_STEPS( 19),  "0.1653", "  19"}, /* 30 */
  235.   {DIAM_STEPS( 18),  "0.1745", "  18"}, /* 31 */
  236.   {DIAM_STEPS( 16),  "0.1963", "  16"}, /* 32 */
  237.   {DIAM_STEPS( 15),  "0.2094", "  15"}, /* 33 */
  238.   {DIAM_STEPS( 14),  "0.2244", "  14"}, /* 34 */
  239.   {DIAM_STEPS(13.5), "0.2327", " 13\xBD"}, /* 35 */
  240.   {DIAM_STEPS( 13),  "0.2417", "  13"   }, /* 36 */
  241.   {DIAM_STEPS( 12),  "0.2618", "  12"   }  /* 37 */
  242. };
  243.  
  244. const int MODULES = 26;
  245.  
  246. FEED_TABLE module[MODULES] = {
  247.   {MOD_STEPS(0.20), "  0.2 ", "----"}, /* 0  */
  248.   {MOD_STEPS(0.25), "  0.25", "----"}, /* 1  */
  249.   {MOD_STEPS(0.30), "  0.3 ", "----"}, /* 2  */
  250.   {MOD_STEPS(0.35), "  0.35", "----"}, /* 3  */
  251.   {MOD_STEPS(0.40), "  0.4 ", "----"}, /* 4  */
  252.   {MOD_STEPS(0.45), "  0.45", "----"}, /* 5  */
  253.   {MOD_STEPS(0.50), "  0.5 ", "----"}, /* 6  */
  254.   {MOD_STEPS(0.55), "  0.55", "----"}, /* 7  */
  255.   {MOD_STEPS(0.60), "  0.6 ", "----"}, /* 8  */
  256.   {MOD_STEPS(0.65), "  0.65", "----"}, /* 9  */
  257.   {MOD_STEPS(0.70), "  0.7 ", "----"}, /* 10 */
  258.   {MOD_STEPS(0.80), "  0.8 ", "----"}, /* 11 */
  259.   {MOD_STEPS(0.90), "  0.9 ", "----"}, /* 12 */
  260.   {MOD_STEPS(0.95), "  0.95", "----"}, /* 13 */
  261.   {MOD_STEPS(1.00), "  1.0 ", "----"}, /* 14 */
  262.   {MOD_STEPS(1.10), "  1.1 ", "----"}, /* 15 */
  263.   {MOD_STEPS(1.20), "  1.2 ", "----"}, /* 16 */
  264.   {MOD_STEPS(1.25), "  1.25", "----"}, /* 17 */
  265.   {MOD_STEPS(1.30), "  1.3 ", "----"}, /* 18 */
  266.   {MOD_STEPS(1.40), "  1.4 ", "----"}, /* 19 */
  267.   {MOD_STEPS(1.50), "  1.5 ", "----"}, /* 20 */
  268.   {MOD_STEPS(1.60), "  1.6 ", "----"}, /* 21 */
  269.   {MOD_STEPS(1.75), "  1.75", "----"}, /* 22 */
  270.   {MOD_STEPS(1.80), "  1.8 ", "----"}, /* 23 */
  271.   {MOD_STEPS(1.90), "  1.9 ", "----"}, /* 24 */
  272.   {MOD_STEPS(2.00), "  2.0 ", "----"}  /* 25 */
  273. };
  274.  
  275.  
  276. #endif // __TABLES_H
  277.  
pozdrawiam Jacek.
ODPOWIEDZ