Arduino go-goal robot angular position calculation -
i'm making differential drive robot needs go (0,0,90)to(100,180,90)with coordinates being x,y,phi. try position robot on trajectory intercepts goal.
for need position of robot:
float vr = 0.0; float vl = 0.0; float omega = 0.0; float r = 0.0; vr = (d / 2.0)*(2.0*pi / 200.0)*((float)right_delta * step_distance); vl = (d / 2.0)*(2.0*pi / 200.0)*((float)left_delta * step_distance); r = l*((vl + vr) / (vr - vl)); omega = (vr - vl) / (2.0*l); if (fabs(r) > 10000.0) { x_pos += (vr*step_distance)*cosf(theta); y_pos += (vr*step_distance)*sinf(theta); } else { x_pos += cosf(omega)*r*sinf(theta) + sinf(omega)*r*cosf(theta) - r*sinf(theta); y_pos += sinf(omega)*r*sinf(theta) - cosf(omega)*r*cosf(theta) + r*cosf(theta); theta += omega; }
when try y_pos , theta off...what causing this
Comments
Post a Comment