Subversion Repositories Projects

/*****************************************************************************
 *   Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net                  *
 *                                                                           *
 *   This program is free software; you can redistribute it and/or modify    *
 *   it under the terms of the GNU General Public License as published by    *
 *   the Free Software Foundation; either version 2 of the License.          *
 *                                                                           *
 *   This program is distributed in the hope that it will be useful,         *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of          *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the           *
 *   GNU General Public License for more details.                            *
 *                                                                           *
 *   You should have received a copy of the GNU General Public License       *
 *   along with this program; if not, write to the                           *
 *   Free Software Foundation, Inc.,                                         *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.               *
 *                                                                           *
 *****************************************************************************/

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

#include "lcd.h"
#include "timer.h"      // Keys
#include "servo.h"

#define SERVO_MIN                       313     // 1.0 ms (Servo min)
#define SERVO_MAX                       625     // 2.0 ms (Servo max)
#define SERVO_CENTER    469     // 1.5 ms

#define SERVO_PULSE     6250    // 20 ms

//*****************************************************************************
//
ISR(TIMER1_COMPA_vect)
{
        PORTD |= (1 << PD7);
}

//*****************************************************************************
//
ISR(TIMER1_COMPB_vect)
{
        PORTD &= ~(1 << PD7);
}

//*****************************************************************************
//
void servo_test (void)
{
        uint8_t chg = 0;
        uint8_t step = 10;
       
        DDRD |= (1 << DDD7);    // PD7 output

        TCCR1B = (1 << WGM12) | (1 << CS11) | (1 << CS10); // Prescaler 1/64, CTC mode
                                                                                                                                                                                                                 // 1 step = 3,2 µs (@ 20 MHz)

        OCR1A = SERVO_PULSE;

        OCR1B = SERVO_CENTER;

        TIMSK1 = (1 << OCIE1A) | (1 << OCIE1B);

        lcd_cls ();
       
        lcd_printp (PSTR("Servo Tester"), 0);
        //lcd_printpns_at (0, 7, PSTR("-1   +1     Exit  \x1d\x7c\x1c"), 0);
        lcd_printpns_at (0, 7, PSTR("-10  +10    Exit  \x1d\x7c\x1c"), 0);
       
        lcd_frect (SGX, SGY, ((OCR1B - SERVO_MIN) * SMAXGX) / (SERVO_MAX - SERVO_MIN), 10, 1);
        //write_ndigit_number_u (SMX, SMY, ((OCR1B - SERVO_MIN) * 100) / (SERVO_MAX - SERVO_MIN), 3, 0);        // Pulse width in %
        write_ndigit_number_u_100th(SMX, SMY, (OCR1B * 64) / 200 , 3, 0);       // Pulse width in ms

        do
        {
                if ((get_key_press (1 << KEY_PLUS) || get_key_rpt (1 << KEY_PLUS)) && (OCR1B < SERVO_MAX))
                {
                        OCR1B += step;
                        if (OCR1B > SERVO_MAX)
                        {
                                OCR1B = SERVO_MAX;
                        }
                        chg++;
                }
                else if ((get_key_press (1 << KEY_MINUS) || get_key_rpt (1 << KEY_MINUS)) && (OCR1B > SERVO_MIN))
                {
                        lcd_frect (SGX, SGY, ((OCR1B - SERVO_MIN) * SMAXGX) / (SERVO_MAX - SERVO_MIN), 10, 0);
                        OCR1B -= step;
                        if (OCR1B < SERVO_MIN)
                        {
                                OCR1B = SERVO_MIN;
                        }
                        chg++;
                }
                else if (get_key_short (1 << KEY_ENTER))
                {
                        lcd_frect (SGX, SGY, ((OCR1B - SERVO_MIN) * SMAXGX) / (SERVO_MAX - SERVO_MIN), 10, 0);
                        OCR1B = SERVO_CENTER;
                        chg++;
                }
                else if (get_key_long (1 << KEY_ENTER))
                {
                        if (step == 1)
                        {
                                step = 10;
                                lcd_printpns_at (0, 7, PSTR("-10  +10    Exit  \x1d\x7c\x1c"), 0);
                        }
                        else
                        {
                                step = 1;
                                lcd_printpns_at (0, 7, PSTR("-1   +1     Exit  \x1d\x7c\x1c"), 0);
                        }
                }
                if (chg)
                {
                        chg = 0;
                        lcd_frect (SGX, SGY, ((OCR1B - SERVO_MIN) * SMAXGX) / (SERVO_MAX - SERVO_MIN), 10, 1);
                        //write_ndigit_number_u (SMX, SMY, ((OCR1B - SERVO_MIN) * 100) / (SERVO_MAX - SERVO_MIN), 3, 0);        // Pulse width in %
                        write_ndigit_number_u_100th(SMX, SMY, (OCR1B * 64) / 200 , 3, 0);       // Pulse width in ms
                }
        }
        while (!get_key_press (1 << KEY_ESC));
        get_key_press(KEY_ALL);

        TIMSK1 = 0;
}