Hi guys, I’m building an auto-balancing robot (Segway principle) with two friends. This robot uses an Arduino Uno device, a gyroscope 2-axis T000060 and a servo motor. We already built the robot, and we have an issue with the program that uses the gyroscope to measure the angle between the robot and the vertical, which is the following program:
#include<Servo.h>
Servo moteur;
const float gyro_zero=511;
const float sensibilite = 0.00067; // sensitivity in V/°/s
const float consigne = 0;
float angle = 0;
float rotation;
const float vref = 5;
const float t = 100; // lenght of the loop, used in summation
int pos;
void setup(){
Serial.begin(9600);
moteur.attach(7);
pinMode(A0,INPUT); // gyroscope on A0 pin
}
void loop(){
pos=0;
rotation=0;
for (int i = 0; i<10; i++){
rotation+=analogRead(A0); // measures the angle 10 times then divide by total time in order to have an average angle
delay(t/10); // so that irrelevant measures can be dampted
}
rotation/=10; // what we get here is a number between 0 and 1023, proportionnal to what the gyroscope
rotation -=gyro_zero; // measures
rotation =vref/1023/sensibilite; // what we get here is now a rotation speed in °/sec
if (sqrt(rotationrotation)>3) { // we want to wipe away vibrations and imperfections, so we add a filter
Serial.print(rotation);
rotation*=t/1000; // then we integrate rotation speed to get rotation angle
angle+=rotation;
}
Serial.print("…");
Serial.println(angle);
delay(t);
}
void rot(float a){
if (a > consigne) { moteur.write(pos++); // finally we rotate the servo so that the robot can remain stood up
delay(t);}
else { moteur.write(pos–);
delay(t);}
}
What goes wrong is that even when the gyroscope remains at rest, it does measure a rotation speed (which is why we had a filter), still we don’t understand how can such significant speed exist without the gyro being moved.
Here are the results from the monitor without the gyro being moved (left : rot. speed ; right: rot. angle)
…(angle initialises at 0, though speed is already at 31/32 at the beginning)
31.37…76.30
32.83…79.59
32.83…82.87
34.29…86.30
34.29…89.73
32.10…92.94
30.64…96.00
33.56…99.36
31.37…102.49
32.10…105.70
34.29…109.13
32.10…112.34
32.83…115.62
33.56…118.98
33.56…122.34
31.37…125.47
33.56…128.83
31.37…131.96
33.56…135.32
35.75…138.89
31.37…142.03
32.10…145.24
33.56…148.60
32.83…151.88
35.02…155.38
33.56…158.74
32.10…161.95
35.75…165.52
33.56…168.88
34.29…172.31
34.29…175.73
34.29…179.16
34.29…182.59
35.02…186.09
etc…
If you see a mistake or something clumsy in the program which would make it not work, I could use some help.
Have a nice day!