28
28
#include " math.h"
29
29
#include " FspTimer.h"
30
30
31
- // uncomment to print servo Debug information
32
- // #define SERVO_PRINT_DEBUG_INFO
33
-
34
31
#define SERVO_MAX_SERVOS (_Nbr_16timers * SERVOS_PER_TIMER)
35
32
#define SERVO_INVALID_INDEX (255 )
36
33
// Lower the timer ticks for finer resolution.
37
34
#define SERVO_TIMER_TICK_US (100 )
38
35
#define SERVO_US_PER_CYCLE (20000 )
39
36
#define SERVO_IO_PORT_ADDR (pn ) &((R_PORT0 + ((uint32_t ) (R_PORT1 - R_PORT0) * (pn)))->PCNTR3)
40
-
41
37
#define MIN_CYCLE_OFF_US 50
42
38
43
39
// Internal Servo sturct to keep track of RA configuration.
@@ -76,21 +72,13 @@ static uint32_t usToticks(uint32_t time_us) {
76
72
}
77
73
78
74
79
-
80
-
81
75
static int servo_timer_config (uint32_t period_us)
82
76
{
83
77
static bool configured = false ;
84
78
if (configured == false ) {
85
79
// Configure and enable the servo timer, for full 20ms
86
80
uint8_t type = 0 ;
87
81
int8_t channel = FspTimer::get_available_timer (type);
88
- #ifdef SERVO_PRINT_DEBUG_INFO
89
- Serial.print (" \n servo_timer_config: type:" );
90
- Serial.print (type, DEC);
91
- Serial.print (" Channel: " );
92
- Serial.println (channel, DEC);
93
- #endif
94
82
if (channel != -1 ) {
95
83
// lets initially configure the servo to 50ms
96
84
servo_timer.begin (TIMER_MODE_PERIODIC, type, channel,
@@ -110,26 +98,13 @@ static int servo_timer_config(uint32_t period_us)
110
98
// Now lets see what the period;
111
99
servo_ticks_per_cycle = servo_timer.get_period_raw ();
112
100
min_servo_cycle_low = usToticks (MIN_CYCLE_OFF_US);
113
- #ifdef SERVO_PRINT_DEBUG_INFO
114
- Serial.print (" Period:" );
115
- Serial.println (servo_ticks_per_cycle, DEC);
116
- uint32_t ticks_544 = usToticks (544 );
117
- uint32_t ticks_2400 = usToticks (2400 );
118
- Serial.print (" Min 544(ticks): " );
119
- Serial.print (ticks_544);
120
- Serial.print (" Max 2400: " );
121
- Serial.print (ticks_2400);
122
- Serial.print (" per degree: " );
123
- Serial.println ((float )(ticks_2400 - ticks_544) / 180 .0f , 2 );
124
- #endif
125
101
126
102
configured = true ;
127
103
}
128
104
}
129
105
return configured ? 0 : -1 ;
130
106
}
131
107
132
-
133
108
static int servo_timer_start ()
134
109
{
135
110
// Start the timer if it's not started
@@ -165,9 +140,9 @@ inline static void updateClockPeriod(uint32_t period) {
165
140
void servo_timer_callback (timer_callback_args_t *args)
166
141
{
167
142
(void )args; // remove warning
168
- static uint8_t channel= SERVO_MAX_SERVOS;
169
- static uint8_t channel_pin_set_high= 0xff ;
170
- static uint32_t ticks_accum= 0 ;
143
+ static uint8_t channel = SERVO_MAX_SERVOS;
144
+ static uint8_t channel_pin_set_high = 0xff ;
145
+ static uint32_t ticks_accum = 0 ;
171
146
172
147
// See if we need to set a servo back low
173
148
if (channel_pin_set_high != 0xff ) {
@@ -254,7 +229,6 @@ uint8_t Servo::attach(int pin, int min, int max)
254
229
if (servo_timer_start () != 0 ) {
255
230
return 0 ;
256
231
}
257
-
258
232
return 1 ;
259
233
}
260
234
0 commit comments