Lekcja 17: Żyroskop
Tematem tej lekcji jest czujnik L3GD20H 3-osiowy, cyfrowy żyroskop I2C SPI. W lekcji wykorzystujemy oprócz standardowych bibliotek arduino
kod napisany w Pythonie, który korzysta z biblioteki podsłuchującej port szeregowy.
Moduł wyposażony w żyroskop L3GD20H, który służy do pomiaru prędkości kątowej w trzech osiach X, Y, Z.
Posiada możliwość wyboru zakresu pomiarowego z dostępnych: ±245 °/s, ±500 °/s lub ±2000 °/s.
Specyfikacja czujnika poniżej:
|
 |
Wyprowadzenia:
| PIN | Opis |
|---|---|
| VIN | Napięcie zasilania: 2,5 V - 5,5 V. Konwerter napięć ustala stan wysoki wyprowadzeń SCL i SDA na tę wartość. |
| GND | Potencjał masy układu. |
| VDD | Wyjście regulatora 3,3 V o wydajności maksymalnej 150 mA. |
| SCL/SPC | Linia zegarowa I2C/SPI, podłączona poprzez konwerter napięć. Stan wysoki równy jest napięciu VIN, niski 0 V. |
| SDA/SDI | Linia danych I2C / wejściowa SPI, podłączona poprzez konwerter napięć. Stan wysoki równy jest napięciu VIN, niski 0 V. |
| SDO | Linia danych wejściowych SPI, używany również do zmiany adresu magistrali I2C. Ten pin nie posiada regulatora napięcia, toleruje tylko napięcie 3,3 V. |
| CS | Pin umożliwiający włączenie magistrali SPI poprzez podanie stanu niskiego. Domyślnie podciągnięty do 3,3 V - włączony interfejs I2C. Ten pin nie posiada regulatora napięcia, toleruje tylko napięcie 3,3 V. |
| DRDY/INT2 | Gotowość danych do odczytu / przerwanie od kolejki FIFO. Ten pin nie posiada regulatora napięcia, toleruje tylko napięcie 3,3 V. |
| INT1 | Konfigurowalne przerwanie. Ten pin nie posiada regulatora napięcia, toleruje tylko napięcie 3,3 V. |
| DEN | Wyzwolenie danych pomiarowych. Ten pin nie posiada regulatora napięcia, toleruje tylko napięcie 3,3 V. |
Łączymy układ z Arduino za pomocą złączy VIN, GND, SCL i SDA.
Piszemy kod w arduino IDE:
#include < Wire.h>
#include < L3G.h>
L3G gyro;
int prev_rate[3];
int angle[3];
unsigned long time;
int sampleTime=10;
int rate[3];
int sampleNum=250;
int dc_offset=0;
void setup(){
Serial.begin(9600);
Wire.begin();
if (!gyro.init())
{
Serial.println("Failed to autodetect gyro type!");
while (1);
}
gyro.enableDefault();
}
void loop()
{
gyro.read();
for(int i=0;i<=2;i++){
if(i==0){
rate[i]=((int)gyro.g.x-dc_offset)/1000;
}
if(i==1){
rate[i]=((int)gyro.g.y-dc_offset)/1000;
}
if(i==2){
rate[i]=((int)gyro.g.z-dc_offset)/1000;
}
angle[i] += ((double)(prev_rate[i] + rate[i]) * sampleTime)/125;
prev_rate[i] = rate[i];
}
Serial.println(String(angle[0])+";"+String(angle[1])+";"+String(angle[2]));
delay(1);
}
Do uruchomienia kodu w Pythonie należy zainstalować Pythona 2.7 wraz z bibliotekami python_visual, pyserial.
Kod w Pythonie:
from visual import *
from visual.graph import *
import serial
x = 0
odlX=0
odlY=0
odlZ=0
grad2rad = 3.141592/180.0
arduinoSerialData = serial.Serial('/dev/ttyACM0', 9600)
f1 = gcurve(color=color.cyan)
f2 = gcurve(color=color.white)
f3 = gcurve(color=color.red)
scene2 = display(width=600, height=600, center=(0, 0, 0))
deska = box(length=1, height=0.05, width=1, color=color.green)
p_line = box(length=1,height=0.08,width=0.1,color=color.yellow)
deska_strzalka = arrow(color=color.red, axis=(1, 0, 0), shaftwidth=0.06, fixedwidth=1)
while (1 == 1):
if (arduinoSerialData.inWaiting() > 0):
dane = arduinoSerialData.readline()
odl = dane.split(';')
dl = len(odl)
print dl
if (dl == 3):
print odl
if (odl[0] != ''):
odlX = int(odl[0])*grad2rad
f1.plot(pos=(x, odlX))
if (odl[1] != ''):
odlY = int(odl[1])*grad2rad
f2.plot(pos=(x, odlY))
if (odl[2] != ''):
odlZ = int(odl[2])*grad2rad
f3.plot(pos=(x, odlZ))
x += 1
axis = (cos(odlY) * cos(odlZ), -cos(odlY) * sin(odlZ), sin(odlY))
up = (sin(odlX) * sin(odlZ) + cos(odlX) * sin(odlY) * cos(odlZ), sin(odlX) * cos(odlZ) - cos(odlX) * sin(odlY) * sin(odlZ), -cos(odlX) * cos(odlY))
deska.axis = axis
deska.up = up
deska.length = 1.0
deska.width = 0.65
deska_strzalka.axis = axis
deska_strzalka.up = up
deska_strzalka.length = 0.8
p_line.axis = axis
p_line.up = up
Powrót do spisu materiałów