Servo motor S-Curve Motion Profile Based on Arduino UNO

การทดลองนี้เป็นการควบคุมเซอร์โวมอเตอร์ให้คลื่อนที่แบบ S-Curve Motion Profile โดยใช้บอร์ดควบคุม Arduino UNO เพื่อเป็นการประยุกต์ใช้งานสำหรับการเคลื่อนที่แบบ S-Curve Motion Profile ให้กับเซอร์โวมอเตอร์ ซึ่งก่อนหน้านี้เป็นการนำเนื้อหาเบื้องต้นโดยให้เอาต์พุตเป็นแอลอีดีเพื่อสังเกตการทำงาน สำหรับเนื้อหาในตอนนี้จะเป็นการนำผลลัพธ์ที่ได้มาควบคุมเซอร์โวมอเตอร์แทน เพื่อให้สามารถนำโปรแกรมคำสั่งจากการทดลองไปใช้งานอื่นๆ ได้ง่ายยิ่งขึ้น

รูปที่ 1 การต่อวงจรสำหรับจำลองการทำงานด้วยโปรแกรม tinkercad.com

ในรูปที่ 1 แสดงการต่อวงจรสำหรับจำลองการทำงานด้วยโปรแกรม tinkercad.com โดยในรูปจะใช้ขา D9 ในการจ่ายสัญญาณพัลซ์วิดธ์มอดูเลตชั่น สำหรับควบคุมแตำแหน่งการเคลื่อนที่ของเซอร์โวมอเตอร์ จากนั้นจะใช้ออสซิลโลสโคปมาวัดสัญญาณที่ขา D9 เช่นกันเพื่อสังเกตความสัมพันธ์ระหว่างการเคลื่อนที่ของเซอร์โวมอเตอร์กับสัญญาณพัลซ์วิดธ์มอดูเลตชั่น

// Servo motor S-Curve Motion Profile Based on Arduino UNO

#include <Servo.h>

Servo myservo;

// Motor Control Pins
const int pwmPin = 9;   //  PWM pin
const int pwmLED = 10;   //  PWM LED
// 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(pwmLED, OUTPUT);  
  Serial.begin(9600);
  myservo.attach(9);

  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

  Serial.print(currentPWM);
  Serial.print(", ");
  analogWrite(pwmLED, currentPWM);
 
  currentPWM = map(currentPWM, 0, 255, 0, 180);
  myservo.write(currentPWM); 
  Serial.println(currentPWM); // Uncomment to plot the profile
  delay(20); // Update speed every 10ms for smooth transitions
}

โปรแกรมควบคุมจะใช้จากบทความ Simple S-Curve Motion Profile Based on Arduino UNO [LEP] และนำมาปรับปรุงเพิ่มเติม สำหรับทดลองให้เซอร์โวมอเตอร์เคลื่อนที่จากตำแหน่ง 0 องศาไปยังตำแหน่ง 180 องศา (Acceleration Phase) จากนั้นให้คงที่ไว้ที่ตำแหน่ง 180 องศาตามที่กำหนด (Constant Speed Phase) เมื่อผ่านช่วงเวลาตำแหน่งคงที่ 180 องศาแล้ว เซอร์โวมอเตอร์จะเคลื่อนที่จากตำแหน่ง 180 องศามายังตำแหน่ง 0 องศาอีกครั้ง (Deceleration Phase) โดยจะสังเกตเห็นว่าตำแหน่งการเคลื่อนที่จะเป็นลักษณะสี่เหลี่ยมคางหมูต่อเนื่องกัน

คลิปวิดีโอแสดงความสัมพันธ์ระหว่างการเคลื่อนที่ของเซอร์โวมอเตอร์กับสัญญาณพัลซ์วิดธ์มอดูเลตชั่น

รูปที่ 2 แสดงการส่งค่ามุมควบคุมการเคลื่อนที่ 15 องศา

รูปที่ 3 แสดงการส่งค่ามุมควบคุมการเคลื่อนที่ 145 องศา

ในรูปที่่ 2 และรูปที่ 3 จะเป็นการทดลองให้เซอร์โวมอเตอร์เคลื่อนที่ไปที่่ตำแหน่ง 15 องศาและ 145 องศา ในกรอบสีแดงหน้าต่างสื่อสาร Serial Monitor โดยข้อมูลด้านซ้ายจะเป็นค่าเดิม 0-255 และข้อมูลด้านขวาจะเป็นข้อมูลคำสั่งให้เซอร์โวมอเตอร์เคลื่อนที่ 0-180 องศา

คลิปวิดีโอการต่อวงจรกับเซอร์โวมอเตอร์และบอร์ด Arduino UNO ในการทดลอง

สำหรับการทดลองควบคุมเซอร์โวมอเตอร์ให้เคลื่อนที่แบบ S-Curve Motion Profile คงพอจะเป็นความรู้เบื้องต้น สำหรับการนำไปประยุกต์ใช้งานระหว่างการเคลื่อนที่แบบ S-Curve Motion Profile ร่วมกับเซอร์โวมอเตอร์และตัวอย่างการเขียนโปรแกรมคำสั่งให้กับบอร์ดควบคุม Arduino UNO เพื่อให้ผู้อ่านสามารถนำไปพัฒนาโครงงานต่างๆ ได้ง่ายและรวดเร็วยิ่งขึ้น

Reference

Post Views: 35

Published by nattapon