Question

I’m trying to track a rotating object with an encoder using Matlab.
I have an encoder that gives me the angular position of the object, then I pass this information to a laser, but how can I move the laser at the same velocity of my object? Because now the laser is slower than the object and it does’t follow it very well!
I wrote this code, does anyone have any advice?

Alpha=pi/2
Positions = [];

t0 = clock;                                

while etime(clock,t0)<20;

  position = data.Position;   %here I read the angular position from the encoder
  Positions = [Positions position];
  A = [floor(Positions/360)];
  angPos = position-(floor(position/360))*360;
  AngPos = [Positions - A*360];
  angRad = angPos*pi/180;
  AngRad = [AngPos*pi/180];

  Angle = angRad+Alpha;
  thetaX = atan(cos(Angle).*(r/d)); 
  thetaY = atan(sin(Angle).*(r/d));

  VoltX=-(5/20)*thetaX*180/pi; 
  VoltY=(5/20)*thetaY*180/pi;    

  VoltageLimit = 10;

  if (max(abs(VoltX))>VoltageLimit) || (max(abs(VoltY))>VoltageLimit)
error('Voltages too large');
  end           

 session.outputSingleScan([VoltX VoltY]);   %here I send the laser the angular position obtained before 

 data = motorA.ReadFromNXT();

end 

I know that I should add the velocity, so I can move the laser with the same velocity of the object, but I don’t know how to give the laser that velocity value!!

sec = etime(clock,t0);
Sec = [Sec sec];

vel=(diff(Positions)/diff(Sec));
Vel = [Vel vel]; %(deg/s);

Maybe it’s a stupid question, but I’m pretty new in Matlab!
Thank you!!!

Answer 1

How about you keep track of the last n = 3 previous angular speeds in some vector omega = [] that is initialized empty before the loop is entered. You should also memorize a timestamp prev_t = [] of the previous iteration and also the previous angle prev_a = [] in order to do this.

In each iteration, you can update your guess for the current angular speed by managing the vector of the n previous observed speeds by

current_t = clock();
if ~isempty(prev_t)
  if length(omega) == n
    omega(1) = [];
  end
  delta_omega = Angle - prev_a;
  if delta_omega < 0
    delta_omega = delta_omega + 2 * pi; 
  end
  omega = [omega, delta_omega / etime(current_t, prev_t)];
end
prev_t = current_t;
prev_a = Angle;

The estimate for the angular speed could be some average of these n previous measurements, perhaps est_omega = median(omega) to be somewhat robust to outliers. The run-over correction for delta_omega assumes that the angle is reported between zero and two pi, and that the angle increases with rotation. You should adjust this correction if your angles behave otherwise.

Then, before you calculate VoltX and VoltY, you can use your estimate for the current angular speed to predict a corrected Angle = Angle + est_omega * dt;, where dt is your guess for the delay in your feedback loop. You could choose that empirically. est_omega should be calculated, and Angle be corrected only if length(omega) == n. You don’t want to calculate the median of an empty list.

Also, I see you calculate x- and y-voltages from some atan(sin(Angle)) or atan(cos(Angle)), respectively. I’m not sure if that is correct. I would believe that these voltages should be proportional to sin(Angle) or cos(Angle) without atan applied.

I would imagine that this solution will work nicely, but for what you are doing here there is a large body of control theory to draw from that may solve your problem in a more sophisticated way.