/* * Multi Servo Control via Serial (C-String Version) * Includes Record/Playback and Pre-defined Sequence execution. * * 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 <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; 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 // === 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 }; // === State Machine & Timing Variables === enum State { IDLE, RECORDING, PLAYBACK }; State currentState = IDLE; unsigned long recordingStartTime = 0; unsigned long playbackStartTime = 0; int playbackIndex = 0; 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: 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); } // --- 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; byte constrainedAngle = 0; // Use byte for angle (0-180) byte currentServoMin = 0; byte currentServoMax = 180; // Basic Format Check if (strlen(segment) < 4 || segment[0] != 'S') { segmentIsValid = false; errorMessage = "[Invalid Segment Format]"; } else { char* colonPtr = strchr(segment, ':'); if (colonPtr == NULL || colonPtr == segment + 1 || *(colonPtr + 1) == '\0') { segmentIsValid = false; errorMessage = "[Malformed Segment]"; } else { *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 { stopRecording(); Serial.println(F("WARN: Recording stopped, memory full.")); } } // Feedback Serial.print(F("S")); Serial.print(servoIndex); Serial.print(F(":")); Serial.print(constrainedAngle); if (requestedAngle != constrainedAngle) { Serial.print(F("(<-")); Serial.print(requestedAngle); Serial.print(F(")")); } } else { Serial.print(errorMessage); Serial.print(F(" '")); Serial.print(segment); Serial.print(F("'")); } 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() { Serial.begin(57600); unsigned long setupStartTime = millis(); while (!Serial && (millis() - setupStartTime < 5000)) { delay(10); } 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("--------------------------------------------------------")); byte pin; // Temporary variable to hold pin from PROGMEM for (int i = 0; i < SERVO_NUM; i++) { 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(F("--------------------------------------------------------")); Serial.println(F("Initialization Complete. State: IDLE")); } // --- Arduino Main Loop --- void loop() { // === 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 // === 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) { ... } // === 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 } } // End main loop