Differential Drive UGV: فرمول‌های حرکت، Odometry و کد آردوینو (فارسی)

کد Odometry با Encoder

// Encoder پولس‌شمار — هر موتور یک encoder ۲۰PPR
volatile long encL=0, encR=0;
void isrL(){ encL++; }
void isrR(){ encR++; }

const float WHEEL_D    = 65.0;   // mm قطر چرخ
const float WHEEL_BASE = 200.0;  // mm فاصله
const int   PPR        = 20;     // پالس در هر دور

float posX=0, posY=0, theta=0;   // mm, mm, radians

void updateOdometry(long dL_pulses, long dR_pulses){
  float dL = (dL_pulses/(float)PPR)*PI*WHEEL_D;
  float dR = (dR_pulses/(float)PPR)*PI*WHEEL_D;
  float v   = (dL+dR)/2.0;
  float dTh = (dR-dL)/WHEEL_BASE;
  posX  += v*cos(theta+dTh/2.0);
  posY  += v*sin(theta+dTh/2.0);
  theta += dTh;
}

void setup(){
  attachInterrupt(digitalPinToInterrupt(2), isrL, RISING);
  attachInterrupt(digitalPinToInterrupt(3), isrR, RISING);
}

void loop(){
  long snapshotL = encL; encL=0;
  long snapshotR = encR; encR=0;
  updateOdometry(snapshotL, snapshotR);
  Serial.print("X:"); Serial.print(posX,1);
  Serial.print(" Y:"); Serial.print(posY,1);
  Serial.print(" θ:"); Serial.println(theta*57.3,1);
  delay(50);
}

• Columbia: راهنمای ریاضی سینماتیک چرخ‌محور
• آپارات: encoder ربات آردوینو