Command rate limit error?

[vamfun]

Looking at the code it appears you intend to limit the command change to < 100 per iteration before it is passed to the Pwm.

The structure seems to be

// ####### SET OUTPUTS (if the target change is less than +/- 100) ####### if ((cmdL > cmdL_prev-100 && cmdL < cmdL_prev+100) && (cmdR > cmdR_prev-100 && cmdR < cmdR_prev+100)) {

ifdef INVERT_R_DIRECTION

      pwmr = cmdR;
    #else
      pwmr = -cmdR;
    #endif
    #ifdef INVERT_L_DIRECTION
      pwml = -cmdL;
    #else
      pwml = cmdL;
    #endif
  }

... CmdL_prev = CmdL; CmdR_prev = CmdR;

This code doesn't perform a rate limit... If you put a step input cmd=1000. this is what I see happening.

Step cmd Pwm

1. 0. 0
2. 1000 0
3. 1000 1000
4. 1000 1000
5. 1000 1000...

Your if test only limits the command for one iteration.

I recommend you add a rate limiting function for 32 bit integers similar to the rateLimiter16(...) in utilities and also rate limit each command separately rather than && them together. I would not rate limit both commands just because one command is on a limit. E.g

//PwmL rateLimiter32(CmdL, 100,&PwmL) //PwmR rateLimiter32(CmdR, 100,&PwmR)

Then should see something like this

Step cmd Pwm

1. 0. 0
2. 1000 100
3. 1000 200
4. 1000 300
5. 1000 400...

Let me know if I'm out to lunch..and please explain what the 100 limit is trying to do. Thanks

[Candas1]

Hi,

As you have seen there is rateLimiter16 function that is used here for individual inputs before mixing it.

The IF statement you are referring to was there in the original repository which had no other rate limiting. I am not sure how useful it is now. Maybe Emanuel knows.

Is it generating issues for you ?

[vamfun]

This is an issue for me ... I'm making a servo with a bandwidth of say 4 hz and this limit does nothing but add a time lag of one iteration for large inputs..nada rate limiting. It adds about 7 deg of phase lag at 4 hz so it's not major but it is a concern since I'm adding an anti-aliasing analog prefilter and it chews up some available phase margin. It seems not a major issue for lower bandwidth users but it's a bug for me and will go. Perhaps I'll post it as an issue on NFauth repository if you agree with my analysis.

[EFeru]

The if statement is there from the original firmware. I think the intention with this if statement is to remove any big spikes from the input signal, but as it is placed after the filters it has no benefit in using it. So, it can be removed. I will do that in the next commit.

However, i find it strange that an if statement is a limiting factor for the bandwidth. That if statement creates a time delay at a nanoseconds level.

[vamfun]

I have simulated the code to test it... it is not the if statement execution nano sec , its the logic that causes a one step delay in the 200 hz loop. So any cmd changes > 100 have a 5 ms delay for no reason. And the code never provided any rate limit. A full step can occur immediately after a one iteration delay once cmd_previous is updated to the command. Perhaps if there was a data drop out for one iteration it had some benefit... but any spike that persists longer than one iteration will pass through with no attenuation on the second iteration.

[Candas1]

Hi Vamfun,

So I understand this if statement is not really useful for us as we are limiting the rate somewhere else anyway, and this is impacting you with your use case. If you have tested the code successfully without the IF statement, I can also test it with different use cases just in case, but based on your inputs it doesn't seem to do much anyway.

Still what I didn't understand is are you not using the real rate limiter with parameter RATE in config.h ?

EDIT: I would be interested if you a blog post about your project to better understand what you are working on

[vamfun]

Yes, I am running a straight PID loop that bypasses the limit. Here is the code // ############### PID CONTROL ################################

if defined PID_CONTROL

          // ####### MOTOR ENABLING: Only if the initial input is very small (for SAFETY) #######
  if (enable == 0 ) {    
    beepShort(6);                     // make 2 beeps indicating the motor enable
    beepShort(4); HAL_Delay(100);
    speed1Fixdt = speed2Fixdt = 0;      // reset filters
    enable = 1;                       // enable motors
    #if defined(DEBUG_SERIAL_USART2) || defined(DEBUG_SERIAL_USART3)
    printf("-- Motors enabled --\r\n");
    #endif
  }
    Motor_Pos( &MotorPosL, &MotorPosR); // Calculate motor electrical positions relative to startup positions
    // ####### LOW-PASS FILTER #######
  rateLimiter16(input1[inIdx].cmd , RATE, &speed1RateFixdt);
  rateLimiter16(input2[inIdx].cmd , RATE, &speed2RateFixdt);
  filtLowPass32(speed1RateFixdt >> 4, FILTER, &speed1Fixdt);
  filtLowPass32(speed2RateFixdt >> 4, FILTER, &speed2Fixdt);
  speed1 = (int16_t)(speed1Fixdt >> 16);  // convert fixed-point to integer
  speed2 = (int16_t)(speed2Fixdt >> 16);  // convert fixed-point to integer
    PIDL.input = speed1;//-input1[inIdx].cmd;
    PIDR.input = speed2 ;// input2[inIdx].cmd;
    #ifdef INVERT_R_DIRECTION
  PIDL.input = -PIDL.input1;       
#endif
#ifdef INVERT_L_DIRECTION
  PIDL.input1 = -PIDR.input1;
#endif       

    PIDL.feedback = (MotorPosL*2000)/5400; // scale to 2000 units per rotation   sf = .37 =2000/(360 deg*15pole pairs= 5400 elec deg)
    PIDR.feedback = (MotorPosR*2000)/5400;  //minimum step is 60 deg elec phase angle, or 4 deg mechanical angle
    PID(&PIDL);// left pid control
  //print_PID(PIDL);  
    PID(&PIDR);// right pid control
    //print_PID(PIDR);
    cmdL = CLAMP(PIDL.output,-1000,1000);
    cmdR = CLAMP(PIDR.output,-1000,1000);       
    pwml = cmdL;
    pwmr = cmdR;

As you can see I'm using cumulative electrical angle , eg MotorPosL computed by summing up electrical angle. I am currently testing with the rate limit opened up and the filter with a coefficient of 1 instead of .05. Still works fine except if I give it a strong command my motor angle running at 200 hz update cannot keep up with the electrical phase angle rate. So I need to move my cumulative motor routine to a faster loop. Perhaps you can suggest the best location. I can thinking of adding it to the BLDC loop. I did see mention of mechanical angle in BLDC but I believe its only as an alternate input from an outside position encoder.

Here is my electrical total angle calculation extern int MotorPosL; extern int MotorPosR; extern int motAngleLeft;
extern int motAngleRight; void Motor_Pos(){ static int motAngleLeftLast = 0; static int motAngleRightLast = 0;
static int cycleDegsL = 0 ; //wheel encoder roll over count in deg static int cycleDegsR = 0 ; //wheel encoder roll over count in deg int diffL = 0; int diffR = 0; static int cnt = 0; diffL = motAngleLeft - motAngleLeftLast ; //if(cnt % 25 == 0 ) {printf( " diffL :%i, diffR :%i , motAngleRightLast :%i ",diffL , diffR, motAngleRightLast );}

if( diffL < -180 ) { cycleDegsL = cycleDegsL + 360; } else if(diffL > 180) { cycleDegsL = cycleDegsL - 360;} MotorPosL = motAngleLeft + cycleDegsL;

diffR = motAngleRight - motAngleRightLast ;

if( diffR < -180 ) { cycleDegsR = cycleDegsR + 360; } else if(diffR > 180) { cycleDegsR = cycleDegsR - 360;} MotorPosR = motAngleRight + cycleDegsR; motAngleLeftLast = motAngleLeft; motAngleRightLast = motAngleRight; cnt++;

}

Sorry about my sketchy blog.... I've yet to do a good description of the project. Basically doing a drone project with the flying water jet device similar to jetlev. I.e. get rid of the man and use robotics to control the jets.

[Candas1]

ok so you code is custom now. I will check and remove the if statement from my side.

Yes the mechanical angle is an input in case you have an encoder, but you can use the ouput electrical angle. I myself had to cumulate the motor angle for one project and did it in bldc.c like this:

In my case it's a bit different as I cumulate the angle of the robot, not the wheel. But it was working fine.

[vamfun]

Yes I put my motor position routine in bldc and it works fine now. I am sort of half and half custom code... I'm trying to write it with other users in mind. I will start with a fresh fork and incrementally add my stuff back and maybe have something to push. I'm thinking there should be a "tank steering enable" that would allow users like me to drive separate speeds and then have a servo variant that uses hall position with a parameterized PID function that is fed by these inputs. If you use speed1 and speed2 in your rate limits and filter functions and then assign them to steer and speed folks that want to control their motors separately could easily set tank mode and still have the rate limit and filter without messing with the limit/filter code. But this is probably best discussed in another issue.

[Candas1]

Yes I have already thought about a tank mode/no mixing option, but I was working on something else. Need to have a look when I have time.

[vamfun]

Do you see much utility in having a parameterized PID servo variant based on hall encoders. For my project, I am hoping to get away with using the 4 degree resolution. But most applications probably need less than 1 deg resolution. The PID controller could be fed with external motor position for added resolution.

[Candas1]

If your code is on github I would be happy to try it

[Candas1]

I think this was released.