View unanswered posts | View active topics It is currently Mon Jun 27, 2016 9:23 am






Reply to topic  [ 1 post ] 
Self Balancing isn't balancing 
Author Message
Rookie

Joined: Thu May 26, 2016 11:47 am
Posts: 1
Post Self Balancing isn't balancing
Hello
I was wondering if anybody could help me. At the moment, I'm building a self balancing robot. It's task is to stay balanced when activated. I'm using an Arduino Uno with an l3gd20 3-axis gyro carrier and an L298N motor driver.
Now here's the problem:
the robot seems to balance itself for about a second but after that, it immediately fails balancing.
Maybe an error is located in my arduino code, but I can't seem to find anything wrong. Maybe somebody else can spot an error in my code?

Here is my code:

byte countS = 0;
int recOmegaI[10];
int omegaI = 0;
long thetaI = 0;
long sumPower = 0;
long sumSumP = 0;
const int kAngle = 50;
const int kOmega = 500;
const long kSpeed = 60;
const long kDistance = 20;
long powerScale;
int power;
long vE5 = 0;
long xE5 = 0;
#include <SPI.h>
int ry;
long R;
void L3GD20_write(byte reg, byte val) {
digitalWrite(10, LOW);
SPI.transfer(reg);
SPI.transfer(val);
digitalWrite(10, HIGH);
}

byte L3GD20_read(byte reg) {
byte ret = 0;
digitalWrite(10, LOW);
SPI.transfer(reg | 0x80);
ret = SPI.transfer(0);
digitalWrite(10, HIGH);
return ret;
}

void setup () {
Serial .begin(115200);
pinMode(4, OUTPUT);
pinMode(5, OUTPUT);
pinMode(6, OUTPUT);
pinMode(7, OUTPUT);
pinMode(8, OUTPUT);
pinMode(9, OUTPUT);
for ( int i = 0 ; i < 10 ; i++ ) { recOmegaI[i] = 0; }


pinMode(10, OUTPUT);
digitalWrite(10, HIGH);
SPI.begin();
SPI.setBitOrder(MSBFIRST);
SPI.setDataMode(SPI_MODE3);
SPI.setClockDivider(SPI_CLOCK_DIV2);
L3GD20_write(0x20, B11001111);
L3GD20_write(0x23, B00000000);

delay(300);
}

void loop () {
chkAndCtl();
if ( power > 0 ) {
analogWrite( 6, power );
digitalWrite( 4, HIGH );
digitalWrite( 5, LOW );
analogWrite( 9, power );
digitalWrite( 7, HIGH );
digitalWrite( 8, LOW );
} else {
analogWrite( 6, - power );
digitalWrite( 4, LOW );
digitalWrite( 5, HIGH );
analogWrite( 9, - power );
digitalWrite( 7, LOW );
digitalWrite( 8, HIGH );
}
}

void chkAndCtl() {
R = 0;
for ( int i = 0 ; i < 45 ; i++ ) {
ry = ( (L3GD20_read(0x2B) << 8 ) | L3GD20_read(0x2A) );
R = R + ry;
delayMicroseconds(90);
}

omegaI = R * 0.00875 / 45;
if ( abs( omegaI ) < 2 ) { omegaI = 0; }
recOmegaI[0] = omegaI; thetaI = thetaI + omegaI; countS = 0;
for ( int i = 0 ; i < 10 ; i++ ) {
if ( abs( recOmegaI[i] ) < 4 ) { countS++; }
} if ( countS > 9 ) { thetaI = 0;
vE5 = 0;
xE5 = 0;
sumPower = 0;
sumSumP = 0;
}
for ( int i = 9 ; i > 0 ; i-- ) { recOmegaI[ i ] = recOmegaI[ i-1 ]; }
powerScale = ( kAngle * thetaI / 100 ) + ( kOmega * omegaI / 100 ) + ( kSpeed * vE5 / 1000 ) + ( kDistance * xE5 / 1000 );
power = max ( min ( 95 * powerScale / 100 , 255 ) , -255 );
sumPower = sumPower + power;
sumSumP = sumSumP + sumPower;

}


If the code is correct, maybe the problem locates at my gyroscope? Is it possible that my gyro gives wrong signals to my Arduino?
Sorry for bad english :biggrin:
I would really appreciate some help :-)


Thu May 26, 2016 12:25 pm
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 1 post ] 

Who is online

Users browsing this forum: No registered users and 1 guest


You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot post attachments in this forum

Search for:
Jump to:  



Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.