a scrappy gimbal that insults you in shakespearean english
dunkirk.sh
1#include <Servo.h>
2
3#define SPU 80 // 20 teeth pulley, 16 microstep, 2mm a tooth belt, 16*200 microsteps per turn == (16*200)/(20*2) == 80
4#define PIN_SERVO D6
5
6Servo servo;
7
8typedef uint8_t (*CallbackFunction)(uint8_t*, int, uint8_t*);
9
10struct EventCallback {
11 String event;
12 CallbackFunction callback;
13};
14
15
16float pos[] = {0, 0};
17
18const int motor1StepPin = D10;
19const int motor1DirPin = D9;
20const int motor2StepPin = D8;
21const int motor2DirPin = D7;
22
23const int enablePin = D1;
24
25void setup() {
26 servo.attach(PIN_SERVO);
27
28 Serial.begin(9600);
29
30 // on("light", onLight);
31 on("go", go);
32 on("servo", moveServo);
33 on("motorsOn", motorsOn);
34 on("motorsOff", motorsOff);
35 on("moveTowardsOrigin", moveTowardsOrigin);
36 on("setOrigin", setOrigin);
37
38 pinMode(motor1StepPin, OUTPUT);
39 pinMode(motor1DirPin, OUTPUT);
40 pinMode(motor2StepPin, OUTPUT);
41 pinMode(motor2DirPin, OUTPUT);
42
43 pinMode(enablePin, OUTPUT); // enable pin
44
45 pinMode(PIN_LED, OUTPUT);
46
47 startupIndicator(); // once startup is finished, indicate to user
48}
49
50void loop() {
51 readSerial();
52}
53
54void startupIndicator() {
55 // flash LED to indicate board is initialized
56 for (int i = 0; i < 3; i++) {
57 digitalWrite(PIN_LED, 0);
58 delay(200);
59 digitalWrite(PIN_LED, 1);
60 delay(200);
61 }
62}
63
64uint8_t onLight(uint8_t* payload, int length, uint8_t* reply) {
65 uint8_t value = payload[0];
66
67 // Serial.println("light it up");
68
69 digitalWrite(PIN_LED, value);
70
71 return 0;
72}
73
74uint8_t motorsOn(uint8_t* payload, int length, uint8_t* reply) {
75 digitalWrite(enablePin, 0);
76 return 0;
77}
78
79uint8_t motorsOff(uint8_t* payload, int length, uint8_t* reply) {
80 digitalWrite(enablePin, 1);
81 return 0;
82}
83
84uint8_t moveTowardsOrigin(uint8_t* payload, int length, uint8_t* reply) {
85 float x = pos[0];
86 float y = pos[1];
87
88 // this ternary may not be neccesary
89 goTo(
90 x + ( x < 0 ? 10 : -10),
91 y + ( y < 0 ? 10 : -10)
92 );
93
94 return 0;
95}
96
97uint8_t setOrigin(uint8_t* payload, int length, uint8_t* reply) {
98 pos[0] = 0;
99 pos[1] = 0;
100
101 return 0;
102}
103
104
105/* ------------------------------------------------------------ */
106
107int bufferIndex = 0;
108uint8_t msgBuffer[100];
109
110void readSerial() {
111 while (Serial.available() > 0) {
112 uint8_t incoming = Serial.read();
113
114 msgBuffer[bufferIndex] = incoming;
115
116 if (incoming != 0) {
117 bufferIndex++;
118 continue; // Proceed to the next byte if current byte is not null
119 }
120
121 // Serial.print("RECEIVED: ");
122 // for (int i = 0; i < bufferIndex; i++) {
123 // Serial.print(msgBuffer[i]);
124 // Serial.print(", ");
125 // }
126 // Serial.println("DONE");
127
128 // Now we have a full message, perform COBS decoding
129 uint8_t decoded[bufferIndex];
130 cobs_decode(decoded, msgBuffer, bufferIndex);
131
132 // Serial.print("DECODED: ");
133 // for (int i = 0; i < bufferIndex; i++) {
134 // Serial.print(decoded[i]);
135 // Serial.print(", ");
136 // }
137 // Serial.println("DONE");
138
139 // Parse the decoded message
140 int i = 0;
141
142 uint8_t msgLength = decoded[i];
143 // Serial.print("MSG-LENGTH: ");
144 // Serial.println(msgLength);
145
146 uint8_t msgArr[msgLength];
147 i++;
148 while (i < 1 + msgLength) {
149 msgArr[i-1] = decoded[i];
150 i++;
151 }
152
153 // Serial.print("MSGARR: ");
154 // for (int i = 0; i < msgLength; i++) {
155 // Serial.print(msgArr[i]);
156 // Serial.print(", ");
157 // }
158 // Serial.println("MSGARR-END");
159
160 uint8_t payloadLength = decoded[i];
161 uint8_t payload[payloadLength];
162 i++;
163 while (i < 1 + msgLength + 1 + payloadLength) {
164 payload[i-1-msgLength-1] = decoded[i];
165 i++;
166 }
167
168 uint8_t msgCount = decoded[i];
169
170 // String msg = String((char*)msgArr);
171 String msg = byteArrayToString(msgArr, msgLength);
172
173 // Serial.println(msg);
174
175 // printArray("PAYLOAD", payload, payloadLength);
176
177 // Serial.print("MSGCOUNT: ");
178 // Serial.println(msgCount);
179
180 bool triggered = triggerEvent(msg, payload, payloadLength, msgCount);
181
182 bufferIndex = 0; // Reset the buffer for the next message
183 }
184}
185
186/* ------------------------------------------------------------ */
187
188
189
190const int MAX_EVENTS = 255; // Maximum number of events to store, adjust as needed
191EventCallback eventCallbacks[MAX_EVENTS];
192int eventCount = 0;
193
194const int REPLY_PAYLOAD_LENGTH = 255;
195uint8_t reply[REPLY_PAYLOAD_LENGTH];
196
197void on(String event, CallbackFunction callback) {
198 if (eventCount < MAX_EVENTS) {
199 eventCallbacks[eventCount].event = event;
200 eventCallbacks[eventCount].callback = callback;
201 eventCount = (eventCount + 1) % MAX_EVENTS;
202 } else {
203 // Serial.println("Max number of events reached. Wrapping events.");
204 }
205}
206
207bool triggerEvent(String event, uint8_t* payload, int payloadLength, uint8_t msgCount) {
208 for (int i = 0; i < eventCount; i++) {
209 if (eventCallbacks[i].event == event) {
210 // want to pass payload and payloadLength, need to get response payload
211 uint8_t reply_length = eventCallbacks[i].callback(payload, payloadLength, reply);
212
213 sendAck(msgCount, reply, reply_length);
214 return true;
215 }
216 }
217
218 // Serial.println(" No event registered.");
219 return false;
220}
221
222const int arrayLength = 7; // + length;
223uint8_t byteArray[arrayLength];
224
225void sendAck(uint8_t msgCount, uint8_t* reply, uint8_t length) {
226 // Serial.println("SEND ACK");
227
228 // int arrayLength = 7; // + length;
229 // static uint8_t byteArray[arrayLength];
230
231 byteArray[0] = 0x03;
232 byteArray[1] = 0x61;
233 byteArray[2] = 0x63;
234 byteArray[3] = 0x6B;
235 byteArray[4] = 0x00;
236 byteArray[5] = msgCount;
237 byteArray[6] = 0x0A;
238
239 // byteArray[4] = length;
240 // for (int i = 0; i < length; i++) {
241 // byteArray[i+5] = reply[i];
242 // }
243 // byteArray[5+length] = msgCount;
244 // byteArray[6+length] = 0x0A;
245
246 // Serial.println(msgCount);
247 // printArray("ACK", byteArray, 5+length);
248
249 Serial.write(byteArray, arrayLength);
250
251 // uint8_t byteArrayEncoded[arrayLength + 2]; // +2 for possible COBS overhead
252 // cobs_encode(byteArrayEncoded, byteArray, arrayLength);
253 // Serial.write(byteArrayEncoded, arrayLength + 2);
254
255 // printArray("ENCODED-ACK", byteArray, 5+length);
256
257}
258
259/* ------------------------------------------------------------ */
260
261void cobs_encode(uint8_t *dst, const uint8_t *src, size_t len) {
262 size_t read_index = 0;
263 size_t write_index = 1;
264 size_t code_index = 0;
265 uint8_t code = 1;
266
267 while (read_index < len) {
268 if (src[read_index] == 0) {
269 dst[code_index] = code;
270 code = 1;
271 code_index = write_index++;
272 read_index++;
273 } else {
274 dst[write_index++] = src[read_index++];
275 code++;
276 if (code == 0xFF) {
277 dst[code_index] = code;
278 code = 1;
279 code_index = write_index++;
280 }
281 }
282 }
283
284 dst[code_index] = code;
285
286 // Add trailing zero
287 if (write_index < len + 2) {
288 dst[write_index] = 0;
289 }
290}
291
292void cobs_decode(uint8_t *dst, const uint8_t *src, size_t len) {
293 size_t i, j, dst_i = 0;
294 for (i = 0; i < len;) {
295 uint8_t code = src[i++];
296 for (j = 1; j < code && i < len; j++) {
297 dst[dst_i++] = src[i++];
298 }
299 if (code < 0xFF && dst_i < len) {
300 dst[dst_i++] = 0;
301 }
302 }
303}
304
305/* ------------------------------------------------------------ */
306
307// TODO: THINK THIS IS BUGGY
308void cobs_print(const String& message) {
309 // Convert the message to a byte array
310 int length = message.length();
311 uint8_t byteArray[length + 1]; // +1 for the null terminator
312 message.getBytes(byteArray, length + 1);
313
314 // Prepare the buffer for the encoded message
315 uint8_t encoded[length + 2]; // +2 for possible COBS overhead
316
317 // Perform COBS encoding
318 cobs_encode(encoded, byteArray, length + 1);
319
320 // Send the encoded message
321 Serial.write(encoded, length + 2); // Write the encoded bytes
322}
323
324/* ------------------------------------------------------------ */
325
326float read_float(uint8_t* buffer, int index) {
327 uint8_t byte0 = buffer[index];
328 uint8_t byte1 = buffer[index+1];
329 uint8_t byte2 = buffer[index+2];
330 uint8_t byte3 = buffer[index+3];
331
332 uint8_t byteArray[] = {byte0, byte1, byte2, byte3};
333 float floatValue;
334 memcpy(&floatValue, &byteArray, sizeof(floatValue));
335
336 return floatValue;
337}
338
339int read_int(uint8_t* buffer, int index) {
340 uint8_t byte0 = buffer[index];
341 uint8_t byte1 = buffer[index+1];
342 uint8_t byte2 = buffer[index+2];
343 uint8_t byte3 = buffer[index+3];
344
345 uint8_t byteArray[] = {byte0, byte1, byte2, byte3};
346 int value;
347 memcpy(&value, &byteArray, sizeof(value));
348
349 return value;
350}
351
352String byteArrayToString(byte arr[], int length) {
353 String result = "";
354
355 for (int i = 0; i < length; i++) {
356 result += (char)arr[i]; // Convert each byte to a character and append it to the string
357 }
358
359 return result;
360}
361
362/* ------------------------------------------------------------------------ */
363
364#define EPSILON 0.01
365
366void goTo(float x, float y) {
367
368 // Serial.println("START GOTO");
369
370 // Set your target distances for each motor (in steps)
371 float motor1Target = (x + y) - pos[0];
372 float motor2Target = (y - x) - pos[1];
373
374 // Set motor direction based on target values
375 digitalWrite(motor1DirPin, motor1Target >= 0 ? HIGH : LOW);
376 digitalWrite(motor2DirPin, motor2Target >= 0 ? HIGH : LOW);
377
378 // Calculate the relative speeds and maximum duration for both motors
379 float maxSteps = max(abs(motor1Target), abs(motor2Target));
380 float motor1Speed = abs(motor1Target) / maxSteps;
381 float motor2Speed = abs(motor2Target) / maxSteps;
382
383 unsigned long stepDuration = 500; // The time it takes to perform one step in microseconds
384 unsigned long motor1StepInterval = stepDuration / motor1Speed;
385 unsigned long motor2StepInterval = stepDuration / motor2Speed;
386
387 // Initialize variables for step timing
388 unsigned long motor1PrevStepTime = 0;
389 unsigned long motor2PrevStepTime = 0;
390 float motor1Step = 0;
391 float motor2Step = 0;
392
393 // Loop until both motors reach their target steps
394 while (abs(motor1Target - motor1Step) > EPSILON || abs(motor2Target - motor2Step) > EPSILON) {
395
396
397 unsigned long currentTime = micros();
398
399 // Motor 1
400 if (abs(motor1Target - motor1Step) > EPSILON && ((currentTime - motor1PrevStepTime) >= motor1StepInterval)) {
401 digitalWrite(motor1StepPin, HIGH);
402 delayMicroseconds(1);
403 digitalWrite(motor1StepPin, LOW);
404 delayMicroseconds(1);
405
406 motor1Step += (motor1Target >= 0 ? 1.0 : -1.0)/SPU;
407 motor1PrevStepTime = currentTime;
408 }
409
410 // Motor 2
411 if (abs(motor2Target - motor2Step) > EPSILON && ((currentTime - motor2PrevStepTime) >= motor2StepInterval)) {
412 digitalWrite(motor2StepPin, HIGH);
413 delayMicroseconds(1);
414 digitalWrite(motor2StepPin, LOW);
415 delayMicroseconds(1);
416
417 motor2Step += (motor2Target >= 0 ? 1.0 : -1.0)/SPU;
418 motor2PrevStepTime = currentTime;
419 }
420 }
421
422 // Serial.println("END GOTO");
423
424 pos[0] += motor1Step;
425 pos[1] += motor2Step;
426}
427
428uint8_t go(uint8_t* payload, int length, uint8_t* reply) {
429 float x = read_float(payload, 0);
430 float y = read_float(payload, 4);
431
432 goTo(x, y);
433
434 return 0;
435}
436
437uint8_t moveServo(uint8_t* payload, int length, uint8_t* reply) {
438 int angle = read_int(payload, 0);
439
440 servo.writeMicroseconds(angle);
441
442 return 0;
443}
444
445/* ------ */
446
447void printArray(String label, uint8_t* arr, int arrSize) {
448 Serial.print(label);
449 Serial.print("-BEGIN: ");
450 for (int i = 0; i < arrSize; i++) {
451 Serial.print(arr[i]);
452 Serial.print(", ");
453 }
454 Serial.print(label);
455 Serial.println("-END");
456}