UGV با موتورهای پله‌ای — ناوبری دقیق در فضای بسته بدون GPS (فارسی)

محاسبات Odometry

کد حرکت به مختصات

#define STEP_L 3
#define DIR_L  4
#define STEP_R 5
#define DIR_R  6

const float MM_PER_STEP = 0.098;  // mm/قدم با 1/16 microstep
const float WHEEL_BASE  = 200.0;  // mm بین دو چرخ

float posX=0, posY=0, heading=0;  // mm, mm, degrees

void stepBoth(long stepsL, long stepsR, int delayUs=800){
  long maxSteps = max(abs(stepsL), abs(stepsR));
  for(long i=0; i0?HIGH:LOW);
      digitalWrite(STEP_L,HIGH); delayMicroseconds(delayUs/2);
      digitalWrite(STEP_L,LOW);
    }
    if(i0?HIGH:LOW);
      digitalWrite(STEP_R,HIGH); delayMicroseconds(delayUs/2);
      digitalWrite(STEP_R,LOW);
    }
    delayMicroseconds(delayUs/2);
  }
}

void moveMM(float dist){
  long steps = (long)(dist/MM_PER_STEP);
  stepBoth(steps,steps);
  posX += dist*cos(heading*PI/180);
  posY += dist*sin(heading*PI/180);
}

void turnDeg(float deg){
  float arc = (deg/360.0)*PI*WHEEL_BASE;
  long steps = (long)(arc/MM_PER_STEP);
  stepBoth(steps,-steps);  // چپ جلو، راست عقب = چرخش چپ
  heading += deg;
}

• کتابخانه StepperDriver — GitHub
• آپارات: موتور پله‌ای ربات