Initial commit

2026-07-07 12:33:24 +08:00
commit a93e3a8878
3 changed files with 555 additions and 0 deletions
@@ -0,0 +1,229 @@
+// --- Hardware Configuration ---
+#define IN1 9
+#define IN2 10
+#define IN3 11
+#define IN4 12
+
+// Step sequences for 28BYJ-48
+// Clockwise sequence
+int stepSequenceCW[4][4] = {
+  { 1, 0, 0, 0 },
+  { 0, 1, 0, 0 },
+  { 0, 0, 1, 0 },
+  { 0, 0, 0, 1 }
+};
+
+// Counter-Clockwise sequence (reverse of CW)
+int stepSequenceCCW[4][4] = {
+  { 0, 0, 0, 1 },
+  { 0, 0, 1, 0 },
+  { 0, 1, 0, 0 },
+  { 1, 0, 0, 0 }
+};
+
+// --- Configuration Constants ---
+const int STEPS_PER_REVOLUTION = 2048; // 28BYJ-48 standard (512 steps * 4 gear reduction)
+const float DEGREES_PER_STEP = 360.0 / STEPS_PER_REVOLUTION;
+
+// --- Global Variables ---
+enum State { MOVE, WAIT };
+enum Mode { AUTO, MANUAL };
+
+Mode autoMode = AUTO;          // Default: AUTO
+float targetAngle = 0.0;       // Target angle in degrees
+float turnSpeed = 5.0;         // Speed in degrees per second
+
+State _currState = MOVE;       // Default: MOVE
+
+float waitInterval = 8.0;      // Wait time in seconds (default 8s)
+unsigned long _waitTime = 0;   // Timestamp for wait state
+
+float _stepInterval = 0.0;     // Time between steps in seconds
+unsigned long _stepTime = 0;   // Timestamp for last step
+
+int _stepCount = 0;            // Total steps to move
+int _stepsRemaining = 0;       // Steps left in current move
+int _currentStepIndex = 0;     // Current index in step sequence (0-3)
+bool _directionCW = true;      // true = CW, false = CCW
+
+// --- Helper Functions ---
+
+void setStep(int a, int b, int c, int d) {
+  digitalWrite(IN1, a);
+  digitalWrite(IN2, b);
+  digitalWrite(IN3, c);
+  digitalWrite(IN4, d);
+}
+
+void updateStepInterval() {
+  if (turnSpeed <= 0) {
+    _stepInterval = 0.1; // Fallback to avoid div by zero
+    return;
+  }
+  // Calculate steps per second: speed / degrees_per_step
+  float stepsPerSecond = turnSpeed / DEGREES_PER_STEP;
+  _stepInterval = 1.0 / stepsPerSecond; // Seconds per step
+}
+
+void calculateStepsForAngle(float angle) {
+  // Determine direction
+  if (angle >= 0) {
+    _directionCW = true;
+  } else {
+    _directionCW = false;
+    angle = -angle; // Work with absolute value
+  }
+  
+  // Calculate steps
+  _stepCount = (int)(angle / DEGREES_PER_STEP);
+  
+  // Update speed interval immediately
+  updateStepInterval();
+  
+  // Reset state to start moving immediately
+  _currState = MOVE;
+  _stepsRemaining = _stepCount;
+  _stepTime = millis();
+  _currentStepIndex = 0;
+  
+  // Reset wait timer just in case
+  _waitTime = 0;
+}
+
+void runStep() {
+  int* sequence = _directionCW ? stepSequenceCW[0] : stepSequenceCCW[0]; // Placeholder logic
+
+  // Actually, we need to access the 2D array correctly
+  int (*seq)[4] = _directionCW ? stepSequenceCW : stepSequenceCCW;
+  
+  setStep(seq[_currentStepIndex][0], 
+          seq[_currentStepIndex][1], 
+          seq[_currentStepIndex][2], 
+          seq[_currentStepIndex][3]);
+          
+  _currentStepIndex++;
+  if (_currentStepIndex >= 4) {
+    _currentStepIndex = 0;
+  }
+}
+
+// --- Setup ---
+void setup() {
+  // Initialize pins
+  pinMode(IN1, OUTPUT);
+  pinMode(IN2, OUTPUT);
+  pinMode(IN3, OUTPUT);
+  pinMode(IN4, OUTPUT);
+  
+  // Initialize Serial
+  Serial.begin(9600);
+  while (!Serial) { ; } // Wait for serial port (optional for native USB)
+  
+  // Initial State Setup
+  autoMode = AUTO;
+  waitInterval = 8.0;
+  turnSpeed = 5.0;
+  _currState = MOVE;
+  
+  // Initialize timers to current time to prevent immediate first step
+  _stepTime = millis();
+  _waitTime = millis();
+  
+  // Initial calculation (move 0 degrees to set defaults)
+  calculateStepsForAngle(0.0); 
+  // Ensure we start in MOVE if we want immediate action, or WAIT if idle
+  // Based on your logic: "initialized in MOVE state"
+  _currState = MOVE;
+}
+
+// --- Loop ---
+void loop() {
+  // --- READ SERIAL ---
+  if (Serial.available()) {
+    String command = Serial.readString();
+    command.trim(); // Remove whitespace
+    
+    if (command.length() < 2) return; // Need at least 'a' + number
+    
+    char cmdType = command.charAt(0);
+    String payload = command.substring(1);
+    
+    switch (cmdType) {
+      case 'm':
+        // 'm0' for AUTO, 'm1' for MANUAL
+        if (payload.length() > 0) {
+          char modeChar = payload.charAt(0);
+          if (modeChar == '0' && autoMode != AUTO) {
+            autoMode = AUTO;
+            _currState = MOVE;
+            _stepsRemaining = _stepCount;
+            _stepTime = millis();
+          } else if (modeChar == '1' && autoMode != MANUAL) {
+            autoMode = MANUAL;
+            _currState = MOVE; // Start moving immediately on switch
+            _stepsRemaining = _stepCount;
+            _stepTime = millis();
+          }
+        }
+        break;
+        
+      case 'a':
+        // 'a90' -> move 90 degrees
+        {
+          float angle = payload.toFloat();
+          calculateStepsForAngle(angle);
+        }
+        break;
+        
+      case 's':
+        // 's10' -> 10 deg/s
+        {
+          float speed = payload.toFloat();
+          if (speed > 0) {
+            turnSpeed = speed;
+            updateStepInterval();
+            // If currently in MOVE, the new interval applies on next step
+          }
+        }
+        break;
+        
+      case 'i':
+        // 'i5' -> 5 seconds wait
+        {
+          float interval = payload.toFloat();
+          if (interval >= 0) {
+            waitInterval = interval;
+          }
+        }
+        break;
+    }
+  }
+
+  // --- EXECUTION LOGIC ---
+
+  // 1. Check if move sequence is finished
+  if (_currState == MOVE && _stepsRemaining == 0) {
+    _currState = WAIT;
+    _waitTime = millis();
+  }
+
+  // 2. Check if we should restart in AUTO mode
+  if (_currState == WAIT) {
+    if (autoMode == AUTO && (millis() - _waitTime > (unsigned long)(waitInterval * 1000))) {
+      _currState = MOVE;
+      _stepsRemaining = _stepCount;
+      _stepTime = millis(); // Reset step timer to prevent burst
+    }
+    // If MANUAL: Stay in WAIT indefinitely until new command
+  }
+
+  // 3. Execute Step if in MOVE mode
+  if (_currState == MOVE) {
+    unsigned long currentTime = millis();
+    if (currentTime - _stepTime > (unsigned long)(_stepInterval * 1000)) {
+      runStep();
+      _stepTime = currentTime;
+      _stepsRemaining--;
+    }
+  }
+}