···11-<svg xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" id="svg" width="400" height="41.058" viewBox="0 0 400 41.058"><style>:root{--borderColor-muted:#363A4F}@media (prefers-color-scheme:light){:root{--borderColor-muted:#CCD0DA}}</style><g id="svgg"><path id="path0" fill="var(--borderColor-muted)" fill-rule="evenodd" stroke="none" d="M360.790 1.671 C 360.900 1.699,361.079 1.699,361.189 1.671 C 361.298 1.642,361.208 1.619,360.989 1.619 C 360.771 1.619,360.681 1.642,360.790 1.671 M355.242 3.554 L 354.564 4.265 355.274 3.586 C 355.665 3.213,355.985 2.893,355.985 2.875 C 355.985 2.791,355.873 2.894,355.242 3.554 M378.846 8.587 C 378.569 8.868,378.369 9.099,378.400 9.099 C 378.431 9.099,378.683 8.868,378.959 8.587 C 379.236 8.305,379.436 8.075,379.405 8.075 C 379.373 8.075,379.122 8.305,378.846 8.587 M367.183 10.719 L 366.733 11.203 367.216 10.753 C 367.666 10.335,367.752 10.236,367.666 10.236 C 367.647 10.236,367.430 10.454,367.183 10.719 M351.747 11.118 C 351.011 11.540,350.225 12.032,350.002 12.211 L 349.596 12.537 350.032 12.990 C 350.981 13.975,352.792 15.208,354.259 15.866 C 354.927 16.165,354.962 16.201,354.962 16.580 L 354.962 16.979 353.568 16.911 C 351.318 16.800,349.088 16.039,347.531 14.851 C 346.902 14.371,347.009 14.385,346.311 14.686 L 345.749 14.927 345.749 15.862 L 345.749 16.798 343.958 16.873 C 342.973 16.914,278.985 16.947,201.762 16.946 L 61.357 16.944 61.050 16.558 C 60.844 16.301,60.354 15.985,59.572 15.606 C 57.925 14.809,57.974 14.816,57.407 15.276 C 55.993 16.425,53.576 17.255,51.322 17.366 L 49.929 17.434 49.929 17.035 C 49.929 16.656,49.964 16.620,50.632 16.321 C 52.097 15.663,53.910 14.430,54.856 13.447 L 55.291 12.996 54.890 12.668 C 54.669 12.488,53.870 11.987,53.115 11.556 L 51.741 10.771 50.732 11.847 C 49.009 13.682,46.936 15.200,44.527 16.388 C 42.414 17.429,42.646 17.401,36.208 17.401 L 30.610 17.401 30.347 16.875 C 29.241 14.661,25.552 14.624,24.855 16.820 C 24.701 17.306,21.655 16.338,19.995 15.277 L 19.461 14.936 17.948 15.686 C 15.683 16.808,12.338 17.856,11.020 17.856 C 10.720 17.856,10.691 17.891,10.691 18.252 C 10.691 18.689,10.705 18.696,11.797 18.820 C 13.073 18.964,15.879 19.944,18.222 21.064 L 19.441 21.646 20.060 21.236 C 20.810 20.738,21.633 20.388,23.170 19.915 C 23.809 19.719,24.400 19.531,24.484 19.499 C 24.577 19.464,24.777 19.657,24.997 19.996 C 26.298 22.004,29.356 21.857,30.351 19.740 L 30.595 19.221 36.342 19.221 C 41.839 19.221,42.119 19.231,42.782 19.455 C 45.342 20.317,49.559 23.216,51.214 25.251 L 51.675 25.817 52.309 25.463 C 53.518 24.788,54.650 24.095,54.979 23.830 L 55.309 23.563 54.592 22.899 C 53.331 21.731,51.733 20.675,50.470 20.174 C 49.975 19.978,49.930 19.928,49.929 19.576 L 49.929 19.191 51.436 19.249 C 53.519 19.328,55.410 19.958,57.219 21.175 L 57.969 21.679 58.812 21.319 C 60.098 20.771,60.976 20.238,61.177 19.884 L 61.359 19.564 202.525 19.563 L 343.691 19.562 344.221 19.920 C 344.513 20.117,345.237 20.492,345.830 20.753 L 346.908 21.227 347.799 20.641 C 349.588 19.464,351.806 18.771,353.796 18.768 L 354.962 18.766 354.961 19.136 C 354.961 19.471,354.910 19.525,354.421 19.719 C 353.724 19.996,352.333 20.797,351.595 21.348 C 350.984 21.804,349.616 23.037,349.616 23.132 C 349.616 23.223,351.426 24.383,352.356 24.889 L 353.219 25.358 353.678 24.794 C 355.332 22.761,359.550 19.862,362.108 19.000 C 362.772 18.776,363.052 18.766,368.549 18.766 L 374.295 18.766 374.541 19.290 C 375.528 21.388,378.596 21.545,379.880 19.562 C 380.199 19.070,380.264 19.025,380.535 19.104 C 383.054 19.842,383.936 20.187,384.831 20.781 L 385.449 21.191 386.668 20.609 C 389.069 19.462,391.773 18.524,393.126 18.370 C 394.164 18.251,394.200 18.232,394.200 17.792 C 394.200 17.437,394.169 17.401,393.871 17.401 C 392.553 17.401,389.207 16.353,386.943 15.231 L 385.429 14.481 384.895 14.822 C 383.236 15.883,380.190 16.851,380.036 16.365 C 379.339 14.169,375.649 14.206,374.543 16.420 L 374.280 16.946 368.682 16.946 C 362.245 16.946,362.477 16.975,360.364 15.933 C 357.963 14.749,355.881 13.227,354.175 11.409 C 353.628 10.826,353.159 10.350,353.133 10.350 C 353.106 10.350,352.483 10.696,351.747 11.118 M366.307 21.576 C 366.447 21.603,366.678 21.603,366.818 21.576 C 366.959 21.549,366.844 21.527,366.562 21.527 C 366.281 21.527,366.166 21.549,366.307 21.576 M343.588 25.192 C 343.800 25.411,344.000 25.590,344.031 25.590 C 344.062 25.590,343.914 25.411,343.702 25.192 C 343.490 24.973,343.291 24.794,343.259 24.794 C 343.228 24.794,343.376 24.973,343.588 25.192 M340.404 24.939 C 340.404 24.956,340.800 25.353,341.285 25.820 L 342.167 26.670 341.316 25.789 C 340.527 24.970,340.404 24.856,340.404 24.939 M362.440 25.102 C 362.580 25.129,362.811 25.129,362.951 25.102 C 363.092 25.074,362.977 25.052,362.695 25.052 C 362.414 25.052,362.299 25.074,362.440 25.102 M372.931 26.443 C 373.271 26.787,373.575 27.069,373.606 27.069 C 373.637 27.069,373.385 26.787,373.045 26.443 C 372.705 26.099,372.402 25.817,372.371 25.817 C 372.339 25.817,372.592 26.099,372.931 26.443 M368.977 29.229 C 368.977 29.448,369.001 29.538,369.029 29.428 C 369.058 29.319,369.058 29.140,369.029 29.030 C 369.001 28.921,368.977 29.011,368.977 29.229 M367.069 33.807 L 366.619 34.291 367.103 33.841 C 367.552 33.423,367.638 33.324,367.552 33.324 C 367.534 33.324,367.316 33.541,367.069 33.807 M360.904 36.132 C 361.014 36.160,361.193 36.160,361.302 36.132 C 361.412 36.103,361.322 36.080,361.103 36.080 C 360.884 36.080,360.795 36.103,360.904 36.132"/></g></svg>
-1
.github/images/line-break.svg
···11-<svg xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" id="svg" width="400" height="51.636" version="1.1" viewBox="0 0 400 51.636"><style>:root{--borderColor-muted:#363A4F}@media (prefers-color-scheme:light){:root{--borderColor-muted:#CCD0DA}}</style><g id="svgg"><path id="path0" fill="var(--borderColor-muted)" fill-rule="evenodd" stroke="none" d="M184.385 14.791 C 180.462 15.819,178.353 19.258,179.221 23.212 C 179.600 24.943,179.746 24.668,177.916 25.674 C 169.922 30.069,162.339 31.816,156.488 30.613 C 147.807 28.826,145.354 20.411,152.485 16.874 C 157.799 14.238,163.973 15.682,166.247 20.092 C 166.445 20.475,166.616 20.807,166.628 20.830 C 166.735 21.034,167.515 20.551,167.448 20.322 C 167.096 19.127,165.169 16.888,163.812 16.099 C 157.062 12.172,146.729 16.511,147.333 23.019 C 147.997 30.160,153.419 33.997,162.848 33.998 C 168.870 33.998,173.658 32.712,179.548 29.510 C 181.114 28.659,181.261 28.602,181.375 28.807 C 181.601 29.216,182.839 30.449,183.612 31.037 C 188.637 34.860,198.240 35.126,206.887 31.682 L 208.258 31.135 209.130 31.499 C 214.746 33.837,222.238 29.431,221.792 24.053 L 221.738 23.398 223.324 22.500 C 231.763 17.720,239.367 16.192,246.083 17.927 C 253.002 19.715,255.081 25.567,250.123 29.297 C 247.213 31.487,242.422 31.809,240.322 29.957 C 240.108 29.768,239.515 30.185,239.515 30.524 C 239.515 31.249,243.233 32.351,245.091 32.175 C 253.916 31.342,256.758 22.845,249.818 18.039 C 242.925 13.265,232.014 13.637,221.539 19.001 L 219.988 19.796 219.479 19.306 C 214.437 14.455,203.974 13.301,195.067 16.613 L 194.074 16.983 193.446 16.551 C 190.916 14.810,187.123 14.074,184.385 14.791 M189.364 15.753 C 190.446 16.042,191.426 16.453,192.328 16.997 L 192.959 17.378 191.964 17.822 C 189.547 18.903,187.339 20.101,182.367 23.031 C 181.435 23.579,180.608 24.053,180.530 24.083 C 180.312 24.167,180.000 22.596,180.000 21.413 C 180.000 17.066,184.377 14.420,189.364 15.753 M213.095 17.674 C 215.299 18.136,217.497 19.052,218.475 19.917 L 218.912 20.303 215.183 22.503 C 211.797 24.500,208.056 26.565,206.275 27.420 L 205.589 27.748 202.710 24.709 C 201.126 23.037,199.294 21.182,198.639 20.587 L 197.449 19.505 197.847 19.339 C 202.188 17.525,208.894 16.793,213.095 17.674 M198.280 21.690 C 199.216 22.530,204.662 28.231,204.588 28.295 C 204.565 28.315,204.082 28.500,203.515 28.705 C 194.788 31.866,187.238 31.642,182.986 28.096 L 182.639 27.807 184.562 26.648 C 185.620 26.011,187.630 24.792,189.030 23.939 C 190.430 23.087,192.664 21.855,193.994 21.202 L 196.412 20.015 196.903 20.455 C 197.173 20.697,197.793 21.253,198.280 21.690 M220.879 24.347 C 220.993 24.781,220.711 26.007,220.303 26.857 C 218.792 29.999,213.426 32.082,210.020 30.849 L 209.433 30.637 210.747 29.944 C 212.733 28.898,214.798 27.707,217.879 25.831 C 220.968 23.951,220.798 24.035,220.879 24.347 M4.000 25.273 L 4.000 26.303 73.697 26.303 L 143.394 26.303 143.394 25.273 L 143.394 24.242 73.697 24.242 L 4.000 24.242 4.000 25.273 M256.727 25.273 L 256.727 26.303 326.424 26.303 L 396.121 26.303 396.121 25.273 L 396.121 24.242 326.424 24.242 L 256.727 24.242 256.727 25.273"/></g></svg>
.github/images/transparent.png
This is a binary file and will not be displayed.
+3-1
README.md
···88</p>
991010<p align="center">
1111- <img src="https://raw.githubusercontent.com/taciturnaxolotl/myrus/master/.github/images/line-break-thin.svg" />
1111+ <img src="https://raw.githubusercontent.com/taciturnaxolotl/carriage/main/.github/images/line-break-thin.svg" />
1212</p>
13131414## pins
···20205. `D7 (motor2DirPin)` - Direction control for Motor 2
21216. `D1 (enablePin)` - Enable/disable control for both motors
22227. `PIN_LED` - LED indicator (pin number not specified in the code snippet)
2323+2424+
23252426<p align="center">
2527 <img src="https://raw.githubusercontent.com/taciturnaxolotl/carriage/main/.github/images/line-break.svg" />
-456
blot.ino
···11-#include <Servo.h>
22-33-#define SPU 80 // 20 teeth pulley, 16 microstep, 2mm a tooth belt, 16*200 microsteps per turn == (16*200)/(20*2) == 80
44-#define PIN_SERVO D6
55-66-Servo servo;
77-88-typedef uint8_t (*CallbackFunction)(uint8_t*, int, uint8_t*);
99-1010-struct EventCallback {
1111- String event;
1212- CallbackFunction callback;
1313-};
1414-1515-1616-float pos[] = {0, 0};
1717-1818-const int motor1StepPin = D10;
1919-const int motor1DirPin = D9;
2020-const int motor2StepPin = D8;
2121-const int motor2DirPin = D7;
2222-2323-const int enablePin = D1;
2424-2525-void setup() {
2626- servo.attach(PIN_SERVO);
2727-2828- Serial.begin(9600);
2929-3030- // on("light", onLight);
3131- on("go", go);
3232- on("servo", moveServo);
3333- on("motorsOn", motorsOn);
3434- on("motorsOff", motorsOff);
3535- on("moveTowardsOrigin", moveTowardsOrigin);
3636- on("setOrigin", setOrigin);
3737-3838- pinMode(motor1StepPin, OUTPUT);
3939- pinMode(motor1DirPin, OUTPUT);
4040- pinMode(motor2StepPin, OUTPUT);
4141- pinMode(motor2DirPin, OUTPUT);
4242-4343- pinMode(enablePin, OUTPUT); // enable pin
4444-4545- pinMode(PIN_LED, OUTPUT);
4646-4747- startupIndicator(); // once startup is finished, indicate to user
4848-}
4949-5050-void loop() {
5151- readSerial();
5252-}
5353-5454-void startupIndicator() {
5555- // flash LED to indicate board is initialized
5656- for (int i = 0; i < 3; i++) {
5757- digitalWrite(PIN_LED, 0);
5858- delay(200);
5959- digitalWrite(PIN_LED, 1);
6060- delay(200);
6161- }
6262-}
6363-6464-uint8_t onLight(uint8_t* payload, int length, uint8_t* reply) {
6565- uint8_t value = payload[0];
6666-6767- // Serial.println("light it up");
6868-6969- digitalWrite(PIN_LED, value);
7070-7171- return 0;
7272-}
7373-7474-uint8_t motorsOn(uint8_t* payload, int length, uint8_t* reply) {
7575- digitalWrite(enablePin, 0);
7676- return 0;
7777-}
7878-7979-uint8_t motorsOff(uint8_t* payload, int length, uint8_t* reply) {
8080- digitalWrite(enablePin, 1);
8181- return 0;
8282-}
8383-8484-uint8_t moveTowardsOrigin(uint8_t* payload, int length, uint8_t* reply) {
8585- float x = pos[0];
8686- float y = pos[1];
8787-8888- // this ternary may not be neccesary
8989- goTo(
9090- x + ( x < 0 ? 10 : -10),
9191- y + ( y < 0 ? 10 : -10)
9292- );
9393-9494- return 0;
9595-}
9696-9797-uint8_t setOrigin(uint8_t* payload, int length, uint8_t* reply) {
9898- pos[0] = 0;
9999- pos[1] = 0;
100100-101101- return 0;
102102-}
103103-104104-105105-/* ------------------------------------------------------------ */
106106-107107-int bufferIndex = 0;
108108-uint8_t msgBuffer[100];
109109-110110-void readSerial() {
111111- while (Serial.available() > 0) {
112112- uint8_t incoming = Serial.read();
113113-114114- msgBuffer[bufferIndex] = incoming;
115115-116116- if (incoming != 0) {
117117- bufferIndex++;
118118- continue; // Proceed to the next byte if current byte is not null
119119- }
120120-121121- // Serial.print("RECEIVED: ");
122122- // for (int i = 0; i < bufferIndex; i++) {
123123- // Serial.print(msgBuffer[i]);
124124- // Serial.print(", ");
125125- // }
126126- // Serial.println("DONE");
127127-128128- // Now we have a full message, perform COBS decoding
129129- uint8_t decoded[bufferIndex];
130130- cobs_decode(decoded, msgBuffer, bufferIndex);
131131-132132- // Serial.print("DECODED: ");
133133- // for (int i = 0; i < bufferIndex; i++) {
134134- // Serial.print(decoded[i]);
135135- // Serial.print(", ");
136136- // }
137137- // Serial.println("DONE");
138138-139139- // Parse the decoded message
140140- int i = 0;
141141-142142- uint8_t msgLength = decoded[i];
143143- // Serial.print("MSG-LENGTH: ");
144144- // Serial.println(msgLength);
145145-146146- uint8_t msgArr[msgLength];
147147- i++;
148148- while (i < 1 + msgLength) {
149149- msgArr[i-1] = decoded[i];
150150- i++;
151151- }
152152-153153- // Serial.print("MSGARR: ");
154154- // for (int i = 0; i < msgLength; i++) {
155155- // Serial.print(msgArr[i]);
156156- // Serial.print(", ");
157157- // }
158158- // Serial.println("MSGARR-END");
159159-160160- uint8_t payloadLength = decoded[i];
161161- uint8_t payload[payloadLength];
162162- i++;
163163- while (i < 1 + msgLength + 1 + payloadLength) {
164164- payload[i-1-msgLength-1] = decoded[i];
165165- i++;
166166- }
167167-168168- uint8_t msgCount = decoded[i];
169169-170170- // String msg = String((char*)msgArr);
171171- String msg = byteArrayToString(msgArr, msgLength);
172172-173173- // Serial.println(msg);
174174-175175- // printArray("PAYLOAD", payload, payloadLength);
176176-177177- // Serial.print("MSGCOUNT: ");
178178- // Serial.println(msgCount);
179179-180180- bool triggered = triggerEvent(msg, payload, payloadLength, msgCount);
181181-182182- bufferIndex = 0; // Reset the buffer for the next message
183183- }
184184-}
185185-186186-/* ------------------------------------------------------------ */
187187-188188-189189-190190-const int MAX_EVENTS = 255; // Maximum number of events to store, adjust as needed
191191-EventCallback eventCallbacks[MAX_EVENTS];
192192-int eventCount = 0;
193193-194194-const int REPLY_PAYLOAD_LENGTH = 255;
195195-uint8_t reply[REPLY_PAYLOAD_LENGTH];
196196-197197-void on(String event, CallbackFunction callback) {
198198- if (eventCount < MAX_EVENTS) {
199199- eventCallbacks[eventCount].event = event;
200200- eventCallbacks[eventCount].callback = callback;
201201- eventCount = (eventCount + 1) % MAX_EVENTS;
202202- } else {
203203- // Serial.println("Max number of events reached. Wrapping events.");
204204- }
205205-}
206206-207207-bool triggerEvent(String event, uint8_t* payload, int payloadLength, uint8_t msgCount) {
208208- for (int i = 0; i < eventCount; i++) {
209209- if (eventCallbacks[i].event == event) {
210210- // want to pass payload and payloadLength, need to get response payload
211211- uint8_t reply_length = eventCallbacks[i].callback(payload, payloadLength, reply);
212212-213213- sendAck(msgCount, reply, reply_length);
214214- return true;
215215- }
216216- }
217217-218218- // Serial.println(" No event registered.");
219219- return false;
220220-}
221221-222222-const int arrayLength = 7; // + length;
223223-uint8_t byteArray[arrayLength];
224224-225225-void sendAck(uint8_t msgCount, uint8_t* reply, uint8_t length) {
226226- // Serial.println("SEND ACK");
227227-228228- // int arrayLength = 7; // + length;
229229- // static uint8_t byteArray[arrayLength];
230230-231231- byteArray[0] = 0x03;
232232- byteArray[1] = 0x61;
233233- byteArray[2] = 0x63;
234234- byteArray[3] = 0x6B;
235235- byteArray[4] = 0x00;
236236- byteArray[5] = msgCount;
237237- byteArray[6] = 0x0A;
238238-239239- // byteArray[4] = length;
240240- // for (int i = 0; i < length; i++) {
241241- // byteArray[i+5] = reply[i];
242242- // }
243243- // byteArray[5+length] = msgCount;
244244- // byteArray[6+length] = 0x0A;
245245-246246- // Serial.println(msgCount);
247247- // printArray("ACK", byteArray, 5+length);
248248-249249- Serial.write(byteArray, arrayLength);
250250-251251- // uint8_t byteArrayEncoded[arrayLength + 2]; // +2 for possible COBS overhead
252252- // cobs_encode(byteArrayEncoded, byteArray, arrayLength);
253253- // Serial.write(byteArrayEncoded, arrayLength + 2);
254254-255255- // printArray("ENCODED-ACK", byteArray, 5+length);
256256-257257-}
258258-259259-/* ------------------------------------------------------------ */
260260-261261-void cobs_encode(uint8_t *dst, const uint8_t *src, size_t len) {
262262- size_t read_index = 0;
263263- size_t write_index = 1;
264264- size_t code_index = 0;
265265- uint8_t code = 1;
266266-267267- while (read_index < len) {
268268- if (src[read_index] == 0) {
269269- dst[code_index] = code;
270270- code = 1;
271271- code_index = write_index++;
272272- read_index++;
273273- } else {
274274- dst[write_index++] = src[read_index++];
275275- code++;
276276- if (code == 0xFF) {
277277- dst[code_index] = code;
278278- code = 1;
279279- code_index = write_index++;
280280- }
281281- }
282282- }
283283-284284- dst[code_index] = code;
285285-286286- // Add trailing zero
287287- if (write_index < len + 2) {
288288- dst[write_index] = 0;
289289- }
290290-}
291291-292292-void cobs_decode(uint8_t *dst, const uint8_t *src, size_t len) {
293293- size_t i, j, dst_i = 0;
294294- for (i = 0; i < len;) {
295295- uint8_t code = src[i++];
296296- for (j = 1; j < code && i < len; j++) {
297297- dst[dst_i++] = src[i++];
298298- }
299299- if (code < 0xFF && dst_i < len) {
300300- dst[dst_i++] = 0;
301301- }
302302- }
303303-}
304304-305305-/* ------------------------------------------------------------ */
306306-307307-// TODO: THINK THIS IS BUGGY
308308-void cobs_print(const String& message) {
309309- // Convert the message to a byte array
310310- int length = message.length();
311311- uint8_t byteArray[length + 1]; // +1 for the null terminator
312312- message.getBytes(byteArray, length + 1);
313313-314314- // Prepare the buffer for the encoded message
315315- uint8_t encoded[length + 2]; // +2 for possible COBS overhead
316316-317317- // Perform COBS encoding
318318- cobs_encode(encoded, byteArray, length + 1);
319319-320320- // Send the encoded message
321321- Serial.write(encoded, length + 2); // Write the encoded bytes
322322-}
323323-324324-/* ------------------------------------------------------------ */
325325-326326-float read_float(uint8_t* buffer, int index) {
327327- uint8_t byte0 = buffer[index];
328328- uint8_t byte1 = buffer[index+1];
329329- uint8_t byte2 = buffer[index+2];
330330- uint8_t byte3 = buffer[index+3];
331331-332332- uint8_t byteArray[] = {byte0, byte1, byte2, byte3};
333333- float floatValue;
334334- memcpy(&floatValue, &byteArray, sizeof(floatValue));
335335-336336- return floatValue;
337337-}
338338-339339-int read_int(uint8_t* buffer, int index) {
340340- uint8_t byte0 = buffer[index];
341341- uint8_t byte1 = buffer[index+1];
342342- uint8_t byte2 = buffer[index+2];
343343- uint8_t byte3 = buffer[index+3];
344344-345345- uint8_t byteArray[] = {byte0, byte1, byte2, byte3};
346346- int value;
347347- memcpy(&value, &byteArray, sizeof(value));
348348-349349- return value;
350350-}
351351-352352-String byteArrayToString(byte arr[], int length) {
353353- String result = "";
354354-355355- for (int i = 0; i < length; i++) {
356356- result += (char)arr[i]; // Convert each byte to a character and append it to the string
357357- }
358358-359359- return result;
360360-}
361361-362362-/* ------------------------------------------------------------------------ */
363363-364364-#define EPSILON 0.01
365365-366366-void goTo(float x, float y) {
367367-368368- // Serial.println("START GOTO");
369369-370370- // Set your target distances for each motor (in steps)
371371- float motor1Target = (x + y) - pos[0];
372372- float motor2Target = (y - x) - pos[1];
373373-374374- // Set motor direction based on target values
375375- digitalWrite(motor1DirPin, motor1Target >= 0 ? HIGH : LOW);
376376- digitalWrite(motor2DirPin, motor2Target >= 0 ? HIGH : LOW);
377377-378378- // Calculate the relative speeds and maximum duration for both motors
379379- float maxSteps = max(abs(motor1Target), abs(motor2Target));
380380- float motor1Speed = abs(motor1Target) / maxSteps;
381381- float motor2Speed = abs(motor2Target) / maxSteps;
382382-383383- unsigned long stepDuration = 500; // The time it takes to perform one step in microseconds
384384- unsigned long motor1StepInterval = stepDuration / motor1Speed;
385385- unsigned long motor2StepInterval = stepDuration / motor2Speed;
386386-387387- // Initialize variables for step timing
388388- unsigned long motor1PrevStepTime = 0;
389389- unsigned long motor2PrevStepTime = 0;
390390- float motor1Step = 0;
391391- float motor2Step = 0;
392392-393393- // Loop until both motors reach their target steps
394394- while (abs(motor1Target - motor1Step) > EPSILON || abs(motor2Target - motor2Step) > EPSILON) {
395395-396396-397397- unsigned long currentTime = micros();
398398-399399- // Motor 1
400400- if (abs(motor1Target - motor1Step) > EPSILON && ((currentTime - motor1PrevStepTime) >= motor1StepInterval)) {
401401- digitalWrite(motor1StepPin, HIGH);
402402- delayMicroseconds(1);
403403- digitalWrite(motor1StepPin, LOW);
404404- delayMicroseconds(1);
405405-406406- motor1Step += (motor1Target >= 0 ? 1.0 : -1.0)/SPU;
407407- motor1PrevStepTime = currentTime;
408408- }
409409-410410- // Motor 2
411411- if (abs(motor2Target - motor2Step) > EPSILON && ((currentTime - motor2PrevStepTime) >= motor2StepInterval)) {
412412- digitalWrite(motor2StepPin, HIGH);
413413- delayMicroseconds(1);
414414- digitalWrite(motor2StepPin, LOW);
415415- delayMicroseconds(1);
416416-417417- motor2Step += (motor2Target >= 0 ? 1.0 : -1.0)/SPU;
418418- motor2PrevStepTime = currentTime;
419419- }
420420- }
421421-422422- // Serial.println("END GOTO");
423423-424424- pos[0] += motor1Step;
425425- pos[1] += motor2Step;
426426-}
427427-428428-uint8_t go(uint8_t* payload, int length, uint8_t* reply) {
429429- float x = read_float(payload, 0);
430430- float y = read_float(payload, 4);
431431-432432- goTo(x, y);
433433-434434- return 0;
435435-}
436436-437437-uint8_t moveServo(uint8_t* payload, int length, uint8_t* reply) {
438438- int angle = read_int(payload, 0);
439439-440440- servo.writeMicroseconds(angle);
441441-442442- return 0;
443443-}
444444-445445-/* ------ */
446446-447447-void printArray(String label, uint8_t* arr, int arrSize) {
448448- Serial.print(label);
449449- Serial.print("-BEGIN: ");
450450- for (int i = 0; i < arrSize; i++) {
451451- Serial.print(arr[i]);
452452- Serial.print(", ");
453453- }
454454- Serial.print(label);
455455- Serial.println("-END");
456456-}