view main/robots/odr-sonar-front/sonar.cpp @ 220:74381e5dd204 main

Demo for the Adafruit LCD product number 1480.
author Bob Cook <bob@bobcookdev.com>
date Mon, 07 Jul 2014 18:42:15 -0700
parents 7411b6a1dba2
children
line wrap: on
line source

// ----------------------------------------------------------------------------------------
//
//  Copyright (C) 2011-2014 Bob Cook
//    
//  Bob Cook Development, Robotics Library
//  http://www.bobcookdev.com/rl/
// 
//  Sonar functions.
//
//  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 "project_defs.h"

#include "func.h"

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

#include "packages/common/filt/filtbuf_u16.h"
#include "packages/common/util/misc.h"

// ----------------------------------------------------------------------------------------
//  To reduce the frequency of the sensor pings, we number them with gaps between each.
//  See the TIMER1_OVF interrupt code to see how this is used.

static const uint8_t sensor_index_offset = 6;

static const uint8_t sensor_index_left         = ( 0 * sensor_index_offset );
static const uint8_t sensor_index_center_left  = ( 1 * sensor_index_offset );
static const uint8_t sensor_index_center_right = ( 2 * sensor_index_offset );
static const uint8_t sensor_index_right        = ( 3 * sensor_index_offset );
static const uint8_t sensor_index_max_value    = ( 4 * sensor_index_offset );

volatile uint8_t  g_curr_sensor_index;

volatile uint8_t  g_sensor_left;               // pin PD6
volatile uint8_t  g_sensor_center_left;        // pin PD5
volatile uint8_t  g_sensor_center_right;       // pin PB6
volatile uint8_t  g_sensor_right;              // pin PB7

volatile bool     g_sensor_triggered;

static const uint8_t sensor_state_idle      = 0;
static const uint8_t sensor_state_pinging   = 1;
static const uint8_t sensor_state_receiving = 2;
static const uint8_t sensor_state_received  = 3;

volatile uint8_t  g_sensor_state;
volatile uint8_t  g_sensor_failures;

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

void sonar_init()
{
    //--    Timer1 gives us a timestamp for sensor inputs.

    TCCR1A = 0

        | ( 0 << COM1A1 ) | ( 0 << COM1A0 )                 // OC1A disconnected
        | ( 0 << COM1B1 ) | ( 0 << COM1B0 )                 // OC1B disconnected
        | ( 0 << WGM11  ) | ( 0 << WGM10  )                 // Normal mode
        ;

    TCCR1B = 0

        | ( 0 << ICNC1 ) | ( 0 << ICES1 )                   // not using input capture
        | ( 0 << WGM13 ) | ( 0 << WGM12 )                   // Normal mode
        | ( 0 << CS12  ) | ( 1 << CS11  ) | ( 0 << CS10 )   // clk/8
        ;

    TCNT1H = 0x00; TCNT1L = 0x00;
    
    //--    Enable the necessary pin-change interrupts.
    
    PCICR |= ( 1 << PCIE0 ) | ( 1 << PCIE2 );
}

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

ISR( TIMER1_OVF_vect )
{
    //--    Check that we got a valid sample for the previous attempt. If not, we may have
    //      a dead sensor. If this happens too often we have an emergency situation.

    if ( g_sensor_state == sensor_state_pinging )
    {
        if ( g_sensor_failures < 200 )
        {
            ++g_sensor_failures;
        }
    }

    //--    Reset the sensor state to idle; tracking the state allaws the detection of
    //      unresponsive sensors.

    g_sensor_state = sensor_state_idle;

    //--    We need to advance to the next sensor then trigger an output pulse. After
    //      the trigger we switch the pin to an input for sensing the hold-off period
    //      and the reception of the final result.
    
    g_curr_sensor_index++;
    g_curr_sensor_index %= sensor_index_max_value;

    switch ( g_curr_sensor_index )
    {
        case sensor_index_left:
            // switch PORTD.6 to an output, set it high
            DDRD  |= ( 1 << PD6 );
            PORTD |= ( 1 << PD6 );
            g_sensor_state = sensor_state_pinging;
            break;

        case sensor_index_center_left:
            // switch PORTD.5 to an output, set it high
            DDRD  |= ( 1 << PD5 );
            PORTD |= ( 1 << PD5 );
            g_sensor_state = sensor_state_pinging;
            break;

        case sensor_index_center_right:
            // switch PORTB.6 to an output, set it high
            DDRB  |= ( 1 << PB6 );
            PORTB |= ( 1 << PB6 );
            g_sensor_state = sensor_state_pinging;
            break;

        case sensor_index_right:
            // switch PORTB.7 to an output, set it high
            DDRB  |= ( 1 << PB7 );
            PORTB |= ( 1 << PB7 );
            g_sensor_state = sensor_state_pinging;
            break;
    }

    spinwait_delay_us( 5 );

    switch ( g_curr_sensor_index )
    {
        case sensor_index_left:
            // set PORTD.6 low then switch it to an input
            PORTD &= ~( 1 << PD6 );
            DDRD  &= ~( 1 << PD6 );
            // enable pin-change interrupt 22
            PCMSK2 |= ( 1 << PCINT22 );
            break;

        case sensor_index_center_left:
            // set PORTD.5 low then switch it to an input
            PORTD &= ~( 1 << PD5 );
            DDRD  &= ~( 1 << PD5 );
            // enable pin-change interrupt 21
            PCMSK2 |= ( 1 << PCINT21 );
            break;

        case sensor_index_center_right:
            // set PORTB.6 low then switch it to an input
            PORTB &= ~( 1 << PB6 );
            DDRB  &= ~( 1 << PB6 );
            // enable pin-change interrupt
            PCMSK0 |= ( 1 << PCINT6 );
            break;

        case sensor_index_right:
            // set PORTB.7 low then switch it to an input
            PORTB &= ~( 1 << PB7 );
            DDRB  &= ~( 1 << PB7 );
            // enable pin-change interrupt
            PCMSK0 |= ( 1 << PCINT7 );
            break;
    }
}

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

ISR( PCINT0_vect )
{
    //--    Pin-change interrupt, could be PORTB.6 (center right) or PORTB.7 (right).
    
    if ( g_curr_sensor_index == sensor_index_center_right )
    {
        if ( ( PINB & ( 1 << PB6 ) ) != 0 )
        {
            // rising edge => hold-off period over, reset timer
            TCNT1H = 0x00; TCNT1L = 0x00;
            g_sensor_state = sensor_state_receiving;
        }
        else
        {
            // falling edge => end of sample
            uint16_t sample = TCNT1L;
            sample |= static_cast< uint16_t >( TCNT1H ) << 8;
            g_sensor_center_right = static_cast< uint8_t >( sample / 291 );
            g_sensor_state = sensor_state_received;
            // disable pin-change interrupt
            PCMSK0 &= ~( 1 << PCINT6 );
        }
    }
    else // sensor_index_right
    {
        if ( ( PINB & ( 1 << PB7 ) ) != 0 )
        {
            // rising edge => hold-off period over, reset timer
            TCNT1H = 0x00; TCNT1L = 0x00;
            g_sensor_state = sensor_state_receiving;
        }
        else
        {
            // falling edge => end of sample
            uint16_t sample = TCNT1L;
            sample |= static_cast< uint16_t >( TCNT1H ) << 8;
            g_sensor_right = static_cast< uint8_t >( sample / 300 );
            g_sensor_state = sensor_state_received;
            // disable pin-change interrupt
            PCMSK0 &= ~( 1 << PCINT7 );
        }
    }
}

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

ISR( PCINT2_vect )
{
    //--    Pin-change interrupt, could be PORTD.5 (center left) or PORTD.6 (left).
    
    if ( g_curr_sensor_index == sensor_index_left )
    {
        if ( ( PIND & ( 1 << PD6 ) ) != 0 )
        {
            // rising edge => hold-off period over, reset timer
            TCNT1H = 0x00; TCNT1L = 0x00;
            g_sensor_state = sensor_state_receiving;
        }
        else
        {
            // falling edge => end of sample
            uint16_t sample = TCNT1L;
            sample |= static_cast< uint16_t >( TCNT1H ) << 8;
            g_sensor_left = static_cast< uint8_t >( sample / 291 );
            g_sensor_state = sensor_state_received;
            // disable pin-change interrupt
            PCMSK2 &= ~( 1 << PCINT22 );
        }
    }
    else // sensor_index_center_left
    {
        if ( ( PIND & ( 1 << PD5 ) ) != 0 )
        {
            // rising edge => hold-off period over, reset timer
            TCNT1H = 0x00; TCNT1L = 0x00;
            g_sensor_state = sensor_state_receiving;
        }
        else
        {
            // falling edge => end of sample
            uint16_t sample = TCNT1L;
            sample |= static_cast< uint16_t >( TCNT1H ) << 8;
            g_sensor_center_left = static_cast< uint8_t >( sample / 291 );
            g_sensor_state = sensor_state_received;
            // disable pin-change interrupt
            PCMSK2 &= ~( 1 << PCINT21 );
        }
    }
}

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

void sonar_start()
{
    //--    Reset the sensor failure counter, things might be better now.

    g_sensor_failures = 0;

    //--    Enable the interrupt on Timer1, which starts the machinery.

    TIMSK1 |= ( 1 << TOIE1 );
}

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

void sonar_stop()
{
    //--    Disable the interrupt on Timer1, which stops the machinery.

    TIMSK1 &= ~( 1 << TOIE1 );
}

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

bool sonar_is_running()
{
    return ( ( TIMSK1 & ( 1 << TOIE1 ) ) != 0 );
}

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

static uint8_t compute_min_sensor_value()
{
    uint8_t curr_min_sensor_value = g_sensor_left;

    if ( g_sensor_center_left < curr_min_sensor_value )
    {
        curr_min_sensor_value = g_sensor_center_left;
    }

    if ( g_sensor_center_right < curr_min_sensor_value )
    {
        curr_min_sensor_value = g_sensor_center_right;
    }

    if ( g_sensor_right < curr_min_sensor_value )
    {
        curr_min_sensor_value = g_sensor_right;
    }

    return curr_min_sensor_value;
}

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

bool sonar_send_update()
{
    //--    Only send data if the sonar machinery is running.
    
    if ( ! sonar_is_running() )
    {
        return true; // no error
    }

    //--    Always send if the current minimum sensor value is significantly different
    //      than the previous minimum. Otherwise, only send every tenth call, or more
    //      frequently for shorter distances. Assumes this function is called at 10 Hz.

    static uint8_t s_last_min_sensor_value = 127; // starts as arbitrarily high
    static uint8_t s_countdown_to_send     = 10;  // send when equal to zero

    uint8_t curr_minimum = compute_min_sensor_value();
    bool    send_update  = false;

    if ( s_last_min_sensor_value < 5 )
    {
        send_update = ( s_last_min_sensor_value > curr_minimum );
    }
    else
    {
        send_update = ( s_last_min_sensor_value - 5 > curr_minimum );
    }

    s_last_min_sensor_value = curr_minimum;

    --s_countdown_to_send;

    if ( s_countdown_to_send == 0 )
    {
        send_update = true;
    }

    if ( ! send_update )
    {
        return true; // no error
    }

    //--    Reset the countdown timer. It doesn't matter why we are sending, just
    //      don't want to send too rapidly, unnecessarily.

    if ( curr_minimum > 20 )
    {
        s_countdown_to_send = 10;
    }
    else
    {
        s_countdown_to_send = util_max( ( curr_minimum / 2 ), 2 );
    }

    //--    Send the four sensor range values.

    return canmsg_send_sonar_front( g_sensor_left,
                                    g_sensor_center_left,
                                    g_sensor_center_right,
                                    g_sensor_right );
}

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

bool sonar_fault()
{
    return ( g_sensor_failures > 10 );
}

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