Let's Make Robots!

PIC 16F876A - Self balancing robot

Hi everyone,

I'm using a PIC16F876A to do a self balancing robot. But I'm having a problem and I don't know more what I can do to solve this.

In addition to PIC I'm using one accelerometer and one gyroscope supplied by sparkfun in IMU combo board: 

http://www.sparkfun.com/products/9250

To simplify the accelerometer(ADXL320) when perpendicular to the earth surface will output 2.5V. For every gravity acceleration it's possible to see 312mV change on the 2.5V

The gyroscope(ADXRS401) will output 2.5V while sitting still. When the module is rotated the output will be 15mv/º/s.

My problem is that my robot can not stay in balance, how can you see in the video (I will put the video next week).

Can anyone see a problem in my PIC code?

Below my PIC code:

#include <16F876A.H>

#fuses XT, NOWDT, NOPROTECT, NOBROWNOUT, PUT, LVP

#device adc=10

#include <Math.H>

#use delay(clock = 4000000) 

#include "lcd.c"

#include "ADXL320.c"

#include "ADXRS401.c"

#define btn_more    PIN_C4

#define btn_less    PIN_C5

#define btn_change  PIN_C6

#define btn_run     PIN_C7

int16 returnADXL320;

int32 returnADXRS401;

float returnADXL320Angle;

float returnADXRS401Angle;

float returnPID = 512;

int32 pwm;

float sum;

float a = 0.96;

float angle = 0.0;

float dt = 0.020;

float proportional = 10;

float derivative = 10;

float zeroG;

float zeroS;

SIGNED int16 ADXRS401_read_axis ()

{

   UNSIGNED int16 readADXRS;

   delay_us(10);

   set_adc_channel (1);

   delay_us (10);

   readADXRS = read_adc ();

   delay_us (10);

   readADXRS += read_adc ();

   delay_us (10);

   readADXRS += read_adc ();

   delay_us (10);

   readADXRS += read_adc ();

   return readADXRS/4;

}

float zeroSensorS()

{

   int16 sum = 0;

   int8 i = 0;

   delay_us(10);

   set_adc_channel (1);

   delay_ms (10);

   for(i = 0; i<50; i++)

   {

      sum += read_adc ();

      delay_ms (10);

   }

   sum = sum/50;

   return (sum/1024.0)*5;

}

float ADXRS401_write_speed(int16 y, float zeroS)

{

   float volt = 0;

   float ydiff = 0;

   float yg = 0;

   volt = (y/1024.0)*5;

   ydiff = zeroS - volt;

   yg = ydiff/0.015;

   return yg;

SIGNED int16 ADXL320_read_axis ()

{

   UNSIGNED int16 readADXL;

   delay_us(10);

   set_adc_channel (0); 

   delay_us (10);

   readADXL = read_adc ();

   delay_us (10);

   readADXL += read_adc ();

   delay_us (10);

   readADXL += read_adc ();

   delay_us (10);

   readADXL += read_adc ();

   return readADXL/4;

}

float zeroSensorG()

{

   int16 sum = 0;

   int8 i = 0;

   //SETUP_ADC_PORTS (AN0_VREF_VREF);

   delay_us(10);

   set_adc_channel (0);

   delay_ms (10);

   for(i = 0; i<50; i++)

   {

      sum += read_adc ();

      delay_ms (10);

   }

   sum = sum/50;

   return (sum/1024.0)*5;

}

float ADXL320_write_angle(int16 y, float zeroG)

{

   float volt = 0;

   float ydiff = 0;

   float yg = 0;

   float radian = 0;

   float degree = 0;

   volt = (y/1024.0)*5;

   ydiff = zeroG - volt;

   yg = ydiff/0.312;

   radian = asin(yg);

   degree = (180/PI)*radian;

   return degree;

}

void initialize(void)

{

   SETUP_CCP1 (CCP_PWM);

   SETUP_TIMER_2 (T2_DIV_BY_1, 255, 1);

   SETUP_SPI (SPI_SS_DISABLED);

   SETUP_COMPARATOR (NC_NC_NC_NC);

   SETUP_ADC_PORTS (ALL_ANALOG);

   //SETUP_ADC_PORTS (AN0_VREF_VREF);

   SETUP_ADC (ADC_CLOCK_DIV_32);

   DISABLE_INTERRUPTS (GLOBAL);

}

void execution()

{

  //AD return

   returnADXL320 = ADXL320_read_axis();

   //returnADXL320Angle wil receive the angle in º

   returnADXL320Angle = ADXL320_write_angle(returnADXL320, zeroG);

   //AD return

   returnADXRS401 = ADXRS401_read_axis();

   //returnADXRS401Angle will receive the rotational speed in º/s

   returnADXRS401Angle = ADXRS401_write_speed(returnADXRS401, zeroS); 

   //Complementary filter

   angle = (a * (angle + (returnADXRS401Angle * dt))) + ((1 - a) * returnADXL320Angle);

   sum = (angle * proportional) + (returnADXRS401Angle * derivative);

   returnPID = returnPID + sum;

   if(returnPID <= 30)

      pwm = 30;

   else{

      if(returnPID >= 1000)

         pwm = 1000;

      else

      {

         if(returnPID > 30 && returnPID < 1000)

            pwm = returnPID;

      }

   }

   SET_PWM1_DUTY(pwm);

}

void main() 

   Initialize();

   //sensor resets

   zeroG = zeroSensorG();

   zeroS = zeroSensorS();

   while(1)

   {

    execution(); 

    delay_ms(20);

   }

 

 

Comment viewing options

Select your preferred way to display the comments and click "Save settings" to activate your changes.

At a glance the code seems reasonable, but have you tuned the PID gains?
I see that both your 'proportional' and 'derivative' values are 10, have you changed these since you first tried this program?
It will be impossible to balance if the gains are not tuned properly for the system.

Thank you TeleFox for your quick comment.

Answer your question, I tried to set many values to proportional and derivative variables, but 10 is the best value for PID gains.

Did you see my methods ADXL320_write_angle() and ADXRS401_write_speed()? Do you think that my conversion values is correct?

 

float ADXL320_write_angle(int16 y, float zeroG)

{

   float volt = 0; float ydiff = 0; float yg = 0; float radian = 0; float degree = 0;

   volt = (y/1024.0)*5;

   //volt = ((y/1024.0)*1.79)+1.37;

   ydiff = zeroG - volt;

   yg = ydiff/0.312;

   radian = asin(yg);

   degree = (180/PI)*radian;   

   return degree;

}

 

float ADXRS401_write_speed(int16 y, float zeroS)

{

   float volt = 0;   float ydiff = 0;   float yg = 0;

   volt = (y/1024.0)*5;

   ydiff = zeroS - volt;

   yg = ydiff/0.015;

   return yg;

 

 

Today I had a new idea. I will try to improve the AD conversion. I will try to set the Vref+ and Vref- to 3,5V and 1,5V to improve the angle precision.

I think that it will not solve the problem, but can help.

I'm waiting for you replie. Thank's.

Changing your Vrefs will increase the measurement accuracy for sure, just remember to scale your volt calculations appropriately afterwards. Also as you have noted, this will not fix your current balancing problem by itself.

How exactly are you driving the motors?
I see that the pwm value is clamped between 30 and 1000 - is zero motor speed right in the middle of this range at pwm = 515?

The middle value is 512, because I'm using h bridge to drive the motors.

If the value is 512, the motor will be immobile. If the value is greater than 512 the motor will rotate clockwise and if the value is less than 512 the motor will rotate counterclockwise.

I can change the value 30 and 1000, but I think that it's not a problem. I'm using this values just to secure the limits of PWM conversion.

Can you understand me?

Ok, that all sounds good, just needed to check a few basics before moving on.

Try changing some of your calculations to perform the divisions last, i.e.:
volt = (y/1024.0)*5; is changed to volt = (y*5)/1024.0;

I know most of your values are floats in these calculations, but there are some integers too so you have to be careful to avoid accidental truncation.

Also I suggest you comment out the complementary filter for now (make angle = returnADXL320Angle;) and just get the PID loop working first. Then you can add the complementary filter in if you need to, but at least you will know that the PID gains are correct.

OK, today night I will try to remove the complementary filter and I will verify the calculation.

When I finish the test I will send the results.

See you soon.

Sorry the delay to respond.

I had a problem in my voltage regulator LM7805 and all the components of my prototype are burned (ADXL, PIC, Gyro...).

I bought new components but the prevision to be delivered is 1 or 2 mounths :(

When I received the components, I'm going to reply again.

Thanks.

Ouch, that sucks... Good luck with getting your replacement parts, I hope they don't take any longer than expected.

I found a problem in code, now I think that it is ok.

But my robot still is not balanced and again I dont know more what can I do. :(

I'm trying to tuning the PID but I can not do it. So I removed the complementary filter and.... I still can not tuning the PID.

How you can see, I was using a small wheel and I think that a larger wheel could be best. Am I sure?

More one question. To tuning the PID I'm doing the follow procedure:

1 - I put zero to derivative value.

2 - Increase the proportional value until fast response of motor (when the robot is around 30º, the maximum speed of motor must be applied).

3 - So increase the derivative value trying balancing the robot.

Am I doing the correct procedure?

Thanks .

 

Using a larger wheel will make the robot faster but decrease the torque you have available to straighten the robot. This means that the motors will be overpowered at a smaller angle, i.e. the robot will be less resistant to disturbances.

You should continue to increase the proportional value until the robot oscillates quickly around the 0° position. Tuning for 30° is no good, as the robot should be able to balance well enough that it never reaches 30° in the first place =)

Once the robot is fighting to stay upright with just proportional control, but probably over-correcting a lot, then you can increase the derivative factor to dampen the response.

It is also important to remember that the speed of the wheels does nothing to help the robot balance - it is the change in speed (acceleration) that determines how much torque the robot is producing to stay upright.