commit d79bf39511b9a7bc8a050a0e53a0003a30541925 · dunkirk.sh/myrus

+456

blot.c

···

       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
       +
       }