Simple S-Curve Motion Profile Based on Arduino UNO [LEP]

ในตอนสุดท้ายสำหรับบทความการเรียนรู้เกี่ยวกับการเคลื่อนที่รูปตัว S (S-curve Motion Profile) ซึ่งเป็นเทคนิคที่ใช้ในการควบคุมการเคลื่อนที่ของมอเตอร์ จะเป็นตัวอย่างของโปรแกรมคำสั่งควบคุมการเคลื่อนที่ของมอเตอร์อย่างต่อเนื่องแบบหมุนไปในทิศทางเดียว และการตัวอย่างการเคลื่อนที่ของมอเตอร์อย่างต่อเนื่องแบบหมุนสองทิศทาง (หมุนไปหน้าและหมุนกลับ) แล้วแสดงผลด้วยกราฟค่าตัวแปรสำหรับควบคุมมอเตอร์ (สัญญาณพัลซ์วิดธ์มอดูเลตชั่นขับมอเตอร์) เพื่อให้สามารถนำไปใช้ควบคุมการเคลื่อนที่ได้อย่างนุ่มนวล

// ** LAB1 : S-Curve Motion Profile for DC Motor using PWM **

// Motor Control Pins
const int pwmPin = 9;   //  PWM pin
// const int dirPin = 2;   // Direction pin

// Motion Parameters
const int V_MAX = 255;      // Maximum PWM duty cycle (0-255)
const long ACCEL_TIME = 1500; // Acceleration/Deceleration time in milliseconds (T_a)
const long HOLD_TIME = 2000;  // Constant velocity time in milliseconds (T_v)
const long MOVE_DURATION = ACCEL_TIME + HOLD_TIME + ACCEL_TIME; // Total move time

// Time tracking
unsigned long startTime = 0;
bool moveStarted = false;

void setup() {
  pinMode(pwmPin, OUTPUT);
  //pinMode(dirPin, OUTPUT);
  Serial.begin(9600);
  Serial.println("S-Curve PWM Demo Ready. Sending PWM pulses now...");
}

void loop() {
  if (!moveStarted) {
    // Start the move sequence
    // digitalWrite(dirPin, HIGH); // Set Direction
    startTime = millis();
    moveStarted = true;
  }

  // --- S-Curve Calculation ---
  unsigned long elapsedTime = millis() - startTime;
  int currentPWM = 0;

  if (elapsedTime <= ACCEL_TIME) {
    // Phase 1: Acceleration (Ramp Up)
    float ratio = (float)elapsedTime / ACCEL_TIME;
    // Sinusoidal ramp from 0 to 1. V(t) will be the integral of a sine wave.
    float smoothRatio = 0.5 * (1.0 - cos(ratio * PI)); 
    currentPWM = (int)(V_MAX * smoothRatio);

  } else if (elapsedTime <= (ACCEL_TIME + HOLD_TIME)) {
    // Phase 2: Constant Velocity (Hold)
    currentPWM = V_MAX;

  } else if (elapsedTime <= MOVE_DURATION) {
    // Phase 3: Deceleration (Ramp Down)
    unsigned long timeInDecel = elapsedTime - (ACCEL_TIME + HOLD_TIME);
    float ratio = 1.0 - ((float)timeInDecel / ACCEL_TIME);
    // Sinusoidal ramp from 1 back to 0.
    float smoothRatio = 0.5 * (1.0 - cos(ratio * PI)); 
    currentPWM = (int)(V_MAX * smoothRatio);

  } else {
    // Move Complete
    currentPWM = 0;
    moveStarted = false; // Reset for next move
    Serial.println("Move Complete. Stopping.");
    delay(2000); // Wait 2 seconds before repeating
  }

  // Apply the calculated PWM value
  analogWrite(pwmPin, currentPWM);
  Serial.println(currentPWM); // Uncomment to plot the profile
  delay(20); // Update speed every 10ms for smooth transitions
}

รูปที่ 1 แสดงผลค่าตัวแปร currentPWM หน้าต่าง Serial Plotter

สำหรับการทดลองที่ 1 เป็นตัวอย่างโปรแกรมควบคุมการเคลื่อนที่ของมอเตอร์แบบต่อเนื่องไปทิศทางเดียว โดยสัญาณควบคุมความเร็วด้วยพัลซ์วิดธ์มอดูเลตชั่น (ความละเอียด 8bit ช่วง 0-255) คล้ายกับการทดลองที่ 3 ในบทความตอนที่ 1 (www.electronicsdna.com/simple-s-curve-motion-profile-based-on-arduino-uno-ep1/) แต่จะเป็นโปรแกรมคำสั่งเดียว และเราสามารถปรับช่วงเวลาเร่งความเร็ว (Acceleration Phase), ช่วงความเร็วคงที่ (Constant Speed Phase) และช่วงลดความเร็ว (Deceleration Phase) ได้ตามที่ต้องการ ในรูปที่ 1 จะสังเกตเห็นว่าช่วงเวลาความเร็วคงที่จะมีระยะเวลาเพิ่มขึ้นเป็นรูปสี่เหลี่ยมคางหมู

// ** LAB2 : S-Curve Motion Profile for DC Motor (Forward and Reverse) **

// --- Motor Control Pins ---
// PWM Pin: Sets the speed (connect to Enable/PWM pin on driver)
const int pwmPin = 9;   // Must be a PWM pin (e.g., 3, 5, 6, 9, 10, or 11 on Uno)
// Direction Pins: Control direction (connect to IN1/IN2 on driver)
const int dirPinA = 2; 
const int dirPinB = 3; 

// --- Motion Parameters ---
const int V_MAX = 255;          // Maximum PWM duty cycle (0-255)
const long ACCEL_TIME = 1500;   // Acceleration/Deceleration time in milliseconds
const long HOLD_TIME = 2000;    // Constant velocity time in milliseconds
const long MOVE_DURATION = ACCEL_TIME + HOLD_TIME + ACCEL_TIME; // Total move time

// --- State Variables ---
unsigned long startTime = 0;
bool moveActive = false;
bool isForward = true; // Start direction

void setup() {
  pinMode(pwmPin, OUTPUT);
  pinMode(dirPinA, OUTPUT);
  pinMode(dirPinB, OUTPUT);
  Serial.begin(9600);
  Serial.println("S-Curve FWD/REV PWM Demo Ready.");
}

void loop() {
  if (!moveActive) {
    // --- Initialize New Move ---
    
    // 1. Set Direction
    if (isForward) {
      // Forward: e.g., IN1=HIGH, IN2=LOW
      digitalWrite(dirPinA, HIGH);
      digitalWrite(dirPinB, LOW);
      Serial.println("\n--- Starting FORWARD Move ---");
    } else {
      // Reverse: e.g., IN1=LOW, IN2=HIGH
      digitalWrite(dirPinA, LOW);
      digitalWrite(dirPinB, HIGH);
      Serial.println("\n--- Starting REVERSE Move ---");
    }

    // 2. Start Timing
    startTime = millis();
    moveActive = true;
  }

  // --- Execute S-Curve Calculation ---
  unsigned long elapsedTime = millis() - startTime;
  int currentPWM = 0;

  if (elapsedTime <= MOVE_DURATION) {
    // Determine the motion phase and calculate PWM
    if (elapsedTime <= ACCEL_TIME) {
      // Phase 1: Acceleration (Ramp Up)
      float ratio = (float)elapsedTime / ACCEL_TIME;
      // Sinusoidal function for smooth ramp: 0 to 1
      float smoothRatio = 0.5 * (1.0 - cos(ratio * PI)); 
      currentPWM = (int)(V_MAX * smoothRatio);

    } else if (elapsedTime <= (ACCEL_TIME + HOLD_TIME)) {
      // Phase 2: Constant Velocity (Hold)
      currentPWM = V_MAX;

    } else { // Deceleration phase
      // Phase 3: Deceleration (Ramp Down)
      unsigned long timeInDecel = elapsedTime - (ACCEL_TIME + HOLD_TIME);
      float ratio = 1.0 - ((float)timeInDecel / ACCEL_TIME);
      // Sinusoidal function for smooth ramp: 1 back to 0
      float smoothRatio = 0.5 * (1.0 - cos(ratio * PI)); 
      currentPWM = (int)(V_MAX * smoothRatio);
    }
  
    // 3. Apply the calculated PWM value
    analogWrite(pwmPin, currentPWM);    
    Serial.print("Time: "); Serial.print(elapsedTime/10); Serial.print(", PWM: "); Serial.println(currentPWM);

  } else {
    // --- Move Finished ---
    analogWrite(pwmPin, 0); // Ensure motor is completely stopped
    moveActive = false;      // Reset state
    isForward = !isForward;  // Toggle direction for the next move
    Serial.println("Move Complete. Waiting 1s before reversing.");
    delay(1000);             // Pause before the next move
  }

  delay(10); // Update PWM value every 10ms for smooth transitions
}

รูปที่ 2 แสดงผลค่าตัวแปร currentPWM หน้าต่าง Serial Plotter

การทดลองที่ 2 เป็นตัวอย่างโปรแกรมควบคุมการเคลื่อนที่ของมอเตอร์แบบต่อเนื่องสองทิศทาง โดยสัญาณควบคุมความเร็วด้วยพัลซ์วิดธ์มอดูเลตชั่น (ความละเอียด 8bit ช่วง 0-255) เช่นเดียวกับการทดลองที่ 1 และเราสามารถปรับช่วงเวลาเร่งความเร็ว (Acceleration Phase), ช่วงความเร็วคงที่ (Constant Speed Phase) และช่วงลดความเร็ว (Deceleration Phase) ได้ ในรูปที่ 2 จะสังเกตเห็นรูปกราฟ 2 แบบคือ 1 กราฟฟันเลื่อยแบบเชิงเส้นโดยจะนับจำนวนช่วง 0-5000 จำนวน (ในรูปจะแสดงในช่วง 0-500 เพื่อให้ดูเช้าใจได้ง่ายด้วยคำสั่ง Serial.print(elapsedTime/10);) โดยรูปฟันเลื่อยด้านซ้ายจะเป็นการควบคุมการเคลื่อนที่ไปหน้า (Forward) และรูปฟันเลื่อยด้านขวาจะเป็นการควบคุมการเคลื่อนที่หมุนกลับ (Reverse) ทั้งนี้รูปฟันเลื่อยทั้งสองจะทำงานสอดคล้องกับการเคลื่อนที่ของมอเตอร์ที่เป็นรูปสี่เหลี่ยมคางหมูในรูปกราฟแบบที่ 2 เส้นสีเหลือง

รูปที่ 3 การแสดงผลแอลอีดีสีเขียวให้ทราบทิศทางการหมุนไปหน้า (Forward)

รูปที่ 4 การแสดงผลแอลอีดีสีเขียวให้ทราบทิศทางการหมุนกลับ (Reverse)

ในรูปที่ 3 และรูปที่ 4 จะเป็นแสดงผลการทดลองที่เกิดขึ้นเมื่อนำโปรแกรมคำสั่งที่ 2 ไปจำลองการทำงานในเว็บไซต์ www.tinkercad.com และต่อแอลอีดีที่ตำแหน่งขา D2 และ D3 เพื่อสังเกตทิศทางการเคลื่อนที่ของมอเตอร์ โดยเมื่อแอลอีดีสีเขียวติดสว่างจะหมายความว่าทิศทางการหมุนไปหน้า (Forward) ดังในรูปที่ 3 และเมื่อแอลอีดีสีแดงติดสว่างหมายความว่าการเคลื่อนที่หมุนกลับ (Reverse) ดังในรูปที่ 4 นั้นเอง ท้ายนี้บทความการควบคุมการเคลื่อนที่แบบ S-curve Motion Profile โดยใช้บอร์ดควบคุม Arduino คงพอจะเป็นความรู้เบื้องต้นสำหรับใช้ในการทดลองควบคุมมอเตอร์แบบต่างๆ และในส่วนของทฤษฎีการออกแบบและการทำงานแนะนำเว็บไซต์อ้างอิงตามลิ้งก์ท้ายบทความนี้ครับ.

Reference

Post Views: 29

Published by nattapon