File name
Commit message
Commit date
/*
* 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 and RECORD)
* 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, 108 }; // 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