imu.c

/*******************************************************************************
* examples/imu.c                                                               *
* ==============                                                               *
*                                                                              *
* Version: 1.2.0                                                               *
* Date   : 05.11.18                                                            *
* Author : Peter Weissig                                                       *
*                                                                              *
* If you are changing this file, you may also consider to update               *
*     demos/diagnosticTool/diagnosticTool.c                                    *
*******************************************************************************/
 
//*********************************<Included files>*****************************
#include <tucbot/tucbot.h>
 
//*********************************<Methods>************************************
void testImu_print(int16_t value, uint8_t digits);
void testImu(void);
int main(void);
 
//*********************************[testImu_print]******************************
void testImu_print(int16_t value, uint8_t digits) {
 
    int16_t temp_value  = value ;
    uint8_t temp_digits = digits;
 
    if ((temp_value < 0) && temp_digits) {
        temp_digits--;
        temp_value = -temp_value;
    }
    while (temp_digits) {
        temp_value/= 10;
        temp_digits--;
    }
 
    if (temp_value == 0) {
        int16tostr(lcdout, value, digits);
    } else if (value >= 0) {
        while (digits) {
            lcdout('+');
            digits--;
        }
    } else {
        while (digits) {
            lcdout('-');
            digits--;
        }
    }
}
 
//*********************************[testImu]************************************
void testImu(void) {
 
    uint8_t resolution;
    int16_t orientation;
 
    int16_t orientation_display;
    uint8_t countdown           = 0;
    uint8_t flag_redraw_menu    = 0xFF;
 
    lcdclr();
    lcdxy(0,1);
    lcdstr_p(PSTR("x####y#### Z####"));
 
    imu_request(kImuSelectAll, kRequestContinuous);
 
    while (1) {
        if (flag_redraw_menu) {
            flag_redraw_menu = 0x00;
 
            orientation_display = 0x7FFF; // force update on new data
 
            lcdxy(0,0);
            // "Imu  ###°C ####°"
            lcdstr_p(PSTR("Imu  ###" "\xDF" "C ####" "\xDF"));
        }
 
        if (buttons_get(kButtonMiddle)) {
            leds_set(kLedMiddle, kLedOn );
            mdelay(100);
            leds_set(kLedMiddle, kLedOff);
 
            lcdxy(4,0);
            lcdstr_p(PSTR(" calibrating")); // "Imu  calibrating"
            mdelay(250);
            leds_set(kLedMiddle, kLedOn);
 
            imu_calibrate();
            leds_set(kLedMiddle, kLedOff);
 
            flag_redraw_menu = 0xFF;
            countdown = 0;
        } else if (buttons_get(kButtonRight)) {
            leds_set(kLedRight , kLedOn );
            mdelay(100);
            leds_set(kLedRight , kLedOff);
 
            lcdxy(4,0);
            lcdstr_p(PSTR("setRes. ")); // "Imu setRes.     "
            mdelay(100);
            leds_set(kLedRight, kLedOn);
 
            // get current resolution
            resolution = imu_get(kImuGetGyroResolution);
 
            // setup and print next resolution
            switch (resolution) {
                case kImuGyroResolution250:
                    resolution = kImuGyroResolution500;
                    lcdstr_p(PSTR(" 500")); // "Imu setRes.  500"
                    break;
 
                case kImuGyroResolution500:
                    resolution = kImuGyroResolution1000;
                    lcdstr_p(PSTR("1000")); // "Imu setRes. 1000"
                    break;
 
                case kImuGyroResolution1000:
                    resolution = kImuGyroResolution2000;
                    lcdstr_p(PSTR("2000")); // "Imu setRes. 2000"
                    break;
 
                case kImuGyroResolution2000:
                    resolution = kImuGyroResolution250;
                    lcdstr_p(PSTR(" 250")); // "Imu setRes.  250"
                    break;
 
                default:
                    resolution = kImuGyroResolution500;
                    lcdstr_p(PSTR("500!")); // "Imu setRes. 500!"
                    break;
            }
 
            // switch to next resolution
            imu_set(kImuSetGyroResolution, resolution);
 
            // check for errors
            if (imu_get(kImuGetGyroResolution) != resolution) {
                lcdxy(12,0);
                lcdstr_p(PSTR(" ERR")); // "Imu setRes.  ERR"
            }
 
            // wait a bit
            mdelay(1000);
            leds_set(kLedRight, kLedOff);
 
            // reset absolute orientation
            imu_set(kImuSetOrientation, 0);
 
            flag_redraw_menu = 0xFF;
            countdown = 0;
        }
 
        if (countdown) {
            countdown--;
        } else {
            countdown = 5;
 
            if (imu_changed(kImuSelectTemperature)) {
                lcdxy(5,0);
                testImu_print((imu_get(kImuGetTemperature) + 5)/ 10,3);
            }
 
            orientation = imu_get(kImuGetOrientation) / 10;
            if (orientation != orientation_display) {
                orientation_display = orientation;
                lcdxy(11,0); testImu_print(orientation,4);
            }
 
            if (imu_changed(kImuSelectAccelerometer)) {
                lcdxy( 1,1); testImu_print(imu_get(kImuGetAccelX),4);
                lcdxy( 6,1); testImu_print(imu_get(kImuGetAccelY),4);
            }
 
            if (imu_changed(kImuSelectGyroscope)) {
                lcdxy(12,1); testImu_print(imu_get(kImuGetGyroZ),4);
            }
        }
 
        mdelay(100);
    }
}
 
//*********************************[main]***************************************
int main (void) {
 
    init_tucbot(0xFF);
 
    buttons_request(kRequestContinuous);
 
    testImu();
 
    return (0);
}