view main/robots/orpheus/test-gripper.cpp @ 269:6774826fd1a5 main

Latest demo code
author Bob Cook <bob@bobcookdev.com>
date Mon, 02 May 2016 19:32:52 -0700
parents 51f23203883c
children
line wrap: on
line source

// ----------------------------------------------------------------------------------------
//
//  Copyright (C) 2015 Bob Cook
//    
//  Bob Cook Development, Robotics Library
//  http://www.bobcookdev.com/rl/
// 
//  For independently testing the servo and contact switches on the gripper.
//
//  This progam uses the AVR ATmega128 chip on the BDMICRO Mavric-IIB development board.
//
//  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 "packages/avr/device/int_helpers.h"
#include "packages/avr/device/spinwait.h"

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

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

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

static void servo_init()
{
    //--    Set up Timer1 to service the servo pulse interrupt. We want a 50 Hz cycle for
    //      the primary PWM cycle and we'll use OCR1n to drive each servo's logic pin.

    TCCR1A = 0

        | ( 1 << COM1A1 ) | ( 1 << COM1A0 ) // set OC1A on up, clear on down
        | ( 1 << COM1B1 ) | ( 1 << COM1B0 ) // set OC1B on up, clear on down
        | ( 0 << COM1C1 ) | ( 0 << COM1C0 ) // OC1C unused
        | ( 1 << WGM11  ) | ( 0 << WGM10  ) // PWM phase correct (mode 10)
        ;

    TCCR1B = 0

        | ( 0 << ICNC1 )                                 // no input capture
        | ( 0 << ICES1 )                                 // no input capture
        | ( 1 << WGM13 ) | ( 0 << WGM12 )                // PWM phase correct (mode 10)
        | ( 0 << CS12  ) | ( 1 << CS11 ) | ( 0 << CS10 ) // clk/8
        ;

    TCCR1C = 0

        | ( 0 << FOC1A ) // no force output compare
        | ( 0 << FOC1B ) // no force output compare
        | ( 0 << FOC1C ) // no force output compare
        ;

    ICR1 = 20000; // 10ms counting up, 10ms counting down (for 16Mhz clock)

    DDRB |= ( 1 << PB5 ) | ( 1 << PB6 ); // gripper servo is on PB5

    OCR1A = 18656; // starting point, determined experimentally
}

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

void servo_suspend()
{
    // turn off the clock
    TCCR1B &= ~( ( 1 << CS12  ) | ( 1 << CS11 ) | ( 1 << CS10 ) );
}

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

void servo_unsuspend()
{
    // re-enable the clock
    TCCR1B |= ( 0 << CS12  ) | ( 1 << CS11 ) | ( 0 << CS10 ); // clk/8
}

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

static void button_init()
{
    //-- The gripper detection switches are on PORTD.6 and PORTD.7. Enable the pull-ups.

    DDRD  &= ~( ( 1 << PD6 ) | ( 1 << PD7 ) );
    PORTD |=    ( 1 << PD6 ) | ( 1 << PD7 );

    //-- The two pushbuttons are on PORTD.4 and PORTD.5. Enable the pull-ups.
    
    DDRD  &= ~( ( 1 << PD4 ) | ( 1 << PD5 ) );
    PORTD |=    ( 1 << PD4 ) | ( 1 << PD5 );
}

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

static void hw_init()
{
    //--    Turn off all interrupts.

    cli();
    interrupts_clear_all();

    //--    Set up a periodic "heartbeat" timer for ~100 Hz on Timer0.

    TCCR0 = 0

        | ( 0 << FOC0  )                                    // no force output compare
        | ( 1 << WGM01 ) | ( 0 << WGM00 )                   // CTC mode
        | ( 0 << COM01 ) | ( 0 << COM00 )                   // OC0 not used
        | ( 1 << CS22  ) | ( 1 << CS21  ) | ( 1 << CS20 )   // clk/1024
        ; 

    OCR0 = 156;

    TIMSK |= ( 1 << OCIE0 );

    //-- The built-in LED is on PORTB.0 and the other three indicator LEDs are on PORTD.0, 
    //   PORTD.1, and PORTD.2. The internal LED is "on when high" while the other LEDs are
    //   set to be "on when low".

    DDRB = ( 1 << PB0 );
    DDRD = ( 1 << PD2 ) | ( 1 << PD1 ) | ( 1 << PD0 );

    PORTB &= ~( 1 << PB0 );
    PORTD |=  ( 1 << PD2 ) | ( 1 << PD1 ) | ( 1 << PD0 );

    //-- Initialize the subsections.

    servo_init();
    button_init();

#if 0
    //--    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 Pololu Qik 2s12v10 motor controller.

    motorctl_init();

    //--    Initialize the LS7366 chip(s).

    DDRD |= ( 1 << PD3 ); // this is the SS pin, required to be an output for SPI master
    ls7366_init();

    //--    Initialize servo support hardware.

    servo_init();

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

    if ( ! canmsg_init() )
    {
        odrmot_fatal_error( odrmot_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.
#endif

    sei();
}

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

ISR( TIMER0_COMP_vect )
{
    static uint8_t counter = 0;

    if ( ++counter == 5 )
    {
        PORTB ^= ( 1 << PB0 );
        counter = 0;
    }
}

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

static const uint16_t k_gripper_position_open   = 18656;
static const uint16_t k_gripper_position_closed = 18050;

int main()
{
    //--    Initialize the hardware.

    hw_init();

    //-- Put the servo into an "open" stage.

    OCR1A = k_gripper_position_open;
    spinwait_delay_ms( 1000 ); // let the servo move
    servo_suspend();

    //--    Loop while we have things to do.

    for ( ;; )
    {
        if ( ( PIND & ( ( 1 << PD6 ) | ( 1 << PD7 ) ) ) != 0 )
        {
            PORTD |= ( 1 << PD2 );
        }
        else
        {
            PORTD &= ~( 1 << PD2 );
        }

        if ( ( PIND & ( 1 << PD4 ) ) == 0 )
        {
            PORTD &= ~( 1 << PD0 );

            OCR1A = k_gripper_position_open;
            servo_unsuspend();
            spinwait_delay_ms( 1000 ); // let the servo move

            OCR1A = k_gripper_position_closed;
            spinwait_delay_ms( 2500 ); // let the servo move

            if ( ( PIND & ( ( 1 << PD6 ) | ( 1 << PD7 ) ) ) != 0 )
            {
                // can was not detected in the gripper
                // move the gripper back to the open position
                OCR1A = k_gripper_position_open;
                spinwait_delay_ms( 1000 ); // let the servo move
                servo_suspend();
            }

            /*
            for ( uint16_t value = 18600; value > 18100; value -= 10 )
            {
                OCR1A = value;
                spinwait_delay_ms( 150 );

                if ( ( PIND & ( ( 1 << PD6 ) | ( 1 << PD7 ) ) ) == 0 )
                {
                    canDetection = true;
                    break;
                }
            }

            if ( ! canDetection )
            {
            }
            */
        }
        else if ( ( PIND & ( 1 << PD5 ) ) == 0 )
        {
            PORTD &= ~( 1 << PD1 );
            OCR1A = k_gripper_position_open;
            spinwait_delay_ms( 1000 ); // let the servo move
            servo_suspend();
        }
        else
        {
            PORTD |= ( 1 << PD0 );
            PORTD |= ( 1 << PD1 );
        }

        spinwait_delay_ms( 1 );
    }

    return 0;
}

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