/* * Multi Servo Control via Serial (JSON-like format) * * 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. */ #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 // --- User Defined Constants --- const int SERVO_NUM = 6; // Total number of servos // Maximum angle limits for each servo (adjust as needed) const int SERVO_MAX[SERVO_NUM] = { 180, 180, 180, 180, 180, 180 }; // Minimum angle limits for each servo (adjust as needed) const int SERVO_MIN[SERVO_NUM] = { 0, 0, 0, 0, 0, 0 }; // 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 --- // Array to hold the Servo objects 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; } // --- 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 = ""; int servoIndex = -1; int requestedAngle = 0; int constrainedAngle = 0; // --- 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]"; } 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]"; } 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]"; } } else { segmentIsValid = false; errorMessage = "[Invalid Servo Index: Out of range (0-" + String(SERVO_NUM - 1) + ")]"; } } // 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); if (requestedAngle != constrainedAngle) { // Show if the angle was constrained Serial.print("(<-"); Serial.print(requestedAngle); Serial.print(")"); } } else { // Feedback for error Serial.print(errorMessage); Serial.print(" '"); Serial.print(segment); Serial.print("'"); } // Mark that at least one segment (valid or invalid) has been processed and outputted firstSegmentProcessed = false; } // --- 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 } 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("---------------------------------------"); // Attach each servo to its pin and set initial position 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("]"); } Serial.println("---------------------------------------"); Serial.println("Initialization Complete. Ready for commands."); } // --- 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 } // --- Validate overall command format FIRST --- bool formatIsValid = command.startsWith("{") && command.endsWith("}"); // --- 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 --- // 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. }