Commit @0abbc3a2d2b2a122221a51d26bb95c7bb3e9a223 - yjyoon/아두이노-5p1축-로봇-펌웨어

윤영준 04-23
fix for wrong RECORD function
@0abbc3a2d2b2a122221a51d26bb95c7bb3e9a223
25fa56e
0abbc3a
firmware.ino
--- firmware.ino
+++ firmware.ino

 /*
- * Multi Servo Control via Serial (JSON-like format)
+ * Multi Servo Control via Serial (C-String Version)
+ * Includes Record/Playback and Pre-defined Sequence execution.
  *
- * Receives commands over Serial monitor to control multiple servos.
- * Expected command format: {S<index>:<angle>,S<index>:<angle>,...}
- * Example: {S0:90,S1:45,S5:180}
- *
- * - Validates the overall { } structure.
- * - Parses comma-separated segments.
- * - Validates each segment format (S<index>:<angle>).
- * - Uses robust integer parsing (strtol) for index and angle.
- * - Checks if servo index is within valid range.
- * - Constrains requested angle to servo's min/max limits.
- * - Controls the servos.
- * - Provides detailed feedback and error messages via Serial.
- *
- * Refactored based on discussions to improve structure, robustness, and readability.
+ * Serial Commands:
+ * {S<index>:<angle>,...} : Direct servo control (only when IDLE)
+ * RECORD                  : Start recording commands
+ * PLAY                    : Play back recorded sequence
+ * STOP                    : Stop recording or playback or sequence
+ * SEQUENCE                : Execute pre-defined interpolated sequence
  */
 
-#include <Servo.h>   // Include the Servo library
-#include <errno.h>   // For strtol error checking
-#include <stdlib.h>  // For strtol
-#include <limits.h>  // For INT_MIN, INT_MAX
+#include <Servo.h>
+#include <errno.h>
+#include <stdlib.h>
+#include <string.h>
+#include <ctype.h>
+#include <limits.h>
+#include <math.h>             // For round() during interpolation
+#include <avr/pgmspace.h>     // For PROGMEM functions
 
 // --- User Defined Constants ---
-const int SERVO_NUM = 6; // Total number of servos
+const int SERVO_NUM = 6;
+const byte SERVO_MAX[SERVO_NUM] PROGMEM = { 180, 180, 180, 130, 180, 180 }; // Store in Flash
+const byte SERVO_MIN[SERVO_NUM] PROGMEM = { 0, 0, 0, 0, 0, 0 };       // Store in Flash
+const byte SERVO_PIN[SERVO_NUM] PROGMEM = { 3, 2, 8, 9, 12, 13 };     // Store in Flash
 
-// Maximum angle limits for each servo (adjust as needed)
-const int SERVO_MAX[SERVO_NUM] = {
-  180, 180, 180, 180, 180, 180
+// === Input Buffer ===
+const byte MAX_COMMAND_LEN = 100;
+char commandInputBuffer[MAX_COMMAND_LEN];
+byte commandInputIndex = 0;
+
+// === Recording & Playback Constants & Data ===
+// Max steps - Adjust based on RAM. ~280 should be safe with ~1150 bytes free RAM
+const int MAX_RECORDED_STEPS = 280;
+//const unsigned long RECORDING_TIMEOUT_MS = 30000; // 30 seconds timeout - Optional
+
+struct RecordedStep {
+    uint16_t timeOffsetMs; // 2 bytes
+    byte servoIndex;       // 1 byte
+    byte servoAngle;       // 1 byte
+}; // Total 4 bytes per step
+
+RecordedStep recordedSequence[MAX_RECORDED_STEPS]; // ~280 * 4 = 1120 bytes SRAM
+int recordedStepsCount = 0;
+
+// === Pre-defined Sequence Constants & Data ===
+const byte NUM_SEQUENCE_STATES = 7;
+const unsigned long SEQUENCE_INTERPOLATION_TIME = 1000; // 1 second interpolation
+
+// Store sequence data in Flash memory
+const byte sequenceData[NUM_SEQUENCE_STATES][SERVO_NUM] PROGMEM = {
+  {100, 85, 175, 80, 90, 0},   // State 0
+  {100, 85, 175, 80, 90, 78},  // State 1
+  {100, 100, 105, 80, 90, 78}, // State 2
+  {180, 90, 120, 80, 90, 78},  // State 3
+  {180, 90, 130, 80, 90, 78},  // State 4
+  {180, 90, 130, 80, 90, 0},   // State 5
+  {100, 90, 90, 90, 90, 0}    // State 6
 };
 
-// Minimum angle limits for each servo (adjust as needed)
-const int SERVO_MIN[SERVO_NUM] = {
-  0, 0, 0, 0, 0, 0
-};
+// === State Machine & Timing Variables ===
+enum State { IDLE, RECORDING, PLAYBACK };
+State currentState = IDLE;
 
-// Arduino pins connected to each servo's signal line
-// Ensure these pins support PWM (usually marked with ~ on Arduino Uno/Nano)
-const int SERVO_PIN[SERVO_NUM] = {
-  3, 2, 8, 9, 12, 13
-};
-// --- End Constants ---
+unsigned long recordingStartTime = 0;
+unsigned long playbackStartTime = 0;
+int playbackIndex = 0;
 
-// Array to hold the Servo objects
+bool isRunningSequence = false;
+int currentSequenceStateIndex = 0; // Index of the *target* state
+unsigned long sequenceStateStartTime = 0;
+int sequenceStartPos[SERVO_NUM];    // Servo positions at the start of an interpolation step
+
+// --- Global Variables ---
 Servo myServos[SERVO_NUM];
 
-// --- Helper Function for Robust Integer Parsing ---
-// Attempts to parse the entire String s as an integer.
-// Returns true on success, false otherwise.
-// Stores the result in 'value' on success.
-bool parseStringToInt(const String& s, int& value) {
-  if (s.length() == 0) {
-    return false; // Empty string is not a valid number
-  }
-  // Get C-style string for strtol
-  const char* s_cstr = s.c_str();
-  char* endptr;
-  errno = 0; // Reset error number
-
-  long result = strtol(s_cstr, &endptr, 10); // Base 10
-
-  // Check for errors:
-  // 1. Did strtol encounter an error (e.g., overflow)?
-  // 2. Was the entire string consumed? (endptr should point to the null terminator '\0')
-  // 3. Check if the result fits within an int range (strtol returns long)
-  if (errno != 0 || *endptr != '\0' || result < INT_MIN || result > INT_MAX) {
-    return false; // Conversion failed, didn't consume the whole string, or out of int range
-  }
-
-  value = (int)result;
-  return true;
+// --- Helper Function: Trim leading/trailing whitespace (Unchanged) ---
+void trimWhitespace(char *str) {
+    if (str == NULL || str[0] == '\0') return;
+    char *start = str;
+    while (isspace((unsigned char)*start)) start++;
+    if (*start == '\0') { *str = '\0'; return; }
+    char *end = str + strlen(str) - 1;
+    while (end > start && isspace((unsigned char)*end)) end--;
+    *(end + 1) = '\0';
+    if (str != start) memmove(str, start, (end - start) + 2);
 }
 
-// --- Function to Process a Single Command Segment (e.g., "S0:90") ---
-// Modifies 'firstSegmentProcessed' flag to control comma printing in feedback.
-void processSegment(const String& segment, bool& firstSegmentProcessed) {
-    // --- Flags for Validation ---
-    bool segmentIsValid = true; // Assume valid initially for this segment
-    String errorMessage = "";
+// --- Helper Function for Robust Integer Parsing from C-string (Unchanged) ---
+bool parseCharStrToInt(const char* s, int& value) {
+    if (s == NULL || s[0] == '\0' || isspace((unsigned char)s[0])) return false;
+    char* endptr; errno = 0;
+    long result = strtol(s, &endptr, 10);
+    if (errno != 0 || *endptr != '\0' || result < INT_MIN || result > INT_MAX) return false;
+    value = (int)result;
+    return true;
+}
+
+// --- Function to Stop Recording ---
+void stopRecording() {
+    if (currentState == RECORDING) {
+        currentState = IDLE;
+        Serial.print(F("\nRecording stopped. ")); // Use F()
+        Serial.print(recordedStepsCount);
+        Serial.println(F(" steps recorded."));
+    }
+}
+
+// --- Function to Process a Single Command Segment (Modified for Recording) ---
+void processSegmentC(char* segment, bool& firstSegmentProcessed) {
+    bool segmentIsValid = true;
+    const char* errorMessage = "";
     int servoIndex = -1;
     int requestedAngle = 0;
-    int constrainedAngle = 0;
+    byte constrainedAngle = 0; // Use byte for angle (0-180)
+    byte currentServoMin = 0;
+    byte currentServoMax = 180;
 
-    // --- Basic Format Check ---
-    int colonIndex = segment.indexOf(':');
-
-    if (!(segment.length() > 2 && segment.startsWith("S"))) {
-        segmentIsValid = false;
-        errorMessage = "[Invalid Segment Format: Doesn't start with 'S' or too short]";
-    } else if (!(colonIndex > 1 && colonIndex < segment.length() - 1)) {
-        segmentIsValid = false;
-        errorMessage = "[Malformed Segment: Missing or misplaced colon]";
+    // Basic Format Check
+    if (strlen(segment) < 4 || segment[0] != 'S') {
+        segmentIsValid = false; errorMessage = "[Invalid Segment Format]";
     } else {
-        // --- Segment format looks okay, parse Index and Angle ---
-        String indexStr = segment.substring(1, colonIndex);
-        String angleStr = segment.substring(colonIndex + 1);
-
-        int parsedIndex;
-        int parsedAngle;
-
-        bool indexParsedOk = parseStringToInt(indexStr, parsedIndex);
-        bool angleParsedOk = false; // Parsed only if index is valid
-        bool indexInRange = false;  // Check only if index parsing was okay
-
-        if (!indexParsedOk) {
-             segmentIsValid = false;
-             errorMessage = "[Invalid Servo Index: Not a valid number]";
+        char* colonPtr = strchr(segment, ':');
+        if (colonPtr == NULL || colonPtr == segment + 1 || *(colonPtr + 1) == '\0') {
+            segmentIsValid = false; errorMessage = "[Malformed Segment]";
         } else {
-            // Index parsed, now check range
-            if (parsedIndex >= 0 && parsedIndex < SERVO_NUM) {
-                indexInRange = true;
-                // Index is valid and in range, try parsing the angle
-                angleParsedOk = parseStringToInt(angleStr, parsedAngle);
-                if (!angleParsedOk) {
-                   segmentIsValid = false;
-                   errorMessage = "[Invalid Angle: Not a valid number]";
+            *colonPtr = '\0'; // Temporarily split
+            char* indexStr = segment + 1;
+            char* angleStr = colonPtr + 1;
+            int parsedIndex, parsedAngle;
+            bool indexParsedOk = parseCharStrToInt(indexStr, parsedIndex);
+            bool angleParsedOk = false;
+            bool indexInRange = false;
+
+            if (!indexParsedOk) {
+                segmentIsValid = false; errorMessage = "[Invalid Index Num]";
+            } else {
+                 if (parsedIndex >= 0 && parsedIndex < SERVO_NUM) {
+                    indexInRange = true;
+                    angleParsedOk = parseCharStrToInt(angleStr, parsedAngle);
+                    if (!angleParsedOk) {
+                        segmentIsValid = false; errorMessage = "[Invalid Angle Num]";
+                    }
+                } else {
+                    segmentIsValid = false; errorMessage = "[Index Out of Range]";
+                }
+            }
+
+            if (segmentIsValid) {
+                servoIndex = parsedIndex;
+                requestedAngle = parsedAngle;
+                // Read min/max from PROGMEM for constraining
+                currentServoMin = pgm_read_byte(&SERVO_MIN[servoIndex]);
+                currentServoMax = pgm_read_byte(&SERVO_MAX[servoIndex]);
+                constrainedAngle = constrain(requestedAngle, currentServoMin, currentServoMax);
+            }
+        }
+    }
+
+    // --- Act, Record, and Print Feedback ---
+    if (!firstSegmentProcessed) Serial.print(F(", "));
+
+    if (segmentIsValid) {
+        myServos[servoIndex].write(constrainedAngle);
+
+        // Record if needed
+        if (currentState == RECORDING) {
+            if (recordedStepsCount < MAX_RECORDED_STEPS) {
+                unsigned long now = millis();
+                unsigned long offset = now - recordingStartTime;
+                if (offset > 65535) {
+                     stopRecording();
+                     Serial.println(F("WARN: Recording stopped due to time limit (>65s)."));
+                } else {
+                   recordedSequence[recordedStepsCount].timeOffsetMs = (uint16_t)offset;
+                   recordedSequence[recordedStepsCount].servoIndex = (byte)servoIndex;
+                   recordedSequence[recordedStepsCount].servoAngle = constrainedAngle; // Store constrained angle
+                   recordedStepsCount++;
                 }
             } else {
-                 segmentIsValid = false;
-                 errorMessage = "[Invalid Servo Index: Out of range (0-" + String(SERVO_NUM - 1) + ")]";
+                stopRecording();
+                Serial.println(F("WARN: Recording stopped, memory full."));
             }
         }
 
-        // If all checks passed so far, store values and constrain angle
-        if (segmentIsValid) {
-            servoIndex = parsedIndex;
-            requestedAngle = parsedAngle;
-            // Constrain the angle using servo-specific limits
-            constrainedAngle = constrain(requestedAngle, SERVO_MIN[servoIndex], SERVO_MAX[servoIndex]);
-        }
-    }
-
-    // --- Act based on validation and Print Feedback ---
-    // Print comma separator if not the first valid/invalid segment processed
-    if (!firstSegmentProcessed) Serial.print(", ");
-
-    if (segmentIsValid) {
-        // *** Robot Direct Control is Here ***
-        myServos[servoIndex].write(constrainedAngle);
-
-        // Feedback for success
-        Serial.print("S"); Serial.print(servoIndex); Serial.print(":"); Serial.print(constrainedAngle);
+        // Feedback
+        Serial.print(F("S")); Serial.print(servoIndex); Serial.print(F(":")); Serial.print(constrainedAngle);
         if (requestedAngle != constrainedAngle) {
-            // Show if the angle was constrained
-            Serial.print("(<-"); Serial.print(requestedAngle); Serial.print(")");
+            Serial.print(F("(<-")); Serial.print(requestedAngle); Serial.print(F(")"));
         }
     } else {
-        // Feedback for error
-        Serial.print(errorMessage); Serial.print(" '"); Serial.print(segment); Serial.print("'");
+        Serial.print(errorMessage); Serial.print(F(" '")); Serial.print(segment); Serial.print(F("'"));
     }
-    // Mark that at least one segment (valid or invalid) has been processed and outputted
     firstSegmentProcessed = false;
 }
 
+// --- Function to process the raw command buffer ---
+void processCommand(char* cmd) {
+    trimWhitespace(cmd);
+    int cmdLen = strlen(cmd);
+    if (cmdLen == 0) return;
+
+    // --- Handle Control Commands (RECORD, PLAY, STOP, SEQUENCE) FIRST ---
+    if (strcmp(cmd, "RECORD") == 0) {
+        if (currentState == IDLE && !isRunningSequence) {
+            currentState = RECORDING;
+            recordedStepsCount = 0;
+            recordingStartTime = millis();
+            Serial.println(F("Recording started... (Send STOP or wait)"));
+        } else {
+            Serial.println(F("Error: Cannot start recording now (Busy/Sequence)."));
+        }
+        return; // Command handled
+    } else if (strcmp(cmd, "PLAY") == 0) {
+        if (currentState == IDLE && !isRunningSequence) {
+            if (recordedStepsCount > 0) {
+                currentState = PLAYBACK;
+                playbackIndex = 0;
+                playbackStartTime = millis();
+                Serial.println(F("Playback started..."));
+            } else {
+                Serial.println(F("Error: Nothing recorded to play."));
+            }
+        } else {
+             Serial.println(F("Error: Cannot start playback now (Busy/Sequence)."));
+        }
+         return; // Command handled
+    } else if (strcmp(cmd, "STOP") == 0) {
+        bool stoppedSomething = false;
+        if (currentState == RECORDING) {
+            stopRecording(); // stopRecording changes state to IDLE
+            stoppedSomething = true;
+        }
+        if (currentState == PLAYBACK) { // Use 'if', not 'else if', in case stopRecording was just called
+            currentState = IDLE;
+            Serial.println(F("\nPlayback stopped by command."));
+            stoppedSomething = true;
+        }
+        if (isRunningSequence) { // Use 'if', check independently
+            isRunningSequence = false; // Stop sequence execution
+            currentState = IDLE;      // Ensure state is IDLE
+            Serial.println(F("\nSequence stopped by command."));
+            stoppedSomething = true;
+        }
+
+        if (!stoppedSomething && currentState == IDLE) { // Only print if nothing was actually stopped
+             Serial.println(F("Status: IDLE (Nothing to stop)."));
+        }
+        return; // Command handled
+    } else if (strcmp(cmd, "SEQUENCE") == 0) {
+        if (currentState == IDLE && !isRunningSequence) {
+            isRunningSequence = true;
+            currentSequenceStateIndex = 0; // Target the first state
+            sequenceStateStartTime = millis();
+            // Record starting positions for interpolation
+            for (int i = 0; i < SERVO_NUM; i++) {
+                sequenceStartPos[i] = myServos[i].read(); // Read current angle
+            }
+            Serial.println(F("Sequence started..."));
+        } else {
+             Serial.println(F("Error: Cannot start sequence now (Busy)."));
+        }
+        return; // Command handled
+    }
+
+    // --- Check for Movement Command Format {Sx:y...} ---
+    if (cmd[0] == '{' && cmd[cmdLen - 1] == '}') {
+        // Command has the potential { } structure for movement.
+        // Now, check if the state *allows* direct servo control commands.
+        if (currentState == IDLE || currentState == RECORDING) {
+            // Process the segments. processSegmentC handles recording IF currentState is RECORDING.
+            cmd[cmdLen - 1] = '\0'; // Null-terminate before '}'
+            char* content = cmd + 1; // Point after '{'
+            trimWhitespace(content);
+            if (content[0] == '\0') {
+                Serial.println(F("Received empty command: {}"));
+                return; // Handled (empty command)
+            }
+
+            Serial.print(F("Processing: {"));
+            char* segment; char* saveptr; bool firstSegment = true;
+            for (segment = strtok_r(content, ",", &saveptr); segment != NULL; segment = strtok_r(NULL, ",", &saveptr))
+            {
+                trimWhitespace(segment);
+                if (segment[0] == '\0') continue; // Skip empty segments like {S1:90,,S2:80}
+                processSegmentC(segment, firstSegment); // This function moves the servo AND records if needed
+            }
+            Serial.println(F("}"));
+
+        } else {
+            // Current state is PLAYBACK or SEQUENCE - cannot accept direct movement commands.
+            Serial.println(F("Info: Movement command ignored while PLAYBACK or SEQUENCE running. Use STOP first."));
+        }
+        return; // Command handled (either processed or intentionally ignored based on state)
+    }
+
+    // --- If the command wasn't a known control word and didn't match {Sx:y...} format ---
+    Serial.print(F("Error: Unknown command or invalid format '")); Serial.print(cmd);
+    Serial.println(F("'. Expected {Sx:y,...} or control word."));
+}
 
 // --- Arduino Setup Function ---
 void setup() {
-  // Start Serial communication (baud rate should match Serial Monitor setting)
   Serial.begin(57600);
-  // Wait for Serial port to connect (needed for native USB like Leonardo, Micro)
-  // Add a timeout to prevent blocking forever if Serial isn't connected
   unsigned long setupStartTime = millis();
-  while (!Serial && (millis() - setupStartTime < 5000)) { // Wait max 5 seconds
-    delay(10); // Small delay while waiting
-  }
+  while (!Serial && (millis() - setupStartTime < 5000)) { delay(10); }
 
-  Serial.println("\n--- Multi Servo Control via Serial ---");
-  Serial.print("Initializing "); Serial.print(SERVO_NUM); Serial.println(" servos...");
-  Serial.println("Command format: {S<index>:<angle>,S<index>:<angle>,...}");
-  Serial.println("Example: {S0:90,S1:45,S5:180}");
-  Serial.println("Valid servo indices: 0 to " + String(SERVO_NUM - 1));
-  Serial.println("---------------------------------------");
+  Serial.println(F("\n--- Multi Servo Control: C-String + Record/Play/Sequence ---"));
+  Serial.print(MAX_RECORDED_STEPS); Serial.println(F(" steps max recording."));
+  Serial.println(F("Commands: {Sx:y,...} | RECORD | PLAY | STOP | SEQUENCE"));
+  Serial.println(F("--------------------------------------------------------"));
 
-  // Attach each servo to its pin and set initial position
+  byte pin; // Temporary variable to hold pin from PROGMEM
   for (int i = 0; i < SERVO_NUM; i++) {
-    myServos[i].attach(SERVO_PIN[i]);
-    // Set servos to a defined starting position (e.g., minimum)
-    // Consider if middle (90) or another position is a safer start for your setup
-    myServos[i].write( (SERVO_MIN[i]+SERVO_MAX[i])/2 );
-    delay(20); // Allow time for servo to potentially reach initial position
-    Serial.print("Servo "); Serial.print(i);
-    Serial.print(" attached to pin "); Serial.print(SERVO_PIN[i]);
-    Serial.print(" | Limits: ["); Serial.print(SERVO_MIN[i]);
-    Serial.print(", "); Serial.print(SERVO_MAX[i]); Serial.println("]");
+    pin = pgm_read_byte(&SERVO_PIN[i]); // Read pin from PROGMEM
+    myServos[i].attach(pin);
+    // Start servos at midpoint
+    myServos[i].write( (pgm_read_byte(&SERVO_MIN[i]) + pgm_read_byte(&SERVO_MAX[i])) / 2 );
+    delay(20);
+    Serial.print(F("Servo ")); Serial.print(i);
+    Serial.print(F(" attached to pin ")); Serial.print(pin);
+    Serial.print(F(" | Limits: [")); Serial.print(pgm_read_byte(&SERVO_MIN[i]));
+    Serial.print(F(", ")); Serial.print(pgm_read_byte(&SERVO_MAX[i])); Serial.println(F("]"));
   }
-  Serial.println("---------------------------------------");
-  Serial.println("Initialization Complete. Ready for commands.");
+  Serial.println(F("--------------------------------------------------------"));
+  Serial.println(F("Initialization Complete. State: IDLE"));
 }
 
 // --- Arduino Main Loop ---
 void loop() {
-  // Check if data is available to read from Serial
-  if (Serial.available() > 0) {
-    // Read the incoming string until newline character
-    // NOTE: Using the String class extensively can lead to memory fragmentation
-    // on memory-constrained boards (like Arduino Uno/Nano/Mega) over long periods.
-    // If the device needs to run reliably for extended durations or handles
-    // very large commands, consider switching to C-style char arrays.
-    String command = Serial.readStringUntil('\n');
-    command.trim(); // Remove leading/trailing whitespace & newline chars
 
-    // Ignore empty commands after trimming
-    if (command.length() == 0) {
-        return; // Nothing to do, wait for next loop iteration
+  // === Sequence Execution Logic ===
+  if (isRunningSequence) {
+      unsigned long now = millis();
+      unsigned long elapsed = now - sequenceStateStartTime;
+
+      if (elapsed >= SEQUENCE_INTERPOLATION_TIME) {
+          // Time's up for this step, ensure final position is reached
+          byte targetAngle;
+          for (int i = 0; i < SERVO_NUM; i++) {
+              targetAngle = pgm_read_byte(&sequenceData[currentSequenceStateIndex][i]);
+              // Use constrain just in case, read limits from PROGMEM
+              targetAngle = constrain(targetAngle, pgm_read_byte(&SERVO_MIN[i]), pgm_read_byte(&SERVO_MAX[i]));
+              myServos[i].write(targetAngle);
+              sequenceStartPos[i] = targetAngle; // Update start pos for next potential step
+          }
+
+          // Move to the next state
+          currentSequenceStateIndex++;
+
+          // Check if sequence is complete
+          if (currentSequenceStateIndex >= NUM_SEQUENCE_STATES) {
+              isRunningSequence = false;
+              currentState = IDLE; // Ensure state is IDLE
+              Serial.println(F("\nSequence finished."));
+          } else {
+              // Reset timer for the next interpolation step
+              sequenceStateStartTime = now; // Use current time to avoid drift
+              // No need to update start positions here again, done above after write
+               Serial.print(F("Sequence step ")); Serial.print(currentSequenceStateIndex-1); Serial.println(F(" complete. Starting next."));
+          }
+      } else {
+          // --- Interpolate ---
+          float fraction = (float)elapsed / SEQUENCE_INTERPOLATION_TIME;
+          byte targetAngle;
+          int startAngle;
+          int interpolatedAngle;
+
+          for (int i = 0; i < SERVO_NUM; i++) {
+              // Read target angle for the current destination state from PROGMEM
+              targetAngle = pgm_read_byte(&sequenceData[currentSequenceStateIndex][i]);
+              startAngle = sequenceStartPos[i];
+
+              // Calculate interpolated angle
+              interpolatedAngle = round(startAngle + (targetAngle - startAngle) * fraction);
+
+              // Constrain (safety, though shouldn't be needed if sequenceData is valid)
+              interpolatedAngle = constrain(interpolatedAngle, pgm_read_byte(&SERVO_MIN[i]), pgm_read_byte(&SERVO_MAX[i]));
+
+              // Write interpolated position
+              // Check if different from last write might save some servo jitter/power? Optional.
+              // if (myServos[i].read() != interpolatedAngle) { // read() might be slow
+                  myServos[i].write(interpolatedAngle);
+              // }
+          }
+      }
+  } // End Sequence Logic
+
+  // === Playback Logic ===
+  else if (currentState == PLAYBACK) { // Only run if not running sequence
+    if (playbackIndex < recordedStepsCount) {
+      RecordedStep& nextStep = recordedSequence[playbackIndex];
+      unsigned long targetTime = playbackStartTime + nextStep.timeOffsetMs;
+      if (millis() >= targetTime) {
+        myServos[nextStep.servoIndex].write(nextStep.servoAngle);
+        playbackIndex++;
+      }
+    } else {
+      currentState = IDLE;
+      Serial.println(F("\nPlayback finished."));
     }
+  } // End Playback Logic
 
-    // --- Validate overall command format FIRST ---
-    bool formatIsValid = command.startsWith("{") && command.endsWith("}");
+  // === Recording Check (Memory Limit handled in processSegmentC) ===
+  // Optional: Timeout check could be re-added here if desired
+  // if (currentState == RECORDING && millis() - recordingStartTime >= RECORDING_TIMEOUT_MS) { ... }
 
-    // --- Guard Clause: If format is NOT valid, print error and exit this iteration ---
-    if (!formatIsValid) {
-      Serial.print("Error: Invalid command format '");
-      Serial.print(command);
-      Serial.println("'. Expected {S<index>:<angle>,...}");
-      return; // Stop processing this invalid command
-    }
 
-    // --- Format IS Valid: Proceed with processing ---
+  // === Command Processing Logic (Reading from Serial) ===
+  while (Serial.available() > 0 && commandInputIndex < MAX_COMMAND_LEN - 1) {
+      char receivedChar = Serial.read();
+      if (receivedChar == '\n' || receivedChar == '\r') {
+          if (commandInputIndex > 0) {
+              commandInputBuffer[commandInputIndex] = '\0';
+              processCommand(commandInputBuffer);
+          }
+          commandInputIndex = 0;
+          break;
+      } else if (isprint(receivedChar)) {
+          commandInputBuffer[commandInputIndex++] = receivedChar;
+      }
+  }
+  // Buffer overflow check
+  if (commandInputIndex >= MAX_COMMAND_LEN - 1) {
+      Serial.println(F("Error: Command buffer overflow. Command discarded."));
+      commandInputIndex = 0;
+      while(Serial.available() > 0 && Serial.read() != '\n'); // Clear rest of serial line
+  }
 
-    // Remove the braces to get the content string
-    String content = command.substring(1, command.length() - 1);
-    content.trim(); // Trim again in case of spaces like { S0:90 }
-
-    // Handle empty content like {}
-    if (content.length() == 0) {
-        Serial.println("Received empty command: {}");
-        return; // Nothing to process
-    }
-
-    // Print command start confirmation
-    Serial.print("Processing: {");
-
-    // Process each command segment separated by commas
-    int startIndex = 0;
-    bool firstSegmentOutput = true; // Flag to manage comma printing in feedback
-
-    while (startIndex < content.length()) {
-        // Extract the next segment based on comma delimiter
-        int commaIndex = content.indexOf(',', startIndex);
-        String currentSegment;
-
-        if (commaIndex == -1) {
-            // No more commas - this is the last segment
-            currentSegment = content.substring(startIndex);
-            startIndex = content.length(); // Set index to exit loop after this segment
-        } else {
-            // Extract segment before the next comma
-            currentSegment = content.substring(startIndex, commaIndex);
-            startIndex = commaIndex + 1; // Move start index past the comma for next iteration
-        }
-
-        currentSegment.trim(); // Clean up the individual segment (remove spaces)
-
-        // Skip empty segments that might result from extra commas (e.g., {S0:90,,S1:90})
-        if (currentSegment.length() == 0) {
-            continue; // Go to the next iteration of the while loop
-        }
-
-        // Process the extracted segment using the helper function
-        processSegment(currentSegment, firstSegmentOutput);
-
-    } // End while loop processing segments
-
-    Serial.println("}"); // End the feedback line for the processed command
-
-  } // End if Serial.available()
-
-  // No delay() needed here in most cases. The loop runs again quickly.
-  // Other non-blocking code (reading sensors, checking buttons) could go here.
-}
+} // End main loop
Add a comment
Open 0
Closed 0
List
Delete comment
