Subversion Repositories Projects

Compare Revisions

Ignore whitespace Rev 729 → Rev 730

/Transportables_Koptertool/trunk/servo.c
0,0 → 1,128
/*****************************************************************************
* 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;
}