forked from bdring/FluidNC
-
Notifications
You must be signed in to change notification settings - Fork 6
/
Maslow.cpp
1507 lines (1340 loc) · 49.3 KB
/
Maslow.cpp
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
// Copyright (c) 2014 Maslow CNC. All rights reserved.
// Use of this source code is governed by a GPLv3 license that can be found in the LICENSE file with
// following exception: it may not be used for any reason by MakerMade or anyone with a business or personal connection to MakerMade
#include "Maslow.h"
#include "../Report.h"
#include "../WebUI/WifiConfig.h"
// Maslow specific defines
#define VERSION_NUMBER "0.69"
#define TLEncoderLine 2
#define TREncoderLine 1
#define BLEncoderLine 3
#define BREncoderLine 0
#define motorPWMFreq 2000
#define motorPWMRes 10
#define tlIn1Pin 45
#define tlIn1Channel 0
#define tlIn2Pin 21
#define tlIn2Channel 1
#define tlADCPin 18
#define trIn1Pin 42
#define trIn1Channel 2
#define trIn2Pin 41
#define trIn2Channel 3
#define trADCPin 6
#define blIn1Pin 37
#define blIn1Channel 4
#define blIn2Pin 36
#define blIn2Channel 5
#define blADCPin 8
#define brIn1Pin 9
#define brIn1Channel 6
#define brIn2Pin 3
#define brIn2Channel 7
#define brADCPin 7
#define coolingFanPin 47
#define SERVOFAULT 40
#define ETHERNETLEDPIN 39
#define WIFILED 35
#define REDLED 14
int ENCODER_READ_FREQUENCY_HZ = 1000; //max frequency for polling the encoders
//------------------------------------------------------
//------------------------------------------------------ Main function loops
//------------------------------------------------------
// Initialization function
void Maslow_::begin(void (*sys_rt)()) {
Wire.begin(5, 4, 400000);
I2CMux.begin(TCAADDR, Wire);
axisTL.begin(tlIn1Pin, tlIn2Pin, tlADCPin, TLEncoderLine, tlIn1Channel, tlIn2Channel);
axisTR.begin(trIn1Pin, trIn2Pin, trADCPin, TREncoderLine, trIn1Channel, trIn2Channel);
axisBL.begin(blIn1Pin, blIn2Pin, blADCPin, BLEncoderLine, blIn1Channel, blIn2Channel);
axisBR.begin(brIn1Pin, brIn2Pin, brADCPin, BREncoderLine, brIn1Channel, brIn2Channel);
axisBLHomed = false;
axisBRHomed = false;
axisTRHomed = false;
axisTLHomed = false;
//Recompute the center XY
updateCenterXY();
pinMode(coolingFanPin, OUTPUT);
pinMode(ETHERNETLEDPIN, OUTPUT);
pinMode(WIFILED, OUTPUT);
pinMode(REDLED, OUTPUT);
digitalWrite(ETHERNETLEDPIN, LOW);
pinMode(SERVOFAULT, INPUT);
currentThreshold = 1500;
lastCallToUpdate = millis();
if (error) {
log_error("Maslow failed to initialize - fix errors and restart");
} else
log_info("Starting Maslow Version " << VERSION_NUMBER);
}
// Maslow main loop, everything is processed here
void Maslow_::update() {
if (error) {
static unsigned long timer = millis();
static bool st = true; //This is used to blink the LED
if (millis() - timer > 300) {
stopMotors();
st = !st;
digitalWrite(REDLED, st);
timer = millis();
log_error(errorMessage.c_str());
}
return;
}
if (random(10000) == 0) {
digitalWrite(ETHERNETLEDPIN, LOW);
}
if (random(10000) == 0) {
digitalWrite(ETHERNETLEDPIN, HIGH);
}
blinkIPAddress();
heartBeat();
//Make sure we're running maslow config file
if (!Maslow.using_default_config) {
lastCallToUpdate = millis();
Maslow.updateEncoderPositions(); //We always update encoder positions in any state,
axisTL.update(); //update motor currents and belt speeds like this for now
axisTR.update();
axisBL.update();
axisBR.update();
if (safetyOn)
safety_control();
//quick solution for delay without blocking
if (holding && millis() - holdTimer > holdTime) {
holding = false;
} else if (holding)
return;
//temp test function
if (test) {
test = false;
}
//------------------------ Maslow State Machine
//-------Jog or G-code execution. Maybe need to add more modes here like Hold?
//Jog doesn't work for lack of feedback, HOLD doesn't get automatically called after jog, so IDK what (TODO)
if (sys.state() == State::Jog || sys.state() == State::Cycle) {
Maslow.setTargets(steps_to_mpos(get_axis_motor_steps(0), 0),
steps_to_mpos(get_axis_motor_steps(1), 1),
steps_to_mpos(get_axis_motor_steps(2), 2));
//This allows the z-axis to be moved without the motors being enabled before calibration is run
if (allAxisExtended()) {
Maslow.recomputePID();
}
}
//--------Homing routines
else if (sys.state() == State::Homing) {
home();
} else { //In any other state, keep motors off
if (!test)
Maslow.stopMotors();
}
//If we are in any state other than idle or alarm turn the cooling fan on
if (sys.state() != State::Idle && sys.state() != State::Alarm) {
digitalWrite(coolingFanPin, HIGH); //keep the cooling fan on
}
//If we are doing calibration turn the cooling fan on
else if (calibrationInProgress || extendingALL || retractingTL || retractingTR || retractingBL || retractingBR) {
digitalWrite(coolingFanPin, HIGH); //keep the cooling fan on
} else {
digitalWrite(coolingFanPin, LOW); //Turn the cooling fan off
}
//Check to see if we need to resend the calibration data
checkCalibrationData();
//------------------------ End of Maslow State Machine
//if the update function is not being called enough, stop everything to prevent damage
if (millis() - lastCallToUpdate > 100) {
Maslow.panic();
int elapsedTime = millis() - lastCallToUpdate;
log_error("Emergency stop. Update function not being called enough." << elapsedTime << "ms since last call");
}
}
}
void Maslow_::blinkIPAddress() {
static size_t currentChar = 0;
static int currentBlink = 0;
static unsigned long lastBlinkTime = 0;
static bool inPause = false;
int shortMS = 400;
int longMS = 500;
int pauseMS = 2000;
std::string IP_String = WebUI::wifi_config.getIP();
if (currentChar >= IP_String.length()) {
currentChar = 0;
currentBlink = 0;
lastBlinkTime = 0;
inPause = false;
digitalWrite(WIFILED, LOW);
return;
}
char c = IP_String[currentChar];
if (isdigit(c)) {
int blinkCount = c - '0';
if (currentBlink < blinkCount * 2) {
if (millis() - lastBlinkTime >= shortMS) {
//log_info("Blinking Digit: " << c);
digitalWrite(WIFILED, currentBlink % 2 == 0 ? HIGH : LOW);
currentBlink++;
lastBlinkTime = millis();
}
} else if (!inPause) {
inPause = true;
lastBlinkTime = millis();
} else if (millis() - lastBlinkTime >= pauseMS) {
inPause = false;
currentChar++;
currentBlink = 0;
}
} else if (c == '.') {
if (millis() - lastBlinkTime >= longMS) {
digitalWrite(WIFILED, LOW);
currentChar++;
currentBlink = 0;
} else {
digitalWrite(WIFILED, HIGH);
}
}
}
void Maslow_::heartBeat(){
static unsigned long heartBeatTimer = millis();
if(millis() - heartBeatTimer > 1000){
heartBeatTimer = millis();
log_info("Heartbeat");
}
}
// -Maslow homing loop
void Maslow_::home() {
//run all the retract functions untill we hit the current limit
if (retractingTL) {
if (axisTL.retract()) {
retractingTL = false;
axis_homed[0] = true;
extendedTL = false;
}
}
if (retractingTR) {
if (axisTR.retract()) {
retractingTR = false;
axis_homed[1] = true;
extendedTR = false;
}
}
if (retractingBL) {
if (axisBL.retract()) {
retractingBL = false;
axis_homed[2] = true;
extendedBL = false;
}
}
if (retractingBR) {
if (axisBR.retract()) {
retractingBR = false;
axis_homed[3] = true;
extendedBR = false;
}
}
// $EXT - extend mode
if (extendingALL) {
//decompress belts for the first half second
if (millis() - extendCallTimer < 700) {
if (millis() - extendCallTimer > 0)
axisBR.decompressBelt();
if (millis() - extendCallTimer > 150)
axisBL.decompressBelt();
if (millis() - extendCallTimer > 250)
axisTR.decompressBelt();
if (millis() - extendCallTimer > 350)
axisTL.decompressBelt();
}
//then make all the belts comply until they are extended fully, or user terminates it
else {
if (!extendedTL)
extendedTL = axisTL.extend(computeTL(0, 0, 0));
if (!extendedTR)
extendedTR = axisTR.extend(computeTR(0, 0, 0));
if (!extendedBL)
extendedBL = axisBL.extend(computeBL(0, 300, 0));
if (!extendedBR)
extendedBR = axisBR.extend(computeBR(0, 300, 0));
if (extendedTL && extendedTR && extendedBL && extendedBR) {
extendingALL = false;
log_info("All belts extended to center position");
}
}
}
// $CMP - comply mode
if (complyALL) {
//decompress belts for the first half second
if (millis() - complyCallTimer < 40) {
axisBR.decompressBelt();
axisBL.decompressBelt();
axisTR.decompressBelt();
axisTL.decompressBelt();
} else if(millis() - complyCallTimer < 800){
axisTL.comply();
axisTR.comply();
axisBL.comply();
axisBR.comply();
}
else {
complyALL = false;
}
}
// $CAL - calibration mode
if (calibrationInProgress) {
calibration_loop();
}
// Runs the take slack sequence
if(takeSlack){
if (takeSlackFunc()) {
takeSlack = false;
}
}
handleMotorOverides();
//if we are done with all the homing moves, switch system state back to Idle?
if (!retractingTL && !retractingBL && !retractingBR && !retractingTR && !extendingALL && !complyALL && !calibrationInProgress &&
!takeSlack && !checkOverides()) {
sys.set_state(State::Idle);
}
}
//Moves to 0,0 takes a measurement
bool Maslow_::takeSlackFunc() {
static int takeSlackState = 0; //0 -> Starting, 1-> Moving to (0,0), 2-> Taking a measurement
static unsigned long holdTimer = millis();
static float startingX = 0;
static float startingY = 0;
//Initialize
if (takeSlackState == 0) {
takeSlackState = 1;
startingX = getTargetX();
startingY = getTargetY();
}
//Move to (0,0)
if(takeSlackState == 1){
if (move_with_slack(startingX, startingY, 0, 0)) {
takeSlackState = 2;
}
}
//Take a measurement
if(takeSlackState == 2){
if (take_measurement_avg_with_check(0, UP)) {
double offset = _beltEndExtension + _armLength;
double threshold = 15;
float diffTL = calibration_data[0][0] - offset - computeTL(0, 0, 0);
float diffTR = calibration_data[1][0] - offset - computeTR(0, 0, 0);
float diffBL = calibration_data[2][0] - offset - computeBL(0, 0, 0);
float diffBR = calibration_data[3][0] - offset - computeBR(0, 0, 0);
log_info("Center point deviation: TL: " << diffTL << " TR: " << diffTR << " BL: " << diffBL << " BR: " << diffBR);
if (abs(diffTL) > threshold || abs(diffTR) > threshold || abs(diffBL) > threshold || abs(diffBR) > threshold) {
log_error("Center point deviation over " << threshold << "mmm, your coordinate system is not accurate, maybe try running calibration again?");
//Should we enter an alarm state here to prevent things from going wrong?
//Reset
takeSlackState = 0;
return true;
}
else{
log_info("Center point deviation within " << threshold << "mm, your coordinate system is accurate");
takeSlackState = 3;
holdTimer = millis();
}
}
}
//Position hold for 2 seconds
if(takeSlackState == 3){
if(millis() - holdTimer > 2000){
takeSlackState = 0;
return true;
}
}
return false;
}
// --Maslow calibration loop
void Maslow_::calibration_loop() {
static int waypoint = 0;
static int direction = UP;
static bool measurementInProgress = false;
//Taking measurment once we've reached the point
if (measurementInProgress) {
if (take_measurement_avg_with_check(waypoint, direction)) { //Takes a measurement and returns true if it's done
measurementInProgress = false;
waypoint++; //Increment the waypoint counter
if (waypoint > pointCount - 1) { //If we have reached the end of the calibration process
calibrationInProgress = false;
waypoint = 0;
log_info("Calibration complete");
print_calibration_data();
calibrationDataWaiting = millis();
sys.set_state(State::Idle);
} else {
hold(250);
}
}
}
//Move to the next point in the grid
else {
if (move_with_slack(calibrationGrid[waypoint - 1][0],
calibrationGrid[waypoint - 1][1],
calibrationGrid[waypoint][0],
calibrationGrid[waypoint][1])) {
measurementInProgress = true;
direction = get_direction(calibrationGrid[waypoint - 1][0],
calibrationGrid[waypoint - 1][1],
calibrationGrid[waypoint][0],
calibrationGrid[waypoint][1]);
x = calibrationGrid[waypoint][0];
y = calibrationGrid[waypoint][1];
hold(250);
}
}
}
//------------------------------------------------------
//------------------------------------------------------ Core utility functions
//------------------------------------------------------
//updating encoder positions for all 4 arms, cycling through them each call, at ENCODER_READ_FREQUENCY_HZ frequency
bool Maslow_::updateEncoderPositions() {
bool success = true;
static unsigned long lastCallToEncoderRead = millis();
static int encoderFailCounter[4] = { 0, 0, 0, 0 };
static unsigned long encoderFailTimer = millis();
if (!readingFromSD && (millis() - lastCallToEncoderRead > 1000 / (ENCODER_READ_FREQUENCY_HZ))) {
static int encoderToRead = 0;
switch (encoderToRead) {
case 0:
if (!axisTL.updateEncoderPosition()) {
encoderFailCounter[TLEncoderLine]++;
}
break;
case 1:
if (!axisTR.updateEncoderPosition()) {
encoderFailCounter[TREncoderLine]++;
}
break;
case 2:
if (!axisBL.updateEncoderPosition()) {
encoderFailCounter[BLEncoderLine]++;
}
break;
case 3:
if (!axisBR.updateEncoderPosition()) {
encoderFailCounter[BREncoderLine]++;
}
break;
}
encoderToRead++;
if (encoderToRead > 3) {
encoderToRead = 0;
lastCallToEncoderRead = millis();
}
}
// if more than 1% of readings fail, warn user, if more than 10% fail, stop the machine and raise alarm
if (millis() - encoderFailTimer > 1000) {
for (int i = 0; i < 4; i++) {
//turn i into proper label
String label = axis_id_to_label(i);
if (encoderFailCounter[i] > 0.1 * ENCODER_READ_FREQUENCY_HZ) {
// log error statement with appropriate label
log_error("Failure on " << label.c_str() << " encoder, failed to read " << encoderFailCounter[i]
<< " times in the last second");
Maslow.panic();
} else if (encoderFailCounter[i] > 0) { //0.01*ENCODER_READ_FREQUENCY_HZ){
log_warn("Bad connection on " << label.c_str() << " encoder, failed to read " << encoderFailCounter[i]
<< " times in the last second");
}
encoderFailCounter[i] = 0;
encoderFailTimer = millis();
}
}
return success;
}
// This computes the target lengths of the belts based on the target x and y coordinates and sends that information to each arm.
void Maslow_::setTargets(float xTarget, float yTarget, float zTarget, bool tl, bool tr, bool bl, bool br) {
//Store the target x and y coordinates for the getTargetN() functions
targetX = xTarget;
targetY = yTarget;
targetZ = zTarget;
if (tl) {
axisTL.setTarget(computeTL(xTarget, yTarget, zTarget));
}
if (tr) {
axisTR.setTarget(computeTR(xTarget, yTarget, zTarget));
}
if (bl) {
axisBL.setTarget(computeBL(xTarget, yTarget, zTarget));
}
if (br) {
axisBR.setTarget(computeBR(xTarget, yTarget, zTarget));
}
}
//updates motor powers for all axis, based on targets set by setTargets()
void Maslow_::recomputePID() {
axisBL.recomputePID();
axisBR.recomputePID();
axisTR.recomputePID();
axisTL.recomputePID();
digitalWrite(coolingFanPin, HIGH); //keep the cooling fan on
if (digitalRead(SERVOFAULT) ==
1) { //The servo drives have a fault pin that goes high when there is a fault (ie one over heats). We should probably call panic here. Also this should probably be read in the main loop
log_info("Servo fault!");
}
}
//This is the function that should prevent machine from damaging itself
void Maslow_::safety_control() {
//We need to keep track of average belt speeds and motor currents for every axis
static bool tick[4] = { false, false, false, false };
static unsigned long spamTimer = millis();
static int tresholdHitsBeforePanic = 15;
static int panicCounter[4] = { 0 };
MotorUnit* axis[4] = { &axisTL, &axisTR, &axisBL, &axisBR };
for (int i = 0; i < 4; i++) {
//If the current exceeds some absolute value, we need to call panic() and stop the machine
if (axis[i]->getMotorCurrent() > 4000 && !tick[i]) {
panicCounter[i]++;
if (panicCounter[i] > tresholdHitsBeforePanic) {
log_error("Motor current on " << axis_id_to_label(i).c_str() << " axis exceeded threshold of " << 4000);
if(!calibrationInProgress){
Maslow.panic();
}
tick[i] = true;
}
} else {
panicCounter[i] = 0;
}
//If the motor torque is high, but the belt is not moving
// if motor is moving IN, this means the axis is STALL, we should warn the user and lower torque to the motor
// if the motor is moving OUT, that means the axis has SLACK, so we should warn the user and stop the motor, until the belt starts moving again
// don't spam log, no more than once every 5 seconds
static int axisSlackCounter[4] = { 0, 0, 0, 0 };
axisSlackCounter[i] = 0; //TEMP
if (axis[i]->getMotorPower() > 450 && abs(axis[i]->getBeltSpeed()) < 0.1 && !tick[i]) {
axisSlackCounter[i]++;
if (axisSlackCounter[i] > 3000) {
// log_info("SLACK:" << axis_id_to_label(i).c_str() << " motor power is " << int(axis[i]->getMotorPower())
// << ", but the belt speed is" << axis[i]->getBeltSpeed());
// log_info(axisSlackCounter[i]);
// log_info("Pull on " << axis_id_to_label(i).c_str() << " and restart!");
tick[i] = true;
axisSlackCounter[i] = 0;
Maslow.panic();
}
} else
axisSlackCounter[i] = 0;
//If the motor has a position error greater than 1mm and we are running a file or jogging
if ((abs(axis[i]->getPositionError()) > 1) && (sys.state() == State::Jog || sys.state() == State::Cycle) && !tick[i]) {
// log_error("Position error on " << axis_id_to_label(i).c_str() << " axis exceeded 1mm, error is " << axis[i]->getPositionError()
// << "mm");
tick[i] = true;
}
if ((abs(axis[i]->getPositionError()) > 15) && (sys.state() == State::Cycle)) {
log_error("Position error on " << axis_id_to_label(i).c_str() << " axis exceeded 15mm while running. Halting. Error is "
<< axis[i]->getPositionError() << "mm");
Maslow.eStop();
}
}
if (millis() - spamTimer > 5000) {
for (int i = 0; i < 4; i++) {
tick[i] = false;
}
spamTimer = millis();
}
}
// Compute target belt lengths based on X-Y-Z coordinates
float Maslow_::computeBL(float x, float y, float z) {
//Move from lower left corner coordinates to centered coordinates
x = x + centerX;
y = y + centerY;
float a = blX - x;
float b = blY - y;
float c = 0.0 - (z + blZ);
float length = sqrt(a * a + b * b + c * c) - (_beltEndExtension + _armLength);
return length; //+ lowerBeltsExtra;
}
float Maslow_::computeBR(float x, float y, float z) {
//Move from lower left corner coordinates to centered coordinates
x = x + centerX;
y = y + centerY;
float a = brX - x;
float b = brY - y;
float c = 0.0 - (z + brZ);
float length = sqrt(a * a + b * b + c * c) - (_beltEndExtension + _armLength);
return length; //+ lowerBeltsExtra;
}
float Maslow_::computeTR(float x, float y, float z) {
//Move from lower left corner coordinates to centered coordinates
x = x + centerX;
y = y + centerY;
float a = trX - x;
float b = trY - y;
float c = 0.0 - (z + trZ);
return sqrt(a * a + b * b + c * c) - (_beltEndExtension + _armLength);
}
float Maslow_::computeTL(float x, float y, float z) {
//Move from lower left corner coordinates to centered coordinates
x = x + centerX;
y = y + centerY;
float a = tlX - x;
float b = tlY - y;
float c = 0.0 - (z + tlZ);
return sqrt(a * a + b * b + c * c) - (_beltEndExtension + _armLength);
}
//------------------------------------------------------
//------------------------------------------------------ Homing and calibration functions
//------------------------------------------------------
// Takes one measurement; returns true when it's done. Waypoint # is used to st
bool Maslow_::take_measurement(int waypoint, int dir, int run) {
//Shouldn't this be handled with the same code as below but with the direction set to UP?
if (orientation == VERTICAL) {
//first we pull two bottom belts tight one after another, if x<0 we pull left belt first, if x>0 we pull right belt first
static bool BL_tight = false;
static bool BR_tight = false;
axisTL.recomputePID();
axisTR.recomputePID();
//On the left side of the sheet we want to pull the left belt tight first
if (x < 0) {
if (!BL_tight) {
if (axisBL.pull_tight(calibrationCurrentThreshold)) {
BL_tight = true;
//log_info("Pulled BL tight");
}
return false;
}
if (!BR_tight) {
if (axisBR.pull_tight(calibrationCurrentThreshold)) {
BR_tight = true;
//log_info("Pulled BR tight");
}
return false;
}
}
//On the right side of the sheet we want to pull the right belt tight first
else {
if (!BR_tight) {
if (axisBR.pull_tight(calibrationCurrentThreshold)) {
BR_tight = true;
//log_info("Pulled BR tight");
}
return false;
}
if (!BL_tight) {
if (axisBL.pull_tight(calibrationCurrentThreshold)) {
BL_tight = true;
//log_info("Pulled BL tight");
}
return false;
}
}
//once both belts are pulled, take a measurement
if (BR_tight && BL_tight) {
//take measurement and record it to the calibration data array
calibration_data[0][waypoint] = axisTL.getPosition() + _beltEndExtension + _armLength;
calibration_data[1][waypoint] = axisTR.getPosition() + _beltEndExtension + _armLength;
calibration_data[2][waypoint] = axisBL.getPosition() + _beltEndExtension + _armLength;
calibration_data[3][waypoint] = axisBR.getPosition() + _beltEndExtension + _armLength;
BR_tight = false;
BL_tight = false;
return true;
}
return false;
}
// in HoRIZONTAL orientation we pull on the belts depending on the direction of the last move. This is important because the other two belts are likely slack
else if (orientation == HORIZONTAL) {
static MotorUnit* pullAxis1;
static MotorUnit* pullAxis2;
static MotorUnit* holdAxis1;
static MotorUnit* holdAxis2;
static bool pull1_tight = false;
static bool pull2_tight = false;
switch (dir) {
case UP:
holdAxis1 = &axisTL;
holdAxis2 = &axisTR;
if (x < 0) {
pullAxis1 = &axisBL;
pullAxis2 = &axisBR;
} else {
pullAxis1 = &axisBR;
pullAxis2 = &axisBL;
}
break;
case DOWN:
holdAxis1 = &axisBL;
holdAxis2 = &axisBR;
if (x < 0) {
pullAxis1 = &axisTL;
pullAxis2 = &axisTR;
} else {
pullAxis1 = &axisTR;
pullAxis2 = &axisTL;
}
break;
case LEFT:
holdAxis1 = &axisTL;
holdAxis2 = &axisBL;
if (y < 0) {
pullAxis1 = &axisBR;
pullAxis2 = &axisTR;
} else {
pullAxis1 = &axisTR;
pullAxis2 = &axisBR;
}
break;
case RIGHT:
holdAxis1 = &axisTR;
holdAxis2 = &axisBR;
if (y < 0) {
pullAxis1 = &axisBL;
pullAxis2 = &axisTL;
} else {
pullAxis1 = &axisTL;
pullAxis2 = &axisBL;
}
break;
}
holdAxis1->recomputePID();
holdAxis2->recomputePID();
if (!pull1_tight) {
if (pullAxis1->pull_tight(calibrationCurrentThreshold)) {
pull1_tight = true;
}
if (run == 0)
pullAxis2->comply();
return false;
}
if (!pull2_tight) {
if (pullAxis2->pull_tight(calibrationCurrentThreshold)) {
pull2_tight = true;
}
return false;
}
if (pull1_tight && pull2_tight) {
//take measurement and record it to the calibration data array
calibration_data[0][waypoint] = axisTL.getPosition() + _beltEndExtension + _armLength;
calibration_data[1][waypoint] = axisTR.getPosition() + _beltEndExtension + _armLength;
calibration_data[2][waypoint] = axisBL.getPosition() + _beltEndExtension + _armLength;
calibration_data[3][waypoint] = axisBR.getPosition() + _beltEndExtension + _armLength;
pull1_tight = false;
pull2_tight = false;
return true;
}
}
return false;
}
// Takes a series of measurements, calculates average and records calibration data; returns true when it's done
bool Maslow_::take_measurement_avg_with_check(int waypoint, int dir) {
//take 5 measurements in a row, (ignoring the first one), if they are all within 1mm of each other, take the average and record it to the calibration data array
static int run = 0;
static double measurements[4][4] = { 0 };
static double avg = 0;
static double sum = 0;
if (take_measurement(waypoint, dir, run)) {
if (run < 2) {
run++;
return false; //discard the first three measurements
}
measurements[0][run - 2] = calibration_data[0][waypoint]; //-3 cuz discarding the first 3 measurements
measurements[1][run - 2] = calibration_data[1][waypoint];
measurements[2][run - 2] = calibration_data[2][waypoint];
measurements[3][run - 2] = calibration_data[3][waypoint];
run++;
static int criticalCounter = 0;
if (run > 5) {
run = 0;
//check if all measurements are within 1mm of each other
double maxDeviation[4] = { 0 };
double maxDeviationAbs = 0;
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 3; j++) {
//find max deviation between measurements
maxDeviation[i] = max(maxDeviation[i], abs(measurements[i][j] - measurements[i][j + 1]));
}
}
for (int i = 0; i < 4; i++) {
maxDeviationAbs = max(maxDeviationAbs, maxDeviation[i]);
}
if (maxDeviationAbs > 2.5) {
log_error("Measurement error, measurements are not within 2.5 mm of each other, trying again");
log_info("Max deviation: " << maxDeviationAbs);
//print all the measurements in readable form:
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
//use axis id to label:
log_info(axis_id_to_label(i).c_str() << " " << measurements[i][j]);
}
}
//reset the run counter to run the measurements again
if (criticalCounter++ > 8) { //This updates the counter and checks
log_error("Critical error, measurements are not within 1.5mm of each other 8 times in a row, stopping calibration");
calibrationInProgress = false;
waypoint = 0;
criticalCounter = 0;
return false;
}
return false;
}
//if they are, take the average and record it to the calibration data array
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
sum += measurements[i][j];
}
avg = sum / 4;
calibration_data[i][waypoint] = avg;
sum = 0;
criticalCounter = 0;
}
log_info("Measured waypoint " << waypoint);
return true;
}
}
return false;
}
// Move pulling just two belts depending in the direction of the movement
bool Maslow_::move_with_slack(double fromX, double fromY, double toX, double toY) {
//This is where we want to introduce some slack so the system
static unsigned long moveBeginTimer = millis();
static bool decompress = true;
const float stepSize = 0.04;
static int direction = UP;
//We only want to decompress at the beginning of each move
if (decompress) {
moveBeginTimer = millis();
decompress = false;
direction = get_direction(fromX, fromY, toX, toY);
}
//Decompress belts for 500ms...this happens by returning right away before running any of the rest of the code
if (millis() - moveBeginTimer < 750) {
if (orientation == VERTICAL) {
axisTL.recomputePID();
axisTR.recomputePID();
axisBL.decompressBelt();
axisBR.decompressBelt();
} else {
switch (direction) {
case UP:
axisBL.decompressBelt();
axisBR.decompressBelt();
break;
case DOWN:
axisTL.decompressBelt();
axisTR.decompressBelt();
break;
case LEFT:
axisTR.decompressBelt();
axisBR.decompressBelt();
break;
case RIGHT:
axisTL.decompressBelt();
axisBL.decompressBelt();
break;
}
}
return false;
}
//Stop for 50ms
//we need to stop motors after decompression was finished once
else if (millis() - moveBeginTimer < 800) {
stopMotors();
return false;
}
if(orientation == VERTICAL){
axisTL.recomputePID();
axisTR.recomputePID();
axisBL.comply();
axisBR.comply();
}
else{
switch (direction) {
case UP:
axisTL.recomputePID();
axisTR.recomputePID();
axisBL.comply();
axisBR.comply();
break;
case DOWN:
axisTL.comply();
axisTR.comply();
axisBL.recomputePID();
axisBR.recomputePID();
break;
case LEFT:
axisTL.recomputePID();
axisTR.comply();
axisBL.recomputePID();
axisBR.comply();
break;
case RIGHT:
axisTL.comply();
axisTR.recomputePID();
axisBL.comply();
axisBR.recomputePID();
break;
}
}
//This system of setting the final target and then waiting until we get there doesn't feel good to me
switch (direction) {
case UP:
setTargets(toX, getTargetY() + stepSize, 0);
if (getTargetY() > toY) {
stopMotors();
reset_all_axis();
decompress = true; //Reset for the next pass
return true;
}
break;
case DOWN:
setTargets(toX, getTargetY() - stepSize, 0);
if (getTargetY() < toY) {
stopMotors();
reset_all_axis();
decompress = true; //Reset for the next pass