/
MovementThread.c
1322 lines (1197 loc) · 47.1 KB
/
MovementThread.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
// Movement Thread -- using G-Code G76 simulation instead of constant area removal
/*
ELeadscrew.c -- Mainline application for an electronic replacement of
gears used for threading or hobbing on a Lathe or Gear Hobber.
Copyright (C) 2005 John Dammeyer
This file is part of The Electronic Lead Screw (ELS) Project.
ELS is free software: you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
Or download here.
http://www.gnu.org/copyleft/gpl.html
John Dammeyer
johnd@autoartisans.com
Initial Version: 0.00
Version changes:
1.02m -- AutoInitialized PassCount and SpringPassCount to 0
1.10h -- See Config.h for description.
*/
#include "processor.h"
#include <Ctype.h>
#include <stdio.h>
#include <stdlib.h>
#include <float.h>
#include <math.h>
#include "common.h"
#include "config.h"
#include "int.h"
#include "menu.h"
#include "globvars.h"
#include "floatio.h"
#include "serial.h" // Need output buffer for diagnostics.
#include "Timer.h"
#include "MotorDriver.h" // Motor Driver Defintions.
#include "MovementThread.h" // Motor Thread Defintions.
/* ------------ Special Documention -------------*/
/*
G76 Simulation has arguments: Global Variable array indices
X -- X End position. END_X_NDX
Z -- Z End position THREAD_END_NDX
Q -- Spring Pass Count SPRING_PASS_CNT_NDX
P -- Pitch THREAD_SIZE_NDX
H -- First Pass cutting depth FIRST_X_PASS_DEPTH_NDX
I -- In feed thread angle. THREAD_ANGLE_NDX
R -- X Start BEGIN_X_NDX
K -- Z Start position with room for synchronization THREAD_BEGIN_NDX
L -- Thread chamfor... not used in ELS.
C -- X retracted position RETRACTED_X_NDX
B -- Last Pass cutting depth END_X_PASS_DEPTH_NDX
T -- Taper angle. Not used directly. System uses " per foot. TAPER_NDX
J -- Cutting pass depth X_PASS_DEPTH_NDX
Variables used for calculating new position of X,Z after each threading pass.
When the system is READY (Green LED Flashes) and the FNC3 key is pressed
the menu shows:
PASS COUNT = nn
CNCL RST SET CLR
When RST is pressed a recalculation is performed and Pass Count is updated.
This way an operator can start and stop the threading but has to explicitely
change to a new set of the threading operations.
When the SET softkey is pressed the display is changed to acitivate the
THREAD PARAMETERS Menu shown below.
THREAD PARAMETERS
TURN THRD POS JOG
|
PITCH ANGLE PASS SPRING
FLOAT FLOAT | Spring Pass Count
| uint8 n
|
THREAD Pass Param
DEPTH FRST EACH LAST
| | | |
| | | Last Path Depth
| | | FLOAT 0.dddd
| | |
| | Each Pass Depth
| | FLOAT 0.dddd
| |
| First Pass Depth
| FLOAT 0.dddd
|
Depth of Thread
FLOAT d.dddd
*/
/* ------------ Private Declarations and Data -------------*/
// Enable MOVE_DIAGNOSTICS in order to have printf statements for debugging.
// The other diagnostic flags are in config.h
//#define MOVE_DIAGNOSTICS 1
#ifdef MOVE_DIAGNOSTICS
#define M_DEBUGSTR(s) printf((far rom int8 *)s)
#else
#define M_DEBUGSTR(s)
#endif
enum PASS_STATES PassState = CALCULATE_PASSES;
static float32 TAN_Angle, Adjust_pass, PassDepth, EachPassDepth;
float32 PassDistance;
/* ------------ Public Data -------------*/
enum MOVEMENT_STATES MovementState = MOVE_WAIT;
int32 ZBeginPositionSteps, ZEndPositionSteps;
int32 ZIncrementDistanceSteps;
float32 ZBeginPosition, ZEndPosition;
int32 XRetractedPositionSteps, XBeginPositionSteps, XEndPositionSteps;
int32 XPassSteps; // Amount to move X per pas
float32 XRetractedPosition, XBeginPosition, XEndPosition;
float32 AdjustPass, RunTimeAdjust, RunTimeLast; //Added RE
int8 PassCount = 0;
int8 SpringPassCount = 0;
int8 AdjustPasses = 0; //Added RE
int16 EachPasses = 0; //Added RE
BITS MovementRunFlags;
/* ------------ Private Functions and Declarations -------------*/
/*
* FUNCTION: SetCalculatePositionState
*
* PARAMETERS: state -- State to change to
*
* USES GLOBALS: passState
*
* DESCRIPTION: Wrapper function to change state of private variable.
*
* RETURNS: Nothing.
*
*/
void
SetCalculatePositionState( enum PASS_STATES state ){
PassState = state;
}
/*
* FUNCTION: SetCalculatePositionState
*
* PARAMETERS: None.
*
* USES GLOBALS:
ZBeginPositionSteps,
ZEndPositionSteps,
XRetractedPositionSteps,
XBeginPositionSteps
*
* DESCRIPTION: Calculates and returns XBeginPositionSteps based on whether we're tapering
or not. If tapering then the begin position is dependant on a specific Z position
which is dependant on the taper angle.
*
* RETURNS: XBeginPositionSteps
*
*/
long
GetXBeginPositionSteps(void) {
float steps, temp;
if (fTapering) {// Need to protect ZMotorPosition since it's changed by interrupts but, we're not really moving here either.
steps = (float)(ZBeginPositionSteps - ZMotorPosition);
if (steps < 0.0 ) steps = -steps;
temp = GetGlobalVarFloat(TAPER_NDX);
if (temp < 0.0) {
temp = 0-temp; // Make positive.
}
// The taper is stored in diameter per 1" distance, we're doing radius movements.
temp /= 2.0; // So we move half the X distance to get the radius.
steps *= temp; // Now we have the proper X position based on X BEGIN = 0.000
// But really, this could be non-zero so we'd better get it.
XBeginPosition = (fMetricMode) ? GetGlobalVarFloat(BEGIN_X_NDX) * 25.4 : GetGlobalVarFloat(BEGIN_X_NDX);
// And then add in the BEGIN POSITION parameter.
XBeginPositionSteps = XDistanceToSteps(XBeginPosition) + steps;
}
return(XBeginPositionSteps);
}
long
GetXRetractedPositionSteps(void) {
}
/*
* FUNCTION: CalculateCompoundPosition
*
* PARAMETERS: None
*
* USES GLOBALS: MOTOR_STEPS_REV_X_NDX
* CROSS_SLIDE_IPITCH_NDX
* fAutoX
* PassDepth
* PassDistance
* fExternalThreading
*
* DESCRIPTION: On each pass, the tool bit is moved in along the hypotenuse of the triangle
* based on the distance desired and the the thread angle parameter. The X axis
* is moved a specific distance inwards (ajacent) while the Z moves the tool
* left (opposite).
*
* RETURNS: Nothing
* XBeginPositionSteps -- Sets with new start position.
*
*/
void
CalculateCompoundPosition(void) {
float32 res1;
int32 stepsX;
int8 OutputBuffer[12];
// Nice little side effect here.
// If Use compound is set ON and also AutoX is set on, we can do the multiple passes
// returning to the same Z and just moving the X axis in.
// Handy for turning down a specific distance.
// The trick is to set the TPI or pitch to very small just like a turning pitch.
if (!fAutoX)
res1 = sqrt((PassDepth*PassDepth) + (PassDistance * PassDistance)); // Calc. Hypotenuse.
else
res1 = PassDepth; // Use PassDepth for turning.
#ifdef MOVE_DIAGNOSTICS
floatToAscii(res1, &OutputBuffer[0],7,4); OutputBuffer[8] = '/0';
#endif
M_DEBUGSTR("Compound move is=%s\n",OutputBuffer);
// Now fake out X axis motion so display of X is really compound distance on LCD.
stepsX = ((res1 *
((float32)GetGlobalVarLong(MOTOR_STEPS_REV_X_NDX) / GetGlobalVarFloat(CROSS_SLIDE_IPITCH_NDX)))+0.5);
if (fExternalThreading) // X moves inwards into cut.
XBeginPositionSteps = XDistanceToSteps(XBeginPosition) - stepsX;
else // X moves outwards into cut.
XBeginPositionSteps = stepsX + XDistanceToSteps(XBeginPosition);
}
/* ------------ Public Functions -------------*/
/*
CalculatePosition :
Determines a new X and Z start position based on the threading parameters and
argument as to which direction to change the parameters.
There are three custom passes.
'First', 'Each + fraction', 'End'.
Before the 'End' distance are n 'Each' passes.
The number of threading passes is determined by subtracting First and End
from the Depth. That result is divided by Each and the remainder added to
Each to create Adjust pass . The numerator of that division minus 1 is the Each pass
count iff > 0.
To cut a thread, there are at least the three custom passes.
For each pass the Z distance is pass * tan(THREAD_ANGLE)
CalculatePosition() uses a state machine to set up each calculation pass.
*/
/*
Following modified by RE 23/12/08
Further modifed 24/12/08
* FUNCTION: CalculatePasses
*
* PARAMETERS:
*
* USES GLOBALS: THREAD_ANGLE_NDX,
* PASS_FIRST_X_DEPTH_NDX
* PASS_EACH_X_DEPTH_NDX
* PASS_END_X_DEPTH_NDX
* PASS_SPRING_CNT_NDX
*
* DESCRIPTION: Sets up the values needed for the CalculatePosition State machine when doing
* multiple passes.
*
* RETURNS: Nothing
* PassCount //Total number of passes to do
* SpringPassCount //Number of spring passes to do
* Adjust_pass //The value of the Adjust pass
* TAN_Angle
* //Plus the value of the First pass in "Pass Depth"
*/
void
CalculatePasses(void) {
float32 totalEachPassDepth, numberOfPasses, fractionOfAPass, AdjustPass ;
int16 passes ; //Additional External integers (EachPasses, AdjustPasses) added earlier and in MovementThread.h
TAN_Angle = GetGlobalVarFloat(THREAD_ANGLE_NDX);
TAN_Angle = tan(TAN_Angle * RADIANS_PER_DEGREE );
// Set up first pass.
PassCount = 0; // No passes to start with.
EachPasses = 0; // Initialise Each Passes
AdjustPasses = 0; // Initialise Adjust Passes
PassDepth = 0.0; // Depth of pass is also zero.
PassDistance = 0.0; // As is the distance to travel in Z
Adjust_pass = 0.0; // No adjustment yet.
RunTimeAdjust = 0.0;
RunTimeLast = 0.0;
M_DEBUGSTR("PassCount=%d, ", PassCount);
// Test if operator has entered in a first pass value.
if (GetGlobalVarFloat(PASS_FIRST_X_DEPTH_NDX) > 0.0) {
PassCount++; // So we have at least one pass.
M_DEBUGSTR("PassCount=%d, ", PassCount);
// Set depth to this and then figure out how far Z moves.
PassDepth = GetGlobalVarFloat(PASS_FIRST_X_DEPTH_NDX);
PassDistance = PassDepth * TAN_Angle;
}
// Test for special case where if First Pass is bigger than Totaldepth we don't do each or last passes.
if ( GetGlobalVarFloat(PASS_FIRST_X_DEPTH_NDX) < GetGlobalVarFloat(DEPTH_X_NDX)) {
// See if user has a last pass value.
if (GetGlobalVarFloat(PASS_END_X_DEPTH_NDX) > 0.0) {
RunTimeLast = GetGlobalVarFloat(PASS_END_X_DEPTH_NDX);
// So we have at least one more pass.
PassCount++; // Yes, so add another pass.
M_DEBUGSTR("PassCount=%d, ", PassCount);
}
// Now we have 2 passes (First and End) so do tests for the intermediate passes
if ( GetGlobalVarFloat(PASS_EACH_X_DEPTH_NDX) > 0.0) {
// Get depth minus custom passes
totalEachPassDepth = GetGlobalVarFloat(DEPTH_X_NDX) -
GetGlobalVarFloat(PASS_FIRST_X_DEPTH_NDX) -
GetGlobalVarFloat(PASS_END_X_DEPTH_NDX);
// Check if total depth is greater than first pass depth + final pass depth.
// If true then we need to do Each passes and maybe an adjust pass
if (totalEachPassDepth > 0.0) {
numberOfPasses = totalEachPassDepth / GetGlobalVarFloat(PASS_EACH_X_DEPTH_NDX);
// Has Whole and possible fractional part
passes = numberOfPasses; // make an integer truncating the fractional part.
fractionOfAPass = numberOfPasses - passes; //Float - Integer = fraction
EachPasses = passes; //Could be zero or greater
// Then determine how much of the fractional part that doesn't fit into an integral distance.
Adjust_pass = fractionOfAPass * GetGlobalVarFloat(PASS_EACH_X_DEPTH_NDX);
RunTimeAdjust = Adjust_pass;
// Test should really be for > zero but this covers for miniscule float division errors?
if (Adjust_pass > 0.0001) {
AdjustPasses = 1;
// Probably getting too clever here but it would be nice if the Last pass
// was always smaller than the previous Adjust pass
if ( GetGlobalVarFloat(PASS_END_X_DEPTH_NDX) > Adjust_pass) {
RunTimeAdjust = GetGlobalVarFloat(PASS_END_X_DEPTH_NDX);
RunTimeLast = Adjust_pass;
}
}
}
// Now we have the exact value for an adjust passs so that the EACH passes plus the adjust pass plus LAST pass add up to the total depth.
PassCount += (AdjustPasses + EachPasses);
}
}
SpringPassCount = GetGlobalVarByte(PASS_SPRING_CNT_NDX);
M_DEBUGSTR("PassCount=%d, SpringPassCount=%d\n", PassCount, SpringPassCount);
}
/*
Following modified by RE 23/12/08 and 24/12/08
I feel that there is a terminology problem here
The "Original" CalculatePosition function appears to actually run First Pass then Adjust Pass, then Each Passes, then Spring
passes. The first "Each" pass was the adjusted pass however it was termed "First Pass" which caused some confusion.
Assuming that the "First Pass" had a value it would fall through the switch and execute
Accordingly the PassStates have been changed slightly to give
FirstPass Not in switch as it is the exit value of CalculatePosition (usually falls straight through)
EachPass (Number as required)
AdjustPass (to nearly complete the required depth)
End pass (to complete the required depth)
Spring Passes (as required)
NOTE all debug information has been removed to make it clearer for me to read!
*/
/*
* FUNCTION: CalculatePosition
*
* PARAMETERS: holdPosition --
*
* USES GLOBALS:
* PassState
* PassCount
* SpringPassCount
* PassDepth
* Adjust_pass
* TAN_Angle
*
* DESCRIPTION: State machine to work through the G76 Passes for threading.
*
* RETURNS: nothing
*
*/
void
CalculatePosition(int8 holdPosition) {
int32 stepsX, stepsZ;
//Note the passes are decremented in MOVE_TO_END
switch (PassState) {
/*
First work out first pass.
*/
case CALCULATE_PASSES :
CalculatePasses();
// PassDepth currently set to FirstPassDepth which will be added to XStart location.
PassState = EACH_PASS; // Next time run the Each pass.
if (PassDepth != 0) {
break; //Falls out of here on First Pass with depth set
}
else { // Oh Ohhh. No First Pass so go do the Each pass right away.
PassState = EACH_PASS;
}
// No break so we should fall through to Each pass.
//Stay here until the cutting passes are done.
case EACH_PASS :
if (EachPasses == 0) { // Cutting passes done, go do Adjust pass.
PassState = ADJUST_PASS;
}
else { // Still cutting so do another pass.
EachPasses --;
PassDepth = PassDepth + GetGlobalVarFloat(PASS_EACH_X_DEPTH_NDX);
break;
}
//Adjustment pass should be less than "Each" pass but may not be needed
case ADJUST_PASS :
PassState = LAST_PASS; //Adjust pass will only be run once if at all
if(AdjustPasses == 0) {
} // Fall through to Last pass
else {
PassDepth += RunTimeAdjust;
break;
}
case LAST_PASS :
PassState = SPRING_PASSES;
if (RunTimeLast == 0.0)
PassCount = 0;
// Fall through to spring passes
else {
PassDepth += RunTimeLast;
break;
}
//Finally stay here while we're doing spring passes.
case SPRING_PASSES :
if (SpringPassCount == 0) {
PassState = FINISHED_PASSES;
}
case FINISHED_PASSES :
break;
} // End of Switch
// Parameters set up s now calculate distance to move.
if (PassState != FINISHED_PASSES) {
#ifdef MOVE_DIAGNOSTICS
floatToAscii(PassDepth, &OutputBuffer[0],7,4); OutputBuffer[8] = '/0';
M_DEBUGSTR("X=%s",OutputBuffer);
#endif
// Calculate X distance in steps remembering to round rather than truncate.
stepsX = ((PassDepth *
((float32)GetGlobalVarLong(MOTOR_STEPS_REV_X_NDX) / GetGlobalVarFloat(CROSS_SLIDE_IPITCH_NDX)))+0.5);
// Z = X Axis[Pass] x tan( Angle )
PassDistance = (PassDepth * TAN_Angle);
// Calculate Z distance in steps remembering to round rather than truncate.
stepsZ = ((PassDistance *
(GlobalVars[MOTOR_STEPS_REV_Z_NDX].f / GlobalVars[LEADSCREW_IPITCH_NDX].f)) + 0.5);
#ifdef MOVE_DIAGNOSTICS
floatToAscii(PassDistance, &OutputBuffer[0],7,4); OutputBuffer[8] = '/0';
M_DEBUGSTR(", Z=%s\n",OutputBuffer);
#endif
if (!holdPosition) { // Don't stay here, calculate new positions.
if (fExternalThreading) // X moves inwards into cut.
XBeginPositionSteps = XDistanceToSteps(XBeginPosition) - stepsX;
else // X moves outwards into cut.
XBeginPositionSteps = stepsX + XDistanceToSteps(XBeginPosition);
if (fMovingRight)
ZBeginPositionSteps = stepsZ + ZDistanceToSteps(ZBeginPosition);
else
ZBeginPositionSteps = ZDistanceToSteps(ZBeginPosition) - stepsZ;
M_DEBUGSTR("ZS=%ld, ZE=%ld, XS=%ld, XR%ld\n",
ZBeginPositionSteps,
ZEndPositionSteps,
XBeginPositionSteps,
XRetractedPositionSteps
);
}
else { // 01DEC08 -- Added so that when USE COMPOUND FOR THREADING IS ON and
// Metric Mode is enabled, the ZBeginPosition is correct for SCREW Cutting.
ZBeginPositionSteps = ZDistanceToSteps(ZBeginPosition);
}
}
}
/*
* FUNCTION: InitMovementThread
*
* PARAMETERS: None
*
* USES GLOBALS: fMetricMode
* THREAD_BEGIN_NDX
* THREAD_END_NDX
* BEGIN_X_NDX
* RETRACTED_X_NDX
* X_DIAMETER_NDX
* Changes Globals
* MovementState
* PassState
* fLOkToStop
* fMTOkToStop
* fMTRdy
* fMTRun
* fMovementThreadMode
* ZBeginPosition
* ZBeginPositionSteps
* XBeginPosition
* XBeginPositionSteps
*
* DESCRIPTION: Initialize all the globals used for Movement and Threading.
*
* RETURNS: Nothing
*
*/
void
InitMovementThread(void) {
MovementState = MOVE_WAIT;
PassState = CALCULATE_PASSES;
fLOkToStop = 1;
fMTOkToStop = 1;
fMTRdy = 1;
fMTRun = 0;
fMovementThreadMode = 0;
ZBeginPosition = (fMetricMode) ? GetGlobalVarFloat(THREAD_BEGIN_NDX) * 25.4 : GetGlobalVarFloat(THREAD_BEGIN_NDX);
ZBeginPositionSteps = ZDistanceToSteps(ZBeginPosition);
ZEndPosition = (fMetricMode) ? GetGlobalVarFloat(THREAD_END_NDX) * 25.4 : GetGlobalVarFloat(THREAD_END_NDX);
ZEndPositionSteps = ZDistanceToSteps(ZEndPosition);
XBeginPosition = (fMetricMode) ? GetGlobalVarFloat(BEGIN_X_NDX) * 25.4 : GetGlobalVarFloat(BEGIN_X_NDX);
XBeginPositionSteps = XDistanceToSteps(XBeginPosition) ;
XRetractedPosition = (fMetricMode) ? GetGlobalVarFloat(RETRACTED_X_NDX) * 25.4 : GetGlobalVarFloat(RETRACTED_X_NDX);
XRetractedPositionSteps = XDistanceToSteps(XRetractedPosition);
SaveGlobalVar(X_DIAMETER_NDX); // Update EEROM menu entry with latest diameter location.
M_DEBUGSTR("ZS=%ld, ZE=%ld, XS=%ld, XR%ld\n",
ZBeginPositionSteps,
ZEndPositionSteps,
XRetractedPositionSteps,
XBeginPositionSteps
);
}
/*
* FUNCTION: MoveHome
*
* PARAMETERS: Motor#, HomeSpeed
*
* USES GLOBALS: MovementState,
* fLOkToStop
* fMTOkToStop
*
* DESCRIPTION: Calls MotorMoveTo with motor# and HomeSpeed
* Wrapper Function for the ALT-nHOME button combination.
*
* RETURNS: Nothing.
*
*/
void
MoveHome(uint8 mtr, WORD spd) {
MotorStop(MOTOR_Z); // Cancel any exising moves.
MotorStop(MOTOR_X); // Especially the X axis which has flags used in non-automatic mode.
MotorMoveTo( mtr, 0, spd, SPINDLE_EITHER );
fLOkToStop = 0; // let it finish.
fMTOkToStop = 0; // Tell system we're Busy
M_DEBUGSTR("Move: MOVE_HOMING\n");
MovementState = MOVE_HOMING;
}
/*
* FUNCTION: MovementStartThread
*
* PARAMETERS: None
*
* USES GLOBALS: MovementState -- New state machine variable
* fMoveStartRQ -- To request motion.
* fXMoveBSY -- Cancel X move request
* CurrentZPosition -- Set with ZMotorPosition.
* ZMotorPosition -- Used for CurrentZPosition
* fMTRdy -- Motor Ready
* fMTRun -- Motor Running.
* fMovementThreadMode --
* fXThreading
*
* DESCRIPTION:
*
* RETURNS:
*
*/
void
MovementStartThread(void) {
if (!fMTRun) {
fMoveStartRQ = 1; // Request motion
// Grab current position
INTCON &= 0x3F;
fXMoveBSY = 0; // X is now in position.
CurrentZPosition = ZMotorPosition;
INTCON |= 0xC0;
// We're no longer ready
fMTRdy = 0; // but instead we're
fMTRun = 1; // running.
// Mode change?
if (fMovementThreadMode != fXThreading) {
// Yes. Start from scratch.
fMovementThreadMode = fXThreading;
MovementState = MOVE_WAIT;
}
}
}
/*
* FUNCTION: MovementStopThread
*
* PARAMETERS: None
*
* USES GLOBALS: fRunMachine
*
* DESCRIPTION: Request that machine stop.
*
* RETURNS: Nothing
*
*/
void
MovementStopThread(void) {
M_DEBUGSTR("Stop Thread RQ\n");
fRunMachine = 0; // Request that we stop.
}
/*
* FUNCTION: MovementThread
*
* PARAMETERS: None
*
* USES GLOBALS: Too many to mention....
*
* DESCRIPTION: State machine thread that walks through
* the steps needed to thread, retract, return and enter
* the material for threading.
* See comments for each state to determine how it works.
*
* RETURNS: Nothing.
*
*/
void
MovementThread(void) {
static uint16 HomeSpeed, XSpeed;
static int32 TargetXPosition, TargetZPosition;
int8 i,p,c;
if ( !fRunMachine && fLOkToStop ) { // Execute this block any time we're stopped in Ready Mode.
if (!fMTRdy) { // Prevent this stuff from running over and over again....
fMTRun = 0; // Say that we are not running any more.
fMTRdy = 1; // Say that we are ready.
fMTOkToStop = 1; // Tell system we're stopped and can go to System Ready state if needed.
}
// Do any other stop initializations.
}
else switch (MovementState) {
/*
MOVE_WAIT --
If tool is in the way of the work first either retract it
automatically or ask user to.
Once that's done (or if it's already done, then check to see if
we're at the start of the thread.
If not, move to the correct Z position otherwise, go to the state
that will move the X into the work.
*/
case MOVE_WAIT :
if (fMoveStartRQ) {
fMoveStartRQ = 0;
fLOkToStop = 1;
// Check if there is anywhere to go
if (ZBeginPositionSteps == ZEndPositionSteps) {
fRunMachine = 0; // Ask machine to stop.
SystemError = MSG_START_EQUALS_END_ERROR;
DisplayModeMenuIndex = SystemError;
break;
}
DisplayModeMenuIndex = MSG_FIND_BEGIN_MODE;
M_DEBUGSTR("Checking for Start Position\n");
// Pull out EEROM'd parameters and convert to steps
HomeSpeed = GetGlobalVarWord(SLEW_RATE_Z_NDX);
// Get variables that are changed inside interrupt routine.
INTCON &= 0x3F;
CurrentXPosition = XMotorRelPosition;
CurrentZPosition = ZMotorPosition;
INTCON |= 0xC0;
// Check if tool in work and if so, move it out.
if (CurrentXPosition != XRetractedPositionSteps) { // Tool in work at the moment.
if (fAutoX) {
// X axis has a stepper which is either direct or through CAN Bus.
// Check where we are and move to retracted position.
XSpeed = GetGlobalVarWord(SLEW_RATE_X_NDX);
ZEndPositionSteps = ZDistanceToSteps(ZEndPosition);
fLOkToStop = 0;
M_DEBUGSTR("Withdrawing Tool Automatically\n");
DisplayModeMenuIndex = MSG_RETRACT_TOOL_MODE;
// Retract tool.
if ((SystemError = MotorMoveTo(MOTOR_X, XRetractedPositionSteps, XSpeed, SPINDLE_EITHER)) != 0) {
fLOkToStop = 1;
fRunMachine = 0; // Ask machine to stop.
MovementState = MOVE_WAIT;
DisplayModeMenuIndex = SystemError;
break;
}
// Tool move started, now wait for it to finish.
M_DEBUGSTR("Move: WAIT_X_OUT\n");
MovementState = MOVE_WAIT_X_OUT;
}
else {
// Ready to move in so ask user to move X out of work.
M_DEBUGSTR("User Withdraw Tool RQ\n");
M_DEBUGSTR("Move: AT_END\n");
MovementState = MOVE_AT_END;
}
}
// We're at the right spot so go insert tool.
else if (CurrentZPosition == ZBeginPositionSteps) {
M_DEBUGSTR("Move: TO_START\n");
MovementState = MOVE_TO_START;
}
// Not at the Z start but Tool is out so go move to Z start location.
else {
M_DEBUGSTR("Move: WAIT_X_OUT\n");
MovementState = MOVE_WAIT_X_OUT;
}
}
break;
/*
User has pressed ALT-XHOME or ALT-ZHOME which sends appropriate motor to
0.000" or 0.00mm
*/
case MOVE_HOMING :
if (!fXMoveBSY && !fZMoveBSY) {
// Move is done or we've hit a limit or ESTOP.
if (SystemError != 0) {
fLOkToStop = 1;
fRunMachine = 0; // Ask machine to stop.
MovementState = MOVE_WAIT;
DisplayModeMenuIndex = SystemError;
break;
}
fLOkToStop = 1;
fRunMachine = 0; // Ask machine to stop.
M_DEBUGSTR("Move: WAIT\n");
MovementState = MOVE_WAIT;
}
break;
/*
Wait for X axis to be automatically removed from work or
for user to press START button again which says he's removed it.
Once it's been removed, then decide which way to
go and then issue move TO_START.
If already at start then go to AT_START
*/
case MOVE_WAIT_X_OUT :
if (!fXMoveBSY) {
// Move is done or we've hit a limit or ESTOP.
if (SystemError != 0) {
fLOkToStop = 1;
fRunMachine = 0; // Ask machine to stop.
MovementState = MOVE_WAIT;
DisplayModeMenuIndex = SystemError;
break;
}
// Check if this is the beginning of a set of passes.
if (PassCount > 0) {
// Then calculate first pass trial.
fLOkToStop = 1;
M_DEBUGSTR("Move: WAIT_END\n");
MovementState = MOVE_WAIT_END;
break;
}
if (ZEndPositionSteps == ZBeginPositionSteps) { // At Headstock
fLOkToStop = 1;
DisplayModeMenuIndex = MSG_FIND_BEGIN_MODE;
M_DEBUGSTR("Move: AT_START\n");
MovementState = MOVE_AT_START;
break;
}
else // Go to start position.
if ((SystemError = MotorMoveTo( MOTOR_Z, ZBeginPositionSteps, HomeSpeed, SPINDLE_EITHER )) != 0) {
fLOkToStop = 1;
fRunMachine = 0; // Ask machine to stop.
MovementState = MOVE_WAIT;
DisplayModeMenuIndex = SystemError;
break;
}
fLOkToStop = 0;
DisplayModeMenuIndex = MSG_FIND_BEGIN_MODE;
M_DEBUGSTR("Move: TO_START\n");
MovementState = MOVE_TO_START;
}
break;
/*
MOVE_TO_START --
Wait for arrival at start position.
*/
case MOVE_TO_START :
if (!fZMoveBSY && !fZMoveRQ) { // move is complete.
fLOkToStop = 1;
// Move is done or we've hit a limit or ESTOP.
if (SystemError != 0) {
fRunMachine = 0; // Ask machine to stop.
MovementState = MOVE_WAIT;
DisplayModeMenuIndex = SystemError;
break;
}
else {
DisplayModeMenuIndex = RUN_MODE;
M_DEBUGSTR("Move: AT_START\n");
MovementState = MOVE_AT_START;
}
}
break;
/*
MOVE_AT_START --
At the start position.
Now issue tracked move_to_end command if total
number of passes aren't complete.
fZMoveRQ set when MotorMoveDistance has everything set up and requests
interrupt routine to start move.
fZMoveRQ cleared when Spindle Index Pulse Occurs
fZMoveBSY set when Spindle Index Pulse Occurs
fZMoveBSY cleared when distance move is complete.
fAutoX set when there is a stepper on the X Axis
fAutoX is clear when LCD dialogs and start button control
each stage of the threading.
*/
case MOVE_AT_START :
// Move is done or we've hit a limit or ESTOP.
if (SystemError != 0) {
fLOkToStop = 1;
fRunMachine = 0; // Ask machine to stop.
MovementState = MOVE_WAIT;
DisplayModeMenuIndex = SystemError;
break;
}
if (fAutoX) {
// X axis has a stepper which is either direct or through CAN Bus.
// So move X into work newDistance = XRetractedPositionSteps - XCurrentPositionSteps;
M_DEBUGSTR("Passes left %2d\n", PassCount);
XSpeed = GetGlobalVarWord(SLEW_RATE_X_NDX);
if ((SystemError = MotorMoveTo( MOTOR_X, XBeginPositionSteps, XSpeed, SPINDLE_EITHER )) != 0) {
fLOkToStop = 1;
fRunMachine = 0; // Ask machine to stop.
MovementState = MOVE_WAIT;
DisplayModeMenuIndex = SystemError;
break;
}
fLOkToStop = 0; // No stop while X axis moving in.
DisplayModeMenuIndex = MSG_INSERT_TOOL_MODE;
M_DEBUGSTR("Moving Tool in\n");
M_DEBUGSTR("Move: WAIT_X_DONE\n");
MovementState = MOVE_WAIT_X_DONE;
}
else {
// Ready to move in so ask user to move X and adjust compound
// Pretend the motor is already there so the run time displays show where an operator should
// put the X axis cross slide.
// Diagnostics also should show start position of X axis.
#ifdef MOVE_DIAGNOSTICS
floatToAscii(XBeginPosition, &OutputBuffer[0],7,3); OutputBuffer[8] = '/0';
M_DEBUGSTR("Steps=%ld, Pos=%s\n",XBeginPositionSteps, OutputBuffer );
#endif
INTCON &= 0x3F;
XMotorRelPosition = XBeginPositionSteps;
fXMoveBSY = 1;
INTCON |= 0xC0;
fLOkToStop = 1;
fRunMachine = 0; // Ask machine to stop.
DisplayModeMenuIndex = MSG_INSERT_TOOL_RQ_MODE;
M_DEBUGSTR("Insert Tool and press START\n");
M_DEBUGSTR("Move: WAIT_START\n");
MovementState = MOVE_WAIT_START;
}
break;
/*
WAIT_X_DONE :
X is being inserted so now wait for it to arrive
*/
case MOVE_WAIT_X_DONE :
if (!fXMoveBSY) {
fLOkToStop = 1; // And allow thread to stop.
// Move is done or we've hit a limit or ESTOP.
if (SystemError != 0) {
MovementState = MOVE_WAIT;
DisplayModeMenuIndex = SystemError;
break;
}
else {
#ifdef TAPERING
c = TaperFlags.Byte;
M_DEBUGSTR("TaperState=%02X \n", c);
/*
if (fMovingRight)
printf((far rom int8 *)"Moving to the right and ");
else
printf((far rom int8 *)"Moving to the left and ");
if (!fExternalThreading)
printf((far rom int8 *)"tapering internally ");
else
printf((far rom int8 *)"tapering externally ");
if (fTaperIn)
printf((far rom int8 *)"with taper smaller at headstock\n");
else
printf((far rom int8 *)"with taper larger at headstock\n");
*/
if (fTapering) {
// Get parity of flags byte.
//
// Calculate parity.
p = 0; c = TaperFlags.Byte;
for (i=0; i<3;i++) {
p += (c & 1);
c >>= 1;
}
if (!(p&1)) {// Even parity means turn X axis around and remove backlash
MotorMoveDistance( MOTOR_X,
0, // 0 Distance, just remove backlash.
XSpeed,
!fMovingOut, // move in opposite direction of previous move
SPINDLE_EITHER,
TRUE
);
fLOkToStop = 0; // Finish move.
// printf((far rom int8 *)" Removing X Backlash\n");
M_DEBUGSTR("Move: MOVE_WAIT_X_BACKLASH_DONE\n");
MovementState = MOVE_WAIT_X_BACKLASH_DONE;
}
else {
DisplayModeMenuIndex = RUN_MODE;
M_DEBUGSTR("Move: WAIT_X_RDY\n");
MovementState = MOVE_WAIT_X_RDY;
}
} // endif Tapering.
else {
DisplayModeMenuIndex = RUN_MODE;
M_DEBUGSTR("Move: WAIT_X_RDY\n");