-
Notifications
You must be signed in to change notification settings - Fork 0
/
tlc_servos.h
154 lines (135 loc) · 4.81 KB
/
tlc_servos.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
/* Copyright (c) 2009 by Alex Leone <acleone ~AT~ gmail.com>
This file is part of the Arduino TLC5940 Library.
The Arduino TLC5940 Library 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 3 of the
License, or (at your option) any later version.
The Arduino TLC5940 Library 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 The Arduino TLC5940 Library. If not, see
<http://www.gnu.org/licenses/>. */
#ifndef TLC_SERVOS_H
#define TLC_SERVOS_H
/** \file
TLC servo functions. */
#include "Tlc5940.h"
#ifndef SERVO_MAX_ANGLE
/** The maximum angle of the servo. */
#define SERVO_MAX_ANGLE 180
#endif
#ifndef SERVO_MIN_WIDTH
/** The 1ms pulse width for zero degrees (0 - 4095). */
#define SERVO_MIN_WIDTH 204
#endif
#ifndef SERVO_MAX_WIDTH
/** The 2ms pulse width for 180 degrees (0 - 4095). */
#define SERVO_MAX_WIDTH 410
#endif
#ifndef SERVO_TIMER1_TOP
/** The top value for XLAT and BLANK pulses. This is with the div8 prescale,
so
\f$\displaystyle f_{PWM} = \frac{f_{osc}}{2 * 8 * SERVO\_TIMER1\_TOP} \f$
The default is 20000, which corresponds to 50Hz. */
#define SERVO_TIMER1_TOP 20000
#endif
#ifndef SERVO_TIMER2_TOP
/** The top value for GSCLK pulses. Related to SERVO_TIMER1_TOP by
\f$\displaystyle SERVO\_TIMER2\_TOP =
\frac{2 * 8 * SERVO\_TIMER1\_TOP}{4096} - 1 \f$
The default is 77. */
#define SERVO_TIMER2_TOP 77
#endif
void tlc_initServos(uint8_t initAngle = 0);
void tlc_setServo(TLC_CHANNEL_TYPE channel, uint8_t angle);
uint8_t tlc_getServo(TLC_CHANNEL_TYPE channel);
uint16_t tlc_angleToVal(uint8_t angle);
uint8_t tlc_valToAngle(uint16_t value);
/** \addtogroup ExtendedFunctions
\code #include "tlc_servos.h" \endcode
- void tlc_initServos(uint8_t initAngle = 0) - initializes the tlc for
servos.
- void tlc_setServo(TLC_CHANNEL_TYPE channel, uint8_t angle) - sets a
servo to an angle
- uint8_t tlc_getServo(TLC_CHANNEL_TYPE channel) - gets the currently set
servo angle */
/* @{ */
/** Initializes the tlc.
\param initAngle the initial angle to set all servos to
(0 - SERVO_MAX_ANGLE). */
void tlc_initServos(uint8_t initAngle)
{
Tlc.init(tlc_angleToVal(initAngle));
#if defined(__AVR__)
TCCR1B &= ~(_BV(CS12) | _BV(CS11) | _BV(CS10)); // stop timer1
ICR1 = SERVO_TIMER1_TOP;
TCNT1 = 0;
#ifdef TLC_ATMEGA_8_H
uint8_t oldTCCR2 = TCCR2;
TCCR2 = 0;
TCNT2 = 0;
OCR2 = SERVO_TIMER2_TOP / 2;
TCCR2 = oldTCCR2;
#elif defined(TLC_TIMER3_GSCLK)
uint8_t oldTCCR3B = TCCR3B;
TCCR3B = 0;
TCNT3 = 0;
ICR3 = SERVO_TIMER2_TOP;
TCCR3B = oldTCCR3B;
#else
uint8_t oldTCCR2B = TCCR2B;
TCCR2B = 0;
TCNT2 = 0;
OCR2A = SERVO_TIMER2_TOP;
TCCR2B = oldTCCR2B;
#endif
TCCR1B |= _BV(CS11); // start timer1 with div 8 prescale
#elif defined(__arm__) && defined(TEENSYDUINO)
//clear_XLAT_interrupt();
uint32_t sc = FTM1_SC;
FTM1_SC = 0; // stop timer
CMT_MSC = 0;
CMT_CGH1 = TLC_TIMER_TEENSY3_SERVO_CGH1;
CMT_CGL1 = TLC_TIMER_TEENSY3_SERVO_CGL1;
CMT_MSC = 0x01; // GSCLK target is 204800 Hz
// TODO: this reconfiguration of FTM1 crashes... why?
// pull requests would be most welcome! ;-)
FTM1_CNT = 0;
FTM1_MOD = TLC_TIMER_TEENSY3_SERVO_MOD;
FTM1_C0V = TLC_TIMER_TEENSY3_SERVO_MOD - TLC_TIMER_TEENSY3_SERVO_CV;
FTM1_C1V = TLC_TIMER_TEENSY3_SERVO_MOD - TLC_TIMER_TEENSY3_SERVO_CV - 1;
FTM1_SC = FTM_SC_CLKS(1) | FTM_SC_CPWMS | TLC_TIMER_TEENSY3_SERVO_PS;
#endif
}
/** Sets a servo on channel to angle.
\param channel which channel to set
\param angle (0 - SERVO_MAX_ANGLE) */
void tlc_setServo(TLC_CHANNEL_TYPE channel, uint8_t angle)
{
Tlc.set(channel, tlc_angleToVal(angle));
}
/** Gets the current angle that channel is set to.
\param channel which channel to get */
uint8_t tlc_getServo(TLC_CHANNEL_TYPE channel)
{
return tlc_valToAngle(Tlc.get(channel));
}
/** Converts and angle (0 - SERVO_MAX_ANGLE) to the inverted tlc channel value
(4095 - 0). */
uint16_t tlc_angleToVal(uint8_t angle)
{
return 4095 - SERVO_MIN_WIDTH - (
((uint16_t)(angle) * (uint16_t)(SERVO_MAX_WIDTH - SERVO_MIN_WIDTH))
/ SERVO_MAX_ANGLE);
}
/** Converts an inverted tlc channel value (4095 - 0) into an angle (0 -
SERVO_MAX_ANGLE). */
uint8_t tlc_valToAngle(uint16_t value)
{
return SERVO_MAX_ANGLE * (4095 - SERVO_MIN_WIDTH - value)
/ (SERVO_MAX_WIDTH - SERVO_MIN_WIDTH);
}
/* @} */
#endif