yjyoon / 아두이노-5p1축-로봇-펌웨어 star
윤영준 윤영준 04-23
Hello Yona
@25fa56e660755ccb42315b3c68ac04198412dbfe
 
25fa56e
README.md (added)
+++ README.md
@@ -0,0 +1,1 @@
+# 아두이노-5p1축-로봇-펌웨어
 
25fa56e
firmware.ino (added)
+++ firmware.ino
@@ -0,0 +1,271 @@
+/*
+ * 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.
+}
Add a comment
List