Wheel rate control - tuning problem

Hello,

I have a problem with WRC (Wheel rate control) on the skid steer rover. AruRover 4.1.5, CUAV V5+. When I try to tune the PID, pidachive is very small, like 0.01 [rad/s], and piddesire is 10[rad/s]. I think it should be more like 1000 times larger.The encoder parameter is good because the distance is similar to what I measure with a ruler. Also pidachive is with inverted sign. Is there any logs for pid WRC in bin file?
Second problem: when I set WRC_RATE_MAX to 10[rad/s], the output PWM only changes from 60 to -60, not in the full range 500 - (-500). Changing WRC_RATE_MAX to 50 fixes this problem, but it is not physically possible to achieve this rate.
I check the code of ardurover, and it should work. I try to do some tests, eg. constant throttle and hold my rover to check if PWM will rise, but nothing happens. I am out of idea. I attache two bin file, one with WRC_RATE_MAX 10 and second with WRC_RATE_MAX 50. GCS_PID_MASK set to 8 (left wheel)

Kind Regards,
Michal

WRC_RATE_MAX 10:


WRC_RATE_MAX 50:

Logs: ardu - Google Drive
10.tlog (265.1 KB)
50.tlog (294.6 KB)

Upgrade to AruRover 4.2 don’t solve the problem :frowning:

I try to debug the problem. It seems that it is to small change dist_count_change compared to dt_ms. In my case it is 40 to 20[s]. It is small because it set full throttle and it should be something like 4’200 in 1[s], because I have 2’800 CPR and max angular velocity of 10[rad/s]. At this moment don’t have any idea why it is so small. Distance is calculated correctly and I turn off LUA script. Can someone help me with this problem ?
act_rate
dist_count_change
distance
dt

Finally I found it :smiley:
There is error in code of Encoder. In file WheelEncoder_Quadrature.cpp in function


void AP_WheelEncoder_Quadrature::irq_handler(uint8_t pin,
                                             bool pin_value,
                                             uint32_t timestamp)
{
    // sanity check
    if (last_pin_a == 0 || last_pin_b == 0) {
        return;
    }

    // update distance and error counts
    if (pin == last_pin_a) {
        last_pin_a_value = pin_value;
    } else if (pin == last_pin_b) {
        last_pin_b_value = pin_value;
    } else {
        return;
    };
    update_phase_and_error_count();

    // record update time
    irq_state.last_reading_ms = timestamp;
}

When timestamp is copied it is not ms but us. So it should be:

    irq_state.last_reading_ms = timestamp*1e-3f;

I will test this tomorrow and write the result but it should be this. In the first post i mention that result is 1’000 times smaller so it will be good now. After test I will set pull request in github.

1 Like

I check and fix work well.

Topic to close.

1 Like