Firmware

[electric_vehicle_lover]

@chriscalandro, I am using a ~10ms loop and counting the DT using a function that measures in micro seconds.

I am not doing average yet but I will try it. Do you think I also should lower the 10ms loop time??

[vezu4iy]

it must be 20 ms

[chriscalandro]

Looking into this a bit more it seems you are trying to use the value from the IMU directly.

While in theory this is the right idea, in practice it is not.  A PID controller is required to drive the pwm of the motor.

You should use the output of the PID to drive the motor rather than the gyro value itself.  using the gyro itself will cause the motor to overcompensate and you end up with this out of control thing.

 

 

 

 

 

[electric_vehicle_lover]

@chriscalandro In my current code (on github), after the IMU angle calc, I use the complementary filter. I will do also some average/low pass filter and test.

I am now starting playing with PID control :-)

 

[vezu4iy]

Basic

 

Edited February 18, 2016 by vezu4iy

[chriscalandro]

The loop should run as fast as you can get it to make it most accurate.  I asked about frequency to see if you had a measurement.

 

This is easy enough with a spare GPIO pin.  just trigger it on and then off at the beginning of the main loop, put it on a scope, and measure it to see how fast the loop is.

 

Stand by - I might have something for you here...  Just need to dig it up

[chriscalandro]

Here is something I have been using and adapting for a balancing desk chair I have been making.  *don't watch for it, I don't have time to make it...  ) :

 

This might get you close - obvously you will have to adjust the values.

pid.c

I think if you can get this implemented you will be most of the way there!

Edited February 18, 2016 by chriscalandro

[esaj]

There's a good series of articles here (the link to the next article is always at the top of the page):

http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/

While it's called "beginners PID", the end result is still pretty versatile & robust.

[chriscalandro]

I'm not clear on how "value" works.  I'm probably missing something, but you have it as a static int, The ADC reading gets sent to "value", but if it's static I don't think that will work.

Seems like value should come from the PID and it should be volatile, not static.

 

static would mean it never changes...

Edited February 18, 2016 by chriscalandro

[esaj]

31 minutes ago, chriscalandro said:

static would mean it never changes...

Could be wrong, as I haven't worked with C/C++ for about seven years, but I recall that static-variables inside a function body in C just "remember" (hold) their values between calls, ie. it's not stored in the stack but heap... could be I remember wrong though. A constant value is a different matter.

Edited February 18, 2016 by esaj

[chriscalandro]

It's inside a the main function though, it's the first line of code before the while loop.

 

int main(void)
{
  static int value;

  initialize();

  init_printf(NULL,putc);

  motor_start ();

  while (1)

Edited February 18, 2016 by chriscalandro

[esaj]

Can't say I recall seeing a static-variable used inside main() anywhere, but in this I'm not sure if it has much effect (the main() is only called once when the board powers up), except of course where it's stored (in heap instead of stack). It shouldn't be visible anywhere else, unless an address to the variable is passed as a pointer somewhere... I haven't looked at the code that much, from a quick glance, it looks like there's a commented out -section where the value of that variable (not the variable itself) was passed to  motor_set_duty_cycle .

[electric_vehicle_lover]

I use static and the compiler will make the variable to keep always the value (sometimes volatile also works). I would avoid this static and volatile for the final code, for being optimized for size, which is not the case right now.

I just push my latest code: https://github.com/generic-electric-unicycle/firmware/blob/master/src/

I decided to avoid the complementary filter and use a average filter as @chriscalandro did mention:

  angle = atan2(acc_x, acc_z); //calc angle between X and Y axis, in rads
  angle = (angle + PI) * RAD_TO_DEG; //convert from rads to degres
//  angle = 0.98 * (angle + (gyro_rate * dt)) + 0.02 * (acc_y); //use the complementary filter.

  angle = (0.25 * angle) + (0.25 * old_angle1) + (0.25 * old_angle2) + (0.25 * old_angle3);
  old_angle1 = angle;
  old_angle2 = old_angle1;
  old_angle3 = old_angle2;

And it seems to work well!!

Then the PID code:

  //printf ("a %3.2f", angle);

  current_error = angle - INITIAL_ANGLE; //error
  sum_error += current_error;

  if (sum_error > SUM_ERROR_MAX) sum_error = SUM_ERROR_MAX;
  else if (sum_error < SUM_ERROR_MIN) sum_error = SUM_ERROR_MIN;

  //Ki*SumE/(Kp*Fs*X)
  integral_term = sum_error * dt * KI / KP * 10.0; // 0.0005
  derivative_term = current_error - previous_error;

  if(derivative_term > 0.1) derivative_term = 0.1;
  else if (derivative_term < -0.1) derivative_term = -0.1;

  // Kd(curErr-prevErr)*Ts/(Kp*X)
  derivative_term = derivative_term * KD * dt / KP;

  if(derivative_term > 120) derivative_term = 120;
  else if (derivative_term < -120) derivative_term = -120;

  duty_cycle = (current_error + integral_term + derivative_term);
  duty_cycle *= KP;

  motor_set_duty_cycle ((int) duty_cycle);

  //printf ("d %3.2f\n", duty_cycle);

  previous_error = current_error;

That also seems to do the calcs correctly (I did some debug to verify).

The final result is that it does not work. But I also found that with duty-cycle of 0, the motor does not have torque, is more like if the torque starts when duty-cycle is 150 and -150, meaning that there is a [-150, 150] gap. I did code to subtract but still not working as I expected...

if (value > 0) value += 150;
if (value < 0) value -= 150;

I also want to look at the loop time, that I expect to be 10ms, but maybe is much more larger... on the loop, there is code to read the IMU by SPI by pooling and there is after some math...

Here are 2 videos I recorded showing the current code:

 

And after apply this code:

if (value > 0) value += 150;
if (value < 0) value -= 150;

 

Edited February 18, 2016 by electric_vehicle_lover

[esaj]

Just a quick note:

  old_angle1 = angle;
  old_angle2 = old_angle1;
  old_angle3 = old_angle2;

You're storing the value of "angle" to all the old_angle* -variables, if angle is 1, old_angle1 becomes 1, then CURRENT value of old_angle1 is stored to old_angle2 (again 1), then the current value of old_angle2 is stored to old_angle3, and they're all at the same value. Reverse the order:

  old_angle3 = old_angle2;

  old_angle2 = old_angle1;

  old_angle1 = angle;

 

[electric_vehicle_lover]

@esaj thanks. The reaction of the motor is now a bit less "strong" but the end result is similar :-(

[esaj]

6 minutes ago, electric_vehicle_lover said:

@esaj thanks. The reaction of the motor is now a bit less "strong" but the end result is similar :-(

Did it start acting like that after you changed the angle-calculation from complementary filter to averaging? It might be that the averaging is "too fast" and it overreacts to sudden changes in angle. Or did you change something else too (the PID-code?)?

Edited February 18, 2016 by esaj

[chriscalandro]

edit to change - There should be 2 PID controllers.  One for angle and one for speed.

The speed controller is probably more important.

Currently your speed (pwm) control is overcompensating.  just like PID helped even out your gyro reading, Speed PID controller will solve that motor overcompensation as well.

Start with low gain values - which should make the motor adjust pitch verry sluggishly.  increase slowly until stiff and stable.

Have another look at my example I posted.  I have tested it and it works.

 

Were pretty close here!

Edited February 19, 2016 by chriscalandro

[vezu4iy]

Control must be slow

 

as you can see, board change angle and speed slow, 

to avoid destabilization

sorry for my english

 

[vezu4iy]

about stability

http://prntscr.com/a57xd2

[chriscalandro]

"Slow" is not the correct answer.  I have a strong feeling PID for the pwm control will stabilize the unit with the correct gain values.

 

  // for delay_ms ()
  _ms++;

  // For IMU reading task
  timer_imu++;
  if (timer_imu > 9)
  {
    timer_imu = 0;
    read_imu_flag = 1;

 

If I am reading this correct you are reading the IMU every 10ms.  This is too slow.  You should read and calculate as fast as possible.

Faster = more resolution = better control.

 

@vezu4iy - I'm sorry you're notes are incorrect.  Unfortunately comments along the lines of "That's good, but can you do it better" are not very helpful..."

Edited February 19, 2016 by chriscalandro

[vezu4iy]

You dont understand. I want help

faster = more unstability = worse control

[chriscalandro]

The speed in which your unit achieves balance has absolutely nothing to do with what we are discussing.

 

The more samples that are taken, and the faster everything is calculated, the more stable the unit.

[Tomek]

i think @vezu4iy has good intentions, but introduces a lot of noise to this otherwise amazing discussion

[chriscalandro]

So where are we at? Have you tried pid for speed as well?

 

I'd help more but I don't have a board/generic wheel. 

[electric_vehicle_lover]

@BerusBer

Criscalandro I need more time to think what I can do. For example, maybe I need to use DMA to read IMU data só the controle loop is faster. I really need to have more time to think and understand/feel the EUC, feel how it works.

If you want to buy the wheel and the board to develop, Just ask and I will provide you with all the links.

Edited February 20, 2016 by electric_vehicle_lover