view main/robots/orpheus/test-gripper.cpp @ 240:e83616798e5c main

initial checkin
author Bob Cook <bob@bobcookdev.com>
date Sun, 04 Jan 2015 12:18:42 -0800
parents
children 51f23203883c
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

    // center_servos(); // initialize to center positions 
}

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

static void button_init()
{
}

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

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 );

    //-- 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 )
{
    // heartbeat
}

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

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

    hw_init();

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

    for ( ;; )
    {
    }

    return 0;
}

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