Subversion Repositories Projects

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
730 woggle 1
/*****************************************************************************
2
 *   Copyright (C) 2009 Peter "woggle" Mack, mac@denich.net                  *
3
 *                                                                           *
4
 *   This program is free software; you can redistribute it and/or modify    *
5
 *   it under the terms of the GNU General Public License as published by    *
6
 *   the Free Software Foundation; either version 2 of the License.          *
7
 *                                                                           *
8
 *   This program is distributed in the hope that it will be useful,         *
9
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of          *
10
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the           *
11
 *   GNU General Public License for more details.                            *
12
 *                                                                           *
13
 *   You should have received a copy of the GNU General Public License       *
14
 *   along with this program; if not, write to the                           *
15
 *   Free Software Foundation, Inc.,                                         *
16
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.               *
17
 *                                                                           *
18
 *****************************************************************************/
19
 
20
#include <avr/io.h>
21
#include <avr/interrupt.h>
22
#include <avr/pgmspace.h>
23
 
24
#include "lcd.h"
25
#include "timer.h"      // Keys
26
#include "servo.h"
27
 
28
#define SERVO_MIN                       313     // 1.0 ms (Servo min)
29
#define SERVO_MAX                       625     // 2.0 ms (Servo max)
30
#define SERVO_CENTER    469     // 1.5 ms
31
 
32
#define SERVO_PULSE     6250    // 20 ms
33
 
34
//*****************************************************************************
35
// 
36
ISR(TIMER1_COMPA_vect)
37
{
38
        PORTD |= (1 << PD7);
39
}
40
 
41
//*****************************************************************************
42
// 
43
ISR(TIMER1_COMPB_vect)
44
{
45
        PORTD &= ~(1 << PD7);
46
}
47
 
48
//*****************************************************************************
49
// 
50
void servo_test (void)
51
{
52
        uint8_t chg = 0;
53
        uint8_t step = 10;
54
 
55
        DDRD |= (1 << DDD7);    // PD7 output
56
 
57
        TCCR1B = (1 << WGM12) | (1 << CS11) | (1 << CS10); // Prescaler 1/64, CTC mode
58
                                                                                                                                                                                                                 // 1 step = 3,2 ┬Ás (@ 20 MHz)
59
 
60
        OCR1A = SERVO_PULSE;
61
 
62
        OCR1B = SERVO_CENTER;
63
 
64
        TIMSK1 = (1 << OCIE1A) | (1 << OCIE1B);
65
 
66
        lcd_cls ();
67
 
68
        lcd_printp (PSTR("Servo Tester"), 0);
69
        //lcd_printpns_at (0, 7, PSTR("-1   +1     Exit  \x1d\x7c\x1c"), 0);
70
        lcd_printpns_at (0, 7, PSTR("-10  +10    Exit  \x1d\x7c\x1c"), 0);
71
 
72
        lcd_frect (SGX, SGY, ((OCR1B - SERVO_MIN) * SMAXGX) / (SERVO_MAX - SERVO_MIN), 10, 1);
73
        //write_ndigit_number_u (SMX, SMY, ((OCR1B - SERVO_MIN) * 100) / (SERVO_MAX - SERVO_MIN), 3, 0);        // Pulse width in %
74
        write_ndigit_number_u_100th(SMX, SMY, (OCR1B * 64) / 200 , 3, 0);       // Pulse width in ms
75
 
76
        do
77
        {
78
                if ((get_key_press (1 << KEY_PLUS) || get_key_rpt (1 << KEY_PLUS)) && (OCR1B < SERVO_MAX))
79
                {
80
                        OCR1B += step;
81
                        if (OCR1B > SERVO_MAX)
82
                        {
83
                                OCR1B = SERVO_MAX;
84
                        }
85
                        chg++;
86
                }
87
                else if ((get_key_press (1 << KEY_MINUS) || get_key_rpt (1 << KEY_MINUS)) && (OCR1B > SERVO_MIN))
88
                {
89
                        lcd_frect (SGX, SGY, ((OCR1B - SERVO_MIN) * SMAXGX) / (SERVO_MAX - SERVO_MIN), 10, 0);
90
                        OCR1B -= step;
91
                        if (OCR1B < SERVO_MIN)
92
                        {
93
                                OCR1B = SERVO_MIN;
94
                        }
95
                        chg++;
96
                }
97
                else if (get_key_short (1 << KEY_ENTER))
98
                {
99
                        lcd_frect (SGX, SGY, ((OCR1B - SERVO_MIN) * SMAXGX) / (SERVO_MAX - SERVO_MIN), 10, 0);
100
                        OCR1B = SERVO_CENTER;
101
                        chg++;
102
                }
103
                else if (get_key_long (1 << KEY_ENTER))
104
                {
105
                        if (step == 1)
106
                        {
107
                                step = 10;
108
                                lcd_printpns_at (0, 7, PSTR("-10  +10    Exit  \x1d\x7c\x1c"), 0);
109
                        }
110
                        else
111
                        {
112
                                step = 1;
113
                                lcd_printpns_at (0, 7, PSTR("-1   +1     Exit  \x1d\x7c\x1c"), 0);
114
                        }
115
                }
116
                if (chg)
117
                {
118
                        chg = 0;
119
                        lcd_frect (SGX, SGY, ((OCR1B - SERVO_MIN) * SMAXGX) / (SERVO_MAX - SERVO_MIN), 10, 1);
120
                        //write_ndigit_number_u (SMX, SMY, ((OCR1B - SERVO_MIN) * 100) / (SERVO_MAX - SERVO_MIN), 3, 0);        // Pulse width in %
121
                        write_ndigit_number_u_100th(SMX, SMY, (OCR1B * 64) / 200 , 3, 0);       // Pulse width in ms
122
                }
123
        }
124
        while (!get_key_press (1 << KEY_ESC));
125
        get_key_press(KEY_ALL);
126
 
127
        TIMSK1 = 0;
128
}