view main/robots/odr-imu/main.cpp @ 248:1fd327ac6ad8 main

Checkpoint for the IMU node firmware.
author Bob Cook <bob@bobcookdev.com>
date Mon, 24 Aug 2015 20:51:16 -0700
parents d1e26bffa503
children 87864f3a89e4
line wrap: on
line source

// ----------------------------------------------------------------------------------------
//
//  Copyright (C) 2015 Bob Cook
//    
//  Bob Cook Development, Robotics Library
//  http://www.bobcookdev.com/rl/
// 
//  IMU node for the Outdoor Robot project, using a CAN interface.
//
//  Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
//  The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
//
// ----------------------------------------------------------------------------------------

#include <avr/interrupt.h>
#include <avr/io.h>
#include <avr/sleep.h>

#include "project_defs.h"

#include "func.h"
#include "leds.h"

#include "packages/avr/device/int_helpers.h"
#include "packages/avr/device/spinwait.h"
#include "packages/avr/device/uart.h"

#include "packages/common/chrobotics/um7.h"
#include "packages/common/chrobotics/um7_data.h"
#include "packages/common/util/misc.h"

// always after other includes
#include "packages/avr/device/workaround34734.h"

// ----------------------------------------------------------------------------------------

static const uint8_t trigger_main_loop_ok    = ( 1 << 0 );
static const uint8_t trigger_send_heartbeat  = ( 1 << 1 );
static const uint8_t trigger_check_emergency = ( 1 << 2 );
static const uint8_t trigger_update_status   = ( 1 << 3 );

static volatile uint8_t g_triggers;

// ----------------------------------------------------------------------------------------

static const uint8_t odrimu_fatal_error_can_init = 1;
static const uint8_t odrimu_fatal_error_can_comm = 2;
static const uint8_t odrimu_fatal_error_exiting  = 9;

void odrimu_fatal_error( uint8_t fault )
{
    cli(); // no more interrupts
    DDRB = 0; DDRC = 0; DDRD = 0; // everything is an input
    DDRB |= ( 1 << PB2 ); // except the leds
    DDRC |= ( 1 << PC4 ) | ( 1 << PC5 );
    DDRD |= ( 1 << PD7 );
    leds_off();

    for ( ;; )
    {
        for ( uint8_t i = 0; i < 5; i++ )
        {
            led_red_on();
            spinwait_delay_ms( 125 );
            led_red_off();
            spinwait_delay_ms( 75 );
        }

        spinwait_delay_ms( 125 );
        led_red_on();
        led_yellow_on();
        spinwait_delay_ms( 125 );

        for ( uint8_t i = 0; i < fault; i++ )
        {
            led_green_1_on();
            led_green_2_on();
            spinwait_delay_ms( 350 );
            
            led_green_1_off();
            led_green_2_off();
            spinwait_delay_ms( 350 );
        }

        spinwait_delay_ms( 1000 );
        
        led_red_off();
        led_yellow_off();
    }
}

// ----------------------------------------------------------------------------------------

void odrimu_hw_init()
{
    //--    Turn off all interrupts.

    cli();
    interrupts_clear_all();

    //--    Indicator LEDs on PORTB.2, PORTC.4, PORTC.5, PORTD.7

    DDRB |= ( 1 << PB2 );
    DDRC |= ( 1 << PC4 ) | ( 1 << PC5 );
    DDRD |= ( 1 << PD7 );
    leds_off();

    //--    Blink the LEDs just to say hello.
    
    for ( uint8_t i = 0; i < 5; i++ )
    {
        led_red_on();
        led_yellow_on();
        led_green_1_on();
        led_green_2_on();
        spinwait_delay_ms( 75 );
        leds_off();
        spinwait_delay_ms( 75 );
    }

    //--    Timer0 gives up a periodic "heartbeat" interrupt at ~100 Hz.

    TCCR0A = 0

        | ( 0 << COM0A1 ) | ( 0 << COM0A0 )                 // OC0A disconnected
        | ( 0 << COM0B1 ) | ( 0 << COM0B0 )                 // OC0B disconnected
        | ( 1 << WGM01  ) | ( 0 << WGM00  )                 // CTC mode
        ;

    TCCR0B = 0

        | ( 0 << FOC0A ) | ( 0 << FOC0B )                   // no force output compare
        | ( 0 << WGM02 )                                    // CTC mode
        | ( 1 << CS02  ) | ( 0 << CS01  ) | ( 1 << CS00 )   // clk/1024
        ;

    OCR0A = 156;

    TIMSK0 |= ( 1 << OCIE0A );

    //--    Initialize the status subsystem.

    status_init();

    //--    Initialize the IMU.

    imu_init();

    //--    Initialize the CAN related functions and hardware.

    if ( ! canmsg_init() )
    {
        odrimu_fatal_error( odrimu_fatal_error_can_init );
    }

    //--    When we sleep we want the "idle" mode e.g. wake up on any interrupt.

    set_sleep_mode( SLEEP_MODE_IDLE );

    //--    Re-enable interrupts.

    sei();
}

// ----------------------------------------------------------------------------------------

ISR( TIMER0_COMPA_vect )
{
    static uint8_t count = 0;

    //--    The trigger_main_loop_ok flag indicates the main loop is "alive" e.g. still
    //      running. We only blink the "heartbeat" LED if it continues to toggle.
    //
    //      led blink pattern: 60 milliseconds on, 150 off, 60 on
    //      check ESTOP status: every 100 milliseconds
    //      update the system status every 500 milliseconds
    //      heartbeat: every 2.5 seconds
    
    if ( count % 10 == 0 )
    {
        g_triggers |= trigger_check_emergency;
    }

    if ( count % 50 == 0 )
    {
        g_triggers |= trigger_update_status;
    }

    switch ( ++count )
    {
        case 10:
            if ( g_triggers & trigger_main_loop_ok ) { led_green_1_on(); }
            break;

        case 16:
            if ( g_triggers & trigger_main_loop_ok ) { led_green_1_off(); }
            break;

        case 31:
            if ( g_triggers & trigger_main_loop_ok ) { led_green_1_on(); }
            break;

        case 37:
            if ( g_triggers & trigger_main_loop_ok ) { led_green_1_off(); }
            g_triggers &= ~trigger_main_loop_ok;
            break;

        case 125:
            g_triggers |= trigger_send_heartbeat;
            break;

        case 250:
            g_triggers |= trigger_send_heartbeat;
            count = 0;
            break;
    }
}

// ----------------------------------------------------------------------------------------

int main()
{
    //--    Initialize the hardware and send a "hello we started" message.

    odrimu_hw_init();
    canmsg_send_all_clear();

    //--    Loop forever responding to CAN messages and system status.

    uint8_t can_comm_errors = 0;

    for ( ;; )
    {
        //--    The main loop is still running.

        g_triggers |= trigger_main_loop_ok;

        //--    Sleep until there is a new interrupt (the CAN driver gets one, as does
        //      the periodic heartbeat, so we won't sleep forever).

//        sleep_mode();

        //--    Process data coming from the IMU module.

        imu_process_pending();
        
        if ( status_is_imu_active() )
        {
            led_green_2_on();
        }
        else
        {
            led_green_2_off();
        }

        //--    Check for an emergency situation?

        if ( g_triggers & trigger_check_emergency )
        {
            g_triggers &= ~trigger_check_emergency;

            if ( status_is_emergency() )
            {
                led_red_on();
            }
            else
            {
                led_red_off();
            }
        }

        //--    Time to send the heartbeat message?

        if ( g_triggers & trigger_send_heartbeat )
        {
            g_triggers &= ~trigger_send_heartbeat;

            if ( ! canmsg_send_heartbeat() )
            {
                ++can_comm_errors;
            }
        }

        //--    Time to check status?

        if ( g_triggers & trigger_update_status )
        {
            g_triggers &= ~trigger_update_status;

            status_update();
        }

        //--    Any pending CAN messages to receive/process?

        if ( ! canmsg_process_pending() )
        {
            ++can_comm_errors;
        }

        //--    Report warning and/or error conditions.

        bool warning = false;

        if ( can_comm_errors > 50 )
        {
            odrimu_fatal_error( odrimu_fatal_error_can_comm );
        }
        else if ( can_comm_errors > 10 )
        {
            warning = true;
        }

        if ( warning )
        {
            led_yellow_on();
        }
        else
        {
            led_yellow_off();
        }
    }

    odrimu_fatal_error( odrimu_fatal_error_exiting );
    return 0;
}

// ----------------------------------------------------------------------------------------