InMotion devBoard

Z kiwiki
Prejsť na: navigácia, hľadanie

Obsah

Odosielacia doska

Odosielacia doska pozostáva s 4 častí:

  • MCU (Arduino)
  • IMU (akcelerometer, gyroskop) modul
  • 2.4GHz RF modul
  • Batéria

Stručný popis fungovania

Akcelerometer a gyroskor posielajú cez I2C zbernicu namerané dáta do MCU ktoré ich spracúva a následne odosiela do RF modulu ktorý ich odosiela prijímaciemu modulu.

Formát dátového paketu

Paket má veľkosť 8 bytov a je rozdelený na tieto časti: Určenie začiatku paketu - 2B Uhol X - 2B Uhol Y - 2B Uhol Z - 2B

Prímacia doska

Prímacia doska pozostáva s 2 častí

  • MCU (Arduino)
  • 2.4GHz RF modul

Posis fungovania

RF modul príma namerané dáta, a následne MCU odosiela tieto dáta cez sériovu linku do PC.

Vizualizačný software

Tento software je napísaný v programovaciom jazyku Java. Do grafu vykresluje získanédáta v reálnom čase.

GraphViz.png

Vizualizácia v prostredí Blender

Na 3D vizualizáciu bol použitý softvér Blender. Získavanie dát zo sériovej linky a ich následnú vizualizáciu bol napísaný skript v jazyku Python.

Blender.png

Bloková schéma zapojenia

Blokova.png

Zdrojový kód

#include <Wire.h>
#include <SPI.h>
#include <RH_NRF24.h>
#include "mpuLib.h"
 
RH_NRF24 nrf24;
 
extern unsigned long last_read_time;
extern float         last_x_angle;
extern float         last_y_angle;
extern float         last_z_angle;
extern float         last_gyro_x_angle;
extern float         last_gyro_y_angle;
extern float         last_gyro_z_angle;
extern float    base_x_accel;
extern float    base_y_accel;
extern float    base_z_accel;
extern float    base_x_gyro;
extern float    base_y_gyro;
extern float    base_z_gyro;
 
float filterVal;
 
void setup()
{
    int error;
    uint8_t c;
    filterVal = 0.4;
 
    Serial.begin(19200);
    Wire.begin();
    error = MPU6050_read (MPU6050_WHO_AM_I, &c, 1);
    error = MPU6050_read (MPU6050_PWR_MGMT_2, &c, 1);
    MPU6050_write_reg (MPU6050_PWR_MGMT_1, 0);
    calibrate_sensors();
    set_last_read_angle_data(millis(), 0, 0, 0, 0, 0, 0);
 
    //NRF
    while (!Serial);
    if (!nrf24.init())
        Serial.println("init failed");
    if (!nrf24.setChannel(1))
        Serial.println("setChannel failed");
    if (!nrf24.setRF(RH_NRF24::DataRate2Mbps, RH_NRF24::TransmitPower0dBm))
        Serial.println("setRF failed");
 
}
 
 
void loop()
{
    int error;
    double dT;
 
    accel_t_gyro_union accel_t_gyro;
 
    error = read_gyro_accel_vals((uint8_t *) &accel_t_gyro);
 
    unsigned long t_now = millis();
 
    float FS_SEL = 131;
 
    float gyro_x = (accel_t_gyro.value.x_gyro - base_x_gyro) / FS_SEL;
    float gyro_y = (accel_t_gyro.value.y_gyro - base_y_gyro) / FS_SEL;
    float gyro_z = (accel_t_gyro.value.z_gyro - base_z_gyro) / FS_SEL;
 
    float accel_x = accel_t_gyro.value.x_accel;
    float accel_y = accel_t_gyro.value.y_accel;
    float accel_z = accel_t_gyro.value.z_accel;
 
    float RADIANS_TO_DEGREES = 180 / 3.14159;
 
    float accel_angle_y = atan(-1 * accel_x / sqrt(pow(accel_y, 2) + pow(accel_z, 2))) * RADIANS_TO_DEGREES;
    float accel_angle_x = atan(accel_y / sqrt(pow(accel_x, 2) + pow(accel_z, 2))) * RADIANS_TO_DEGREES;
 
    float accel_angle_z = atan(sqrt(pow(accel_x, 2) + pow(accel_y, 2)) / accel_z) * RADIANS_TO_DEGREES;;
 
    // Compute the (filtered) gyro angles
    float dt = (t_now - get_last_time()) / 1000.0;
    float gyro_angle_x = gyro_x * dt + get_last_x_angle();
    float gyro_angle_y = gyro_y * dt + get_last_y_angle();
    float gyro_angle_z = gyro_z * dt + get_last_z_angle();
 
    // Compute the drifting gyro angles
    float unfiltered_gyro_angle_x = gyro_x * dt + get_last_gyro_x_angle();
    float unfiltered_gyro_angle_y = gyro_y * dt + get_last_gyro_y_angle();
    float unfiltered_gyro_angle_z = gyro_z * dt + get_last_gyro_z_angle();
 
    // Apply the complementary filter to figure out the change in angle - choice of alpha is
    // estimated now.  Alpha depends on the sampling rate...
    float alpha = 0.96;
    float angle_x = alpha * gyro_angle_x + (1.0 - alpha) * accel_angle_x;
    float angle_y = alpha * gyro_angle_y + (1.0 - alpha) * accel_angle_y;
    float angle_z = gyro_angle_z;  //Accelerometer doesn't give z-angle
 
    // Update the saved data with the latest values
    set_last_read_angle_data(t_now, angle_x, angle_y, angle_z, unfiltered_gyro_angle_x, unfiltered_gyro_angle_y, unfiltered_gyro_angle_z);
    uint16_t angleX;
    uint16_t angleY;
    uint16_t angleZ;
    angleX = 100 * (uint16_t)angle_x;
    angleY = 100 * (uint16_t)angle_y;
    angleZ = 100 * (uint16_t)angle_z;
 
    // Readings from IMU has size of 16bits per axis, so these values are need to be split into two 8bit integers because nRF24 can send only 8bit integers
    uint8_t buffer[8]  =
    {
        (uint8_t)0x25,  // First start bit
        (uint8_t)0x72,  // Second start bit
        (uint8_t)((angleX >> 8)),    // Rotation of X axis
        (uint8_t)(angleX & 0xFF),    // Rotation of X axis
        (uint8_t)((angleY >> 8)),    // Rotation of Y axis
        (uint8_t)(angleY & 0xFF),    // Rotation of Y axis
        (uint8_t)((angleZ >> 8)),    // Rotation of Z axis
        (uint8_t)(angleZ & 0xFF),    // Rotation of Z axis
    };
 
 
    // Sending data using nRF24
    nrf24.send(buffer, sizeof(buffer));
 
    // Sending data to serial port
 
    for (int i = 0; i < 8; i++) {
        Serial.write(buffer[i]);
    }
 
}
 
// Data from IMU are very unstable and noise so we need to filter them
int smooth(int data, float filterVal, float smoothedVal)
{
    if (filterVal > 1)
    {
        filterVal = .99;
    }
    else if (filterVal <= 0)
    {
        filterVal = 0;
    }
    smoothedVal = (data * (1 - filterVal)) + (smoothedVal  *  filterVal);
    return (int16_t)smoothedVal;
}

Kompletný zdrojový kód je dostupný na stiahnutie na git-hube. https://github.com/dudakp/inMotion

Zoznam použitych komponentov

Použitý hardvér

Arduino ProMini https://www.adafruit.com/products/2377 Ard.jpeg
MPU-6050 modul (akcelerometer, gyroskop) - https://www.sparkfun.com/products/11028 Nrf24.jpeg
nRF24 modul (2.4GHz) - https://www.sparkfun.com/products/705 Nrf24breakout.jpeg
FTDI prevodník - https://www.sparkfun.com/products/9873 Ftdi.jpeg
3.7V Li-Po batéria - http://www.ebay.com/itm/2x-3-7v-240mAh-LiPo-Battery-for-SH-6020-6020-1-6023-1-Mini-Helicopter-SY027x2-/271631717682?pt=LH_DefaultDomain_0&hash=item3f3e831532 Bat.jpeg

Použité knižnice

Arduino:

Java:

Python / Blender:

Použitý softvér

Osobné nástroje
Menné priestory

Varianty
Operácie
Navigácia
Rýchle linky
Nástroje
Tlačiť/exportovať