AX12 control oddities

[Marmakoide]

Hi,

=== The intented goal ===
I'm trying to understand how to have an accurate control of a AX-12 from a PC with an USB2Dynamixel interface.

I want it to move as following
x(t)=0.5 * pi * sin(t)
where x(t) is the angle in radians and t is the time in seconds

=== My approach ===
I use a very naive approach. I repeat in an infinite loop the following instructions
1) Compute XGoal = x(T+K)
2) Read the current position XCurrent of the AX12
3) Compute V=|Xcurrent - XGoal| / K
4) Write the AX12 register to move to XGoal at speed V
5) K = time elapsed to do step 1) to 4)

When watching XGoal and XCurrent, XCurrent is always a bit late, being behind XGoal. A perfect control would achieve XGoal = XCurrent, up to a given error of course

=== My question ===
How do you, ladies and gentlemen, would do that very simple task ? A less naive approach ? My approach is just broken ?

[/i]

[Marmakoide]

Hi, ok, so the following pseudo-code is performing the intended goal, meaning that a very naive approach works well enough

Code: Select all

       lSamplingFreq = 0.05
   lUpdateDuration = 1 / lSamplingFreq // Rough initial estimate
   lPos = 0.0
   lGoalPos = 0.0
   lSpeed = 0.0
   lStartTime = lUpdateStartDate = lUpdateEndDate = getCurrentTime()

   while(1) {
      // Compute the next pos
      lT = lUpdateStartDate - lStartTime + lUpdateDuration
      lGoalPos = 0.5 * Constantsf::PI * sin(lT)

      // Read the current pos
      lPos = lServo.readPos()

      // Compute the speed
      lSpeed = abs(lPos - lGoalPos) / lUpdateDuration

      // Send the command
      lServo.moveTo(lGoalPos, lSpeed)
      sleep(1 / lSamplingFreq)

      // Compute update duration
      lUpdateEndDate = getCurrentTime();
      lUpdateDuration = lUpdateEndDate - lUpdateStartDate;
      swap(lUpdateEndDate, lUpdateStartDate);
   }


sleep => do nothing
Time => seconds
Angles => in radians
Speeds => in radians per second

The precision obtained is really nice, and what is nice is that code is sampling frequency independent.