dunkirk.sh
a scrappy gimbal that insults you in shakespearean english
feat: add blot firmware
+456
blot.ino
+456
blot.ino
···
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
+
6
+
Servo servo;
7
+
8
+
typedef uint8_t (*CallbackFunction)(uint8_t*, int, uint8_t*);
9
+
10
+
struct EventCallback {
11
+
String event;
12
+
CallbackFunction callback;
13
+
};
14
+
15
+
16
+
float pos[] = {0, 0};
17
+
18
+
const int motor1StepPin = D10;
19
+
const int motor1DirPin = D9;
20
+
const int motor2StepPin = D8;
21
+
const int motor2DirPin = D7;
22
+
23
+
const int enablePin = D1;
24
+
25
+
void 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
+
50
+
void loop() {
51
+
readSerial();
52
+
}
53
+
54
+
void 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
+
64
+
uint8_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
+
74
+
uint8_t motorsOn(uint8_t* payload, int length, uint8_t* reply) {
75
+
digitalWrite(enablePin, 0);
76
+
return 0;
77
+
}
78
+
79
+
uint8_t motorsOff(uint8_t* payload, int length, uint8_t* reply) {
80
+
digitalWrite(enablePin, 1);
81
+
return 0;
82
+
}
83
+
84
+
uint8_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
+
97
+
uint8_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
+
107
+
int bufferIndex = 0;
108
+
uint8_t msgBuffer[100];
109
+
110
+
void 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
+
190
+
const int MAX_EVENTS = 255; // Maximum number of events to store, adjust as needed
191
+
EventCallback eventCallbacks[MAX_EVENTS];
192
+
int eventCount = 0;
193
+
194
+
const int REPLY_PAYLOAD_LENGTH = 255;
195
+
uint8_t reply[REPLY_PAYLOAD_LENGTH];
196
+
197
+
void 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
+
207
+
bool 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
+
222
+
const int arrayLength = 7; // + length;
223
+
uint8_t byteArray[arrayLength];
224
+
225
+
void 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
+
261
+
void 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
+
292
+
void 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
308
+
void 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
+
326
+
float 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
+
339
+
int 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
+
352
+
String 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
+
366
+
void 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
+
428
+
uint8_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
+
437
+
uint8_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
+
447
+
void 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
+
}