search

SkyentificGit / SmallRobotArm

This is the open source project of the 6DoF robot arm based on stepper motors
GNU General Public License v3.0
1.24k stars 380 forks source link

goStrightLine function #6

Open yangtfu opened 4 years ago

yangtfu commented 4 years ago 
 //
  float lmax = max(abs(xff[0]-xfi[0]),abs(xff[1]-xfi[1]));
  lmax = max(lmax,abs(xff[2]-xfi[2]));
  lmax = max(lmax,abs(xff[3]-xfi[3]));
  lmax = max(lmax,abs(xff[4]-xfi[4]));
  lmax = max(lmax,abs(xff[5]-xfi[5]));
  unsigned long preMil = micros();
  double l = 0.0;
  vel0 = min(vel0,sqrt(lmax*acc0+0.5*velini*velini+0.5*velfin*velfin));
  unsigned long curMil = micros();
  unsigned long t = 0;
  double tap = vel0/acc0-velini/acc0;
  double lap = velini*tap+acc0*tap*tap/2.0;
  double lcsp = lmax-(vel0*vel0/2.0/acc0-velfin*velfin/2.0/acc0);
  double tcsp = (lcsp-lap)/vel0+tap;
  double tfin = vel0/acc0-velfin/acc0+tcsp;

It's a bit confusing. Are you using trapezoidal velocity profile here? Vini is initial velocity, V0 is constant velocity and Vf is final velocity, it seems that calculation is wrong to me.

gattarin commented 3 years ago

Hi! I would love to know that as well. Thanks! Gat