Help for implementing my own Gimbal controller

alex934mas · Feb 2, 2015

Hi everyone,
I'm currently studying in engineering school and I try to make my own gimbal as the Gimbal Brushless Controller by Martinez. It's just a challenge for me to design my own gimbal, mechanical, PCB and software.

I got some questions that no one can answer around me, so I hope that there are matinez's users who can help me here...

Firstly I made my PCB as martinez I use driver L6234 to drive my motors ans use a MPU6050 to get the angle, the différence is that I use a PSoC instead of ATMEGA... :

To control my motor I made a array of 256 PWM's values (from 0 to 255 =>8bits). I send to each motors's phase a value, dephase of 83values (120°). If I test, my motors turns well...
I get the value of the MPU from I2C and make a "kalman filter light" It takes around 1ms to get my final value x,y and z

Now I would like to make my control, but my tests doesn't work very well.

The PWM frequency is around 31250HZ while I reload the pwm value each 500Hz in interrupt.
In another interrupt, I load my MPU value and make my filter, each 100Hz
I would like to make my PID in my main, but It doesn't work very well, my motor is not smooth or it unstable.

And I'm not sure about my PID, cause my array are INT type, and when I dot my PID I do

Code:

 err=consigne-pitch; //or roll dep
    
    errDif = err - lastError;
    lastError = err;
    if (err>debIntergr) {
				integr=0;
			} else {
				integr += err;
			}
    increment=err*Kp + integr * Ki+ errDif * Kd ;
    
    if (increment> maxIncr)  
    {increment=maxIncr;
    }
    if (increment< -maxIncr) 
    {increment=-maxIncr;}
    
 
    
    currentStepA = currentStepA - increment; // THE PWM VALUE FOR PHASE 1 MOTOR
    if(currentStepA > 255) currentStepA = 0;
    if(currentStepA<0) currentStepA =255;
   
    currentStepB = currentStepB - increment; // THE PWM VALUE FOR PHASE 1 MOTOR
    if(currentStepB > 255) currentStepB = 0;
    if(currentStepB<0) currentStepB =255;
   
    currentStepC = currentStepC - increment;
    if(currentStepC > 255) currentStepC = 0;
    if(currentStepC<0) currentStepC =255;

I know that I'm not far away from a good result.

So what do you think of my approach and my algorythm?
Should I make my PID in an interrupt, should I make everything at 500Hz?

Thank you for your help

infinity · Feb 2, 2015

Hey there,

first of all: Depending on your Sensor, you cannot refresh in a 500Hz cycle because the sensor has an update value of 100Hz.
Also, a Kalman Filter isn't the best for IMU processing as it is quite cpu heavy. Maybe a simple complementary filter will work?
It's just calculating the angle by using gyro data and fusion it with the angle by the accelerometer by adding these like this:

Code:

angle_x = angle_x_gyro * (1-acc_infuence) + angle_x_acc *acc_influence

This will work if the acceleration sum ( |a| ) is less than 12 m/s². This threshold worked fine in my last tests.

Apart from that, I can't help you much more because I haven't got the chance to dive into gimbal algorithms.

Cheers

alex934mas · Feb 3, 2015

Yep thank you infinity, I will reduce my scanning frequency. For the filter, I already make a Kalman "light" which is lighter than a real Kalman:
http://www.pieter-jan.com/node/11

Thank you

ahahn · Feb 10, 2015

Hi Alex,

just to make sure, w/o knowing the details.

Is the update sequence of the PWM counters clean?
I mean, is the update sequence of the PWM counter values atomic, so that it can not be interrrupted in an intermediate state. e.g. if one phase would be updated where as the other is still in the previous state.
In this case you would have an error at the motor phase for some time.
A similiar thing might happen if you do not update synchronously to the PWM frequency, one counter could update its counter register, wheras the other updates one full period later

regards
Alois

alex934mas · Feb 10, 2015

Hey alois, thank you for your answer,
actually I don't really understand, what do you means by "PWM counter values atomic,"
In global variable I defined my array of 255 values of 8bit pwm (0-255) :

Code:

 int pwmSinMotor[255]={127,124,121,118,115,111,108,105,102,99,96,93,90,87,84,81,78,76,73,70,67,64,62,59,56,54,51,49,46,44,42,39,37,35,33,31,29,27,25,23,21,20,18,16,15,14,12,11,10,9,7,6,5,5,4,3,2,2,1,1,1,0,0,0,0,0,0,0,1,1,1,2,2,3,4,5,5,6,7,9,10,11,12,14,15,16,18,20,21,23,25,27,29,31,33,35,37,39,42,44,46,49,51,54,56,59,62,64,67,70,73,76,78,81,84,87,90,93,96,99,102,105,108,111,115,118,121,124,127,130,133,136,139,143,146,149,152,155,158,161,164,167,170,173,176,178,181,184,187,190,192,195,198,200,203,205,208,210,212,215,217,219,221,223,225,227,229,231,233,234,236,238,239,240,242,243,244,245,247,248,249,249,250,251,252,252,253,253,253,254,254,254,254,254,254,254,253,253,253,252,252,251,250,249,249,248,247,245,244,243,242,240,239,238,236,234,233,231,229,227,225,223,221,219,217,215,212,210,208,205,203,200,198,195,192,190,187,184,181,178,176,173,170,167,164,161,158,155,152,149,146,143,139,136,133,130};

and in my 500Hz interrupt I come change my pwm:
FOR THE ROLL:

Code:

errRoll=consigne-roll;
   
    if (abs(errRoll)>1) {
				integrRoll=0;
			} else {
				integrRoll += errRoll;
			}    
    errDifRoll = errRoll - lastErrorRoll;
       
    incrementRoll=errRoll* KpRoll + errDifRoll * KiRoll + integrRoll * KdRoll ;
    incre=(int)incrementRoll;


    currentStepA = currentStepA - incre;
    if(currentStepA > 255) currentStepA = 0;
    if(currentStepA<0) currentStepA =255;
   
    currentStepB = currentStepB - incre;
    if(currentStepB > 255) currentStepB = 0;
    if(currentStepB<0) currentStepB =255;
   
    currentStepC = currentStepC - incre;
    if(currentStepC > 255) currentStepC = 0;
    if(currentStepC<0) currentStepC =255;
         
    
    PWM_1_WriteCompare1(pwmSinMotor[(int)currentStepA]) ;
    PWM_2_WriteCompare1(pwmSinMotor[(int)currentStepB]);
    PWM_3_WriteCompare1(pwmSinMotor[(int)currentStepC]) ;

So normally each phase update at the same time right?

Thank you for everything

ahahn · Feb 10, 2015

These functions should be atomic.
PWM_1_WriteCompare1()
PWM_2_WriteCompare1()
PWM_3_WriteCompare1()
As they are called in the interrupt, you should be fine.

But still you should make sure, that the 500Hz interrupt is synchronous to the 31250Hz PWM rate.
It should never happen that the PWM hardware counters update their compare registers exactly during the same time when PWM_1_WriteCompare1() and PWM_3_WriteCompare1() are executed.

Imagine this sequence:
1. PWM_1_WriteCompare1() sets a new value StepA1
2. Hardware Counters PWM1, PWM2 and PWM3 do their update (at the 31250Hz rate)
3. PWM_2_WriteCompare1() sets a new value StepB1
4. PWM_3_WriteCompare1() sets a new value StepC1

In this case PWM1 would take the new value StepA1, but PWM2 and PWM3 would keep their old values.

Just as an idea. You could try some tests to localize the problem, if its either at the drive side or at the PID side.

E.g. drive the motor with a simple ramp, I think a constant value on incrementRoll would do the job, without PID. And observe if the motor runs smooth.

Alois

alex934mas · Feb 20, 2015

Thank you ahahn,
I tryed to synchronized my 500hz with the 31250Hz, I put an interrupt from the pwm block and increment a variable, and in my main i did:

Code:

for(;;)
    {
    
    while(updateCount<224);
    updateCount=0;
    errRoll=consigne-roll;
   
    if (abs(errRoll)>10) {
				integrRoll=0;
			} else {
				integrRoll += errRoll;
			}    
    errDifRoll = errRoll - lastErrorRoll;
       
    incrementRoll=errRoll*0.1 + errDifRoll * 0.05 + integrRoll * 0.005 ;
    incre=(int)incrementRoll;

    currentStepA = currentStepA - incre;
    if(currentStepA > 255) currentStepA = 0;
    if(currentStepA<0) currentStepA =255;
   
    currentStepB = currentStepB - incre;
    if(currentStepB > 255) currentStepB = 0;
    if(currentStepB<0) currentStepB =255;
   
    currentStepC = currentStepC - incre;
    if(currentStepC > 255) currentStepC = 0;
    if(currentStepC<0) currentStepC =255;
    
    
    PWM_1_WriteCompare(pwmSinMotor[(int)currentStepA]) ;
    PWM_2_WriteCompare(pwmSinMotor[(int)currentStepB]);
    PWM_3_WriteCompare(pwmSinMotor[(int)currentStepC]) ; 
}

I thought it was better but after some tests it's not really smooth enough...
You can look the result here:
http://youtu.be/4A89w6UqHNk

ahahn · Feb 21, 2015

Hi,

just a few comments.

you checked the math ? currentStepA, incre are signed integers ?
e.g. (currentStepA<0) would not work as expected for unsigned int.
currentStepA = currentStepA - incre;
if(currentStepA > 255) currentStepA = 0;
if(currentStepA<0) currentStepA =255;

under the assumption that the drive path is clean:
Check the controller polarity, or simply try to reverse the motor direction.
The controller might lock, even with wrong direction. I guess this could look like on your video.

Alois

infinity · Feb 21, 2015

Hi there,

It seems like the gimbal "locks" in position and jumps into the next when the angle difference is great enough.
I don't know your Algorithm, just had a quick look, but I think you have not enough steps for a smooth stabilisation.

alex934mas · Feb 22, 2015

ahahn hat gesagt.:

Well, that's a good question, I don't know what is the best type of variable.

Code:

incrementRoll=errRoll*0.1 + errDifRoll * 0.05 + integrRoll * 0.005 ;
    incre=(int)incrementRoll;

incrementRoll should be a float type, but after that I got:

Code:

 currentStepA = currentStepA - incre;
    if(currentStepA > 255) currentStepA = 0;
    if(currentStepA<0) currentStepA =255;
   
    currentStepB = currentStepB - incre;
    if(currentStepB > 255) currentStepB = 0;
    if(currentStepB<0) currentStepB =255;
   
    currentStepC = currentStepC - incre;
    if(currentStepC > 255) currentStepC = 0;
    if(currentStepC<0) currentStepC =255;
    
    
    PWM_4_WriteCompare(pwmSinMotor[(int)currentStepA]) ;
    PWM_5_WriteCompare(pwmSinMotor[(int)currentStepB]);
    PWM_6_WriteCompare(pwmSinMotor[(int)currentStepC]) ;

So incre should be an integer right?
I defined my PWM array as : int pwmSinMotor[]={127,124.....}; and I defined float currentStepA=0;
float currentStepB=85;
float currentStepC=170;

After thinkin, these truncations from float, to int, to float to int can be the problem right?

I really appreciate your help ahahn, thank you

infinity hat gesagt.:

Yep but I use 255 values of PWM as the majority of gimbal, normaly it should be enough to get a smooth rotation. I will try with more, thank you

alex934mas · Mrz 2, 2015

No one?

alex934mas · Mrz 11, 2015

ahahn where are you :'(

alex934mas · Mai 29, 2015

Well well well

Help for implementing my own Gimbal controller

alex934mas

Neuer Benutzer

infinity

Erfahrener Benutzer

alex934mas

Neuer Benutzer

ahahn

Erfahrener Benutzer

alex934mas

Neuer Benutzer

ahahn

Erfahrener Benutzer

alex934mas

Neuer Benutzer

ahahn

Erfahrener Benutzer

infinity

Erfahrener Benutzer

alex934mas

Neuer Benutzer

alex934mas

Neuer Benutzer

alex934mas

Neuer Benutzer

alex934mas

Neuer Benutzer