Pages: [1]   Go Down
Author Topic: Self-balancing platform and issues with sensors  (Read 907 times)
0 Members and 1 Guest are viewing this topic.
RJ, Brazil
Offline Offline
Newbie
*
Karma: 0
Posts: 9
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Hello, everyone.

I am building a project that is a self-balancing platform, much like this one, however I'm not using the same components. Here's the list:

** Sensors:
- Accelerometer: MMA7361 (datasheet)
- Gyroscope: L3G4200D (datasheet)
** And for further information:
- Arduino UNO R3
- A pair of Servomotors: S3003 (datasheet)
** Libraries I'm using:
- Accelerometer: AcceleroMMA7361.h (source code)
- Gyroscope: L3G4200D.h (source code)
- Kalman filter: Kalman.h (source code)
- Wire.h
- math.h
- Servo.h

The code is the following:
Code:
#include <math.h>
#include <Wire.h>
#include <Servo.h>
#include <AcceleroMMA7361.h>
#include <Kalman.h>
#include <L3G4200D.h>

Servo servo_x;
Servo servo_y;
L3G4200D gyro;
Kalman kalman_x;
Kalman kalman_y;
AcceleroMMA7361 accelerometer;

short accX, accY, accZ;
short gyroX, gyroY, gyroZ;
double accXangle, accYangle;
double kalman_x, kalman_y;
double map_x, map_y, p_map_x, p_map_y;
unsigned long int timer;
int set_default_servo = 0;

void setup()

  Serial.begin(9600);
 
  servo_x.attach(6);
  servo_y.attach(9);
 
  accelerometer.begin(13, 12, 11, 10, A0, A1, A2);
  accelerometer.setSensitivity(LOW);
  //accelerometer.calibrate();
 
  Wire.begin();
  gyro.enableDefault();
 
  servo_x.writeMicroseconds(1480);
  servo_y.writeMicroseconds(1480);

  kalman_x.setAngle(90);
  kalman_y.setAngle(90);

  timer = micros();
}

void loop()

  gyro.read();
  accX = accelerometer.getXAccel(); // Why are you so stubborn?
  accY = accelerometer.getYAccel();
//accZ = accelerometer.getZAccel();
  gyroX = gyro.g.x;
  gyroY = gyro.g.y;
//gyroZ = gyro.g.z;
 
  if (set_default_servo == 0)
  {
    Serial.print("\nWriting default servo angulation...\n");
    servo_x.writeMicroseconds(1480);
    servo_y.writeMicroseconds(1480);
    set_default_servo = 1;
  }

  double p_map_x = (double)map_x;
  double p_map_y = (double)map_y;

  map_x = map(round(accX), -97, 105, 0, 180);
  map_y = map(round(accY), -75, 118, 0, 180);

  double gyro_x_rate = (double)gyroX/131.0; // is this right? i couldn't find how to get the rate
  double gyro_y_rate = -((double)gyroY/131.0);
 
  kalman_x = kalman_x.getAngle(map_x, gyro_x_rate, (double)(micros()-timer)/1000000);
  kalman_y = kalman_y.getAngle(map_y, gyro_y_rate, (double)(micros()-timer)/1000000);
 
  if ((kalman_x > 0 && kalman_x < 181) && abs(map_x - p_map_x) >= 5) // last condition because the values vary a lot
  {
servo_x.write(180-round(kalman_x));
  }
  if ((kalman_y > 0 && kalman_y < 181) && abs(map_y - p_map_y) >= 5)
  {
    servo_y.write(180-round(kalman_y));
  }

  timer = micros();

  Serial.print("ax=");
  Serial.print(accX);
  Serial.print("  ay=");
  Serial.print(accY);
  Serial.print("\tgx=");
  Serial.print(gyroX);
  Serial.print("  gy=");
  Serial.print(gyroY);
  Serial.print("\tx=");
  Serial.print((int)map_x);
  Serial.print("  y=");
  Serial.print((int)map_y);
  Serial.print("\tKx=");
  Serial.print(round(kalman_x));
  Serial.print("  Ky=");
  Serial.print(round(kalman_y));
  Serial.print("\tsx=");
  Serial.print(servo_x.read());
  Serial.print("  sy=");
  Serial.print(servo_y.read());
  Serial.print("\r\n");
  delay(450);
}

     The problem I'm having is that the accX and accY outputs strangely spiking values for a single loop and then go back to normal (or what they should be outputting). I have tried a lot of things, but the problem still happens when I use analogRead() to get the output from the accelerometer.
     My gyro also does this, with the platform standing still, parallel to its referential (in theory the Earth), it varies its output values so much, but it is not giving me any problem, because I'm just using it for the Kalman filter. But the core problem seems to be pointing at the accelerometer, but I'm not quite sure where to look, I've tried so much, different methods, none worked.
     If you know something that might help me, I will be very grateful; and since this project was built with pieces of code from other projects, I do plan to make a full tutorial once I everything is sorted out. And, if you have any suggestion to better this code, even if apart from the accelerometer matter, please reply.
     And if there is any information I may have forgotten or someting that you would like to know, please also reply.

Thank you very much.
Logged

0
Offline Offline
Sr. Member
****
Karma: 8
Posts: 394
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Any time motors are involved power/noise problems can arise.  How are the servos connected?
Logged

RJ, Brazil
Offline Offline
Newbie
*
Karma: 0
Posts: 9
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Any time motors are involved power/noise problems can arise.  How are the servos connected?

Thanks for taking an interest, Joe.

The servos are connected in parallel.
Here is a picture with all the connections for further explanation:

The servos don't actually share a negative wire, I just put them as one to make drawing easier.
And sorry for the poor Paint skills.
Logged

0
Offline Offline
Sr. Member
****
Karma: 8
Posts: 394
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Next time use this http://fritzing.org/ rather than Paint.

It looks like the servos are drawing power from the Arduino 5V supply.  This not good practice.  Power them from a seperate source (perhaps 4 AA batteries) and see if this helps.  Remember to connect the grounds in common.

Logged

Offline Offline
Faraday Member
**
Karma: 62
Posts: 3030
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Take the servos away and just monitor what the accelerometer is telling you then.
Logged

RJ, Brazil
Offline Offline
Newbie
*
Karma: 0
Posts: 9
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Take the servos away and just monitor what the accelerometer is telling you then.
Next time use this http://fritzing.org/ rather than Paint.

It looks like the servos are drawing power from the Arduino 5V supply.  This not good practice.  Power them from a seperate source (perhaps 4 AA batteries) and see if this helps.  Remember to connect the grounds in common.


Thanks both of you, it indeed worked! The output values from the accelerometer are no longer spiking.
But I still have one last problem: the range in the accelerometer is not fixed.
For example, the gyro I'm using has a range from -32768 to 32767, and I verified that, it works. But the accelerometer does not. Every time I reset or make some change the range also changes.
Do any of you have an idea of what that might be? I'm blaming on the accelerometer itself, but I'm not really sure.
Thanks once more for your help so far!
Logged

Offline Offline
Faraday Member
**
Karma: 62
Posts: 3030
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Does your device actually move ?    The accelerometer is going to be recording the device's movement,  as well as gravity.
Logged

RJ, Brazil
Offline Offline
Newbie
*
Karma: 0
Posts: 9
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Does your device actually move ?    The accelerometer is going to be recording the device's movement,  as well as gravity.
I got it working, thanks!
Instead of getting the raw value, I used the voltage outputs, and apparently the range fixed, and the values were in accord with the datasheet.
Logged

United Kingdom
Offline Offline
Tesla Member
***
Karma: 224
Posts: 6619
Hofstadter's Law: It always takes longer than you expect, even when you take into account Hofstadter's Law.
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

I think the biggest problem with your original arrangement was that you were using a common ground wire both to feed power to the servos and to connect the ground side of the accelerometer module. This is absolutely guaranteed to cause errors in the analog readings when the servos move. When using analog sensors, always dedicate one of the Arduino ground pins for use as the sensor ground connection. Use the other Arduino ground pins for power (if applicable) and the ground side of output devices.
Logged

Formal verification of safety-critical software, software development, and electronic design and prototyping. See http://www.eschertech.com. Please do not ask for unpaid help via PM, use the forum.

Pages: [1]   Go Up
Jump to: