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);
}
}
https://www.youtube.com/watch?v=_z_wQgHoJeA
