File Coverage

blib/lib/HiPi/Interface/PCA9685.pm
Criterion Covered Total %
statement 18 185 9.7
branch 0 64 0.0
condition 0 31 0.0
subroutine 6 24 25.0
pod 0 18 0.0
total 24 322 7.4


line stmt bran cond sub pod time code
1             #########################################################################################
2             # Package HiPi::Interface::PCA9685
3             # Description : Control NXP PCA9685 16-channel, 12-bit PWM Fm+ I2C-bus controller
4             # Copyright : Copyright (c) 2013-2017 Mark Dootson
5             # License : This is free software; you can redistribute it and/or modify it under
6             # the same terms as the Perl 5 programming language system itself.
7             #########################################################################################
8              
9             package HiPi::Interface::PCA9685;
10              
11             #########################################################################################
12              
13 1     1   1246 use strict;
  1         3  
  1         29  
14 1     1   5 use warnings;
  1         4  
  1         33  
15 1     1   6 use parent qw( HiPi::Interface );
  1         2  
  1         6  
16 1     1   63 use HiPi qw( :i2c :rpi :pca9685 );
  1         2  
  1         445  
17 1     1   7 use Carp;
  1         1  
  1         136  
18              
19             __PACKAGE__->create_ro_accessors( qw(
20             devicename clock frequency _servo_position _servo_types
21             external_clock internal_clock debug allcall
22             backend
23             ) );
24              
25             our $VERSION ='0.80';
26              
27             use constant {
28 1         2499 MODE1 => 0x00,
29            
30             RESTART => 0x80,
31             EXTCLK => 0x40,
32             AI => 0x20,
33             SLEEP => 0x10,
34             SUB1 => 0x08,
35             SUB2 => 0x04,
36             SUB3 => 0x02,
37             ALLCALL => 0x01,
38            
39             MODE2 => 0x01,
40            
41             INVRT => 0x10,
42             OCH => 0x08,
43             OUTDRV => 0x04,
44             OUTNE_HIMP => 0x02,
45             OUTNE_ODRAIN_HIMP => 0x01,
46             OUTNE_TOPOLE_ON => 0x01,
47            
48             SUBADR1 => 0x02,
49             SUBADR2 => 0x03,
50             SUBADR3 => 0x04,
51             ALLCALLADR => 0x05,
52            
53             CHAN_BASE => 0x06,
54            
55             ALL_CHAN => 0xFA,
56             PRE_SCALE => 0xFE,
57            
58             INTERNAL_CLOCK_MHZ => 25,
59            
60             CLEAR_REG => 0x00,
61 1     1   7 };
  1         3  
62              
63             sub new {
64 0     0 0   my ($class, %userparams) = @_;
65            
66 0           my $pi = HiPi::RaspberryPi->new();
67            
68 0 0         my %params = (
69             devicename => ( $pi->board_type == RPI_BOARD_TYPE_1 ) ? '/dev/i2c-0' : '/dev/i2c-1',
70             address => 0x40,
71             device => undef,
72             backend => 'smbus',
73             frequency => 50,
74             external_clock => 0,
75             internal_clock => INTERNAL_CLOCK_MHZ,
76             allcall => 0,
77             );
78            
79             # get user params
80 0           foreach my $key( keys (%userparams) ) {
81 0           $params{$key} = $userparams{$key};
82             }
83            
84 0 0         if( $params{clock} ) {
85 0           print q(you cannot set the clock param directly. If your board uses an external clock then pass its MHz frequency in the constructor:
86              
87             my $pwm = HiPi::Interface::PCA9685->new( external_clock => 16 );
88              
89             );
90 0           exit(1);
91             }
92            
93             # set internal params
94 0           $params{_servo_position} = [];
95            
96 0           $params{_servo_types} = [];
97            
98 0 0         if($params{external_clock}) {
99 0           $params{clock} = $params{external_clock};
100             } else {
101 0           $params{clock} = $params{internal_clock};
102             }
103            
104 0 0         unless( defined($params{device}) ) {
105 0 0         if ( $params{backend} eq 'bcm2835' ) {
106 0           require HiPi::BCM2835::I2C;
107             $params{device} = HiPi::BCM2835::I2C->new(
108             address => $params{address},
109 0 0         peripheral => ( $params{devicename} eq '/dev/i2c-0' ) ? HiPi::BCM2835::I2C::BB_I2C_PERI_0() : HiPi::BCM2835::I2C::BB_I2C_PERI_1(),
110             );
111             } else {
112 0           require HiPi::Device::I2C;
113             $params{device} = HiPi::Device::I2C->new(
114             devicename => $params{devicename},
115             address => $params{address},
116             busmode => $params{backend},
117 0           );
118             }
119             }
120            
121 0           my $self = $class->SUPER::new(%params);
122            
123 0           my $servotypes = [
124             # PCA_9685_SERVOTYPE_DEFAULT
125             {
126             pulse_min => 1000,
127             pulse_max => 2000,
128             degree_range => 160,
129             degree_min => 10,
130             degree_max => 170,
131             },
132             # PCA_9685_SERVOTYPE_DEFAULT
133             {
134             pulse_min => 1000,
135             pulse_max => 2000,
136             degree_range => 160,
137             degree_min => 10,
138             degree_max => 170,
139             },
140             # PCA_9685_SERVOTYPE_EXT_1
141             {
142             pulse_min => 600,
143             pulse_max => 2400,
144             degree_range => 160,
145             degree_min => 10,
146             degree_max => 170,
147             },
148             # PCA_9685_SERVOTYPE_EXT_2
149             {
150             pulse_min => 800,
151             pulse_max => 2200,
152             degree_range => 160,
153             degree_min => 10,
154             degree_max => 170,
155             },
156             # PCA_9685_SERVOTYPE_SG90
157             {
158             pulse_min => 550,
159             pulse_max => 2350,
160             degree_range => 150,
161             degree_min => 15,
162             degree_max => 165,
163             },
164            
165             ];
166            
167 0           for my $stype( @$servotypes ) {
168 0           $self->register_servotype(%$stype);
169             }
170              
171 0           $self->restart();
172              
173 0           return $self;
174             }
175              
176             sub restart {
177 0     0 0   my($self) = @_;
178            
179 0           my $prescale = $self->calculate_prescale;
180            
181 0 0         my $allcall = ( $self->allcall ) ? ALLCALL : 0;
182            
183             # set sleep register
184 0           $self->device->bus_write( MODE1, SLEEP );
185            
186             # set prescale
187 0           $self->device->bus_write( PRE_SCALE, $prescale );
188            
189             # external clock ?
190 0 0         if( $self->external_clock ) {
191 0           $self->device->bus_write( MODE1, SLEEP | EXTCLK );
192             }
193            
194             # bring out of sleep
195 0           $self->device->bus_write( MODE1, CLEAR_REG | $allcall );
196 0           $self->delay( 10 );
197            
198             # use autoincrement and restart
199 0           $self->device->bus_write( MODE1, RESTART | AI | $allcall );
200             }
201              
202             sub calculate_prescale {
203 0     0 0   my $self = shift;
204 0           my $prescale = int( 0.5 + ( $self->clock * 1000000.0 ) / ( 4096.0 * $self->frequency ) ) -1 ;
205             # hardware defines a minimum value of 3 anyway so we can avoid returning a zero value
206 0   0       $prescale ||= 3;
207 0           return $prescale;
208             }
209              
210             sub set_servo_degrees {
211 0     0 0   my($self, $channel, $servotype, $degrees, $delay ) = @_;
212            
213 0           my $position;
214            
215 0 0 0       if( $delay && $delay > 0 ) {
216             # delay defined in microseconds
217            
218 0 0         if( defined( $self->_servo_position->[$channel] ) ) {
219 0           $position = $self->_servo_position->[$channel];
220             } else {
221             # read it from device
222 0           my ( $on, $duration ) = $self->read_channel( $channel ) ;
223 0           $duration &= PCA_9685_SERVO_CHANNEL_MASK;
224 0   0       $position = $duration || undef;
225             }
226             }
227            
228             # return if nothing bo do
229            
230 0           my $desired_postion = $self->servo_degrees_to_duration($servotype, $degrees);
231            
232 0 0 0       return $position if defined($position) && $position == $desired_postion;
233            
234 0 0 0       my $increment = ( defined($position) && $position > $desired_postion ) ? -1 : 1;
235            
236 0   0       $position //= $desired_postion - $increment;
237            
238 0           while( $position != $desired_postion ) {
239 0           $position += $increment;
240 0           $self->write_channel( $channel, 0x00, $position & PCA_9685_SERVO_CHANNEL_MASK );
241 0 0         $self->delayMicroseconds( $delay ) if $delay;
242             }
243            
244 0           $self->_servo_position->[$channel] = $position;
245            
246 0           return $position;
247             }
248              
249             sub get_servo_degrees {
250 0     0 0   my( $self, $channel, $servotype ) = @_;
251 0           my ( $on, $duration ) = $self->read_channel( $channel ) ;
252 0           $duration &= PCA_9685_SERVO_CHANNEL_MASK;
253 0           my $degrees = $self->servo_duration_to_degrees($servotype, $duration);
254 0           return $degrees;
255             }
256              
257             sub set_servo_pulse {
258 0     0 0   my( $self, $channel, $us ) = @_;
259 0           my $duration = $self->microseconds_to_duration( $us );
260 0           $self->write_channel( $channel, 0x00, $duration & PCA_9685_SERVO_CHANNEL_MASK );
261 0           return $duration;
262             }
263              
264             sub get_servo_pulse {
265 0     0 0   my( $self, $channel ) = @_;
266 0           my ( $on, $duration ) = $self->read_channel( $channel ) ;
267 0           $duration &= PCA_9685_SERVO_CHANNEL_MASK;
268 0           my $us = $self->duration_to_microseconds($duration);
269 0           return $us;
270             }
271              
272             sub sleep {
273 0     0 0   my $self = shift;
274 0           $self->device->bus_write( MODE1, SLEEP );
275             }
276              
277             sub read_channel {
278 0     0 0   my( $self, $channel ) = @_;
279            
280 0   0       $channel //= 0;
281            
282 0           my ( $on_lsb, $on_msb, $off_lsb, $off_msb ) = $self->device->bus_read( CHAN_BASE + ( 4 * $channel ) , 4 );
283            
284 0           my $on = ( ( $on_msb & 0x1F ) << 8 ) + $on_lsb;
285 0           my $off = ( ( $off_msb & 0x1F ) << 8 ) + $off_lsb;
286            
287 0           return ( $on, $off );
288             }
289              
290             sub write_channel {
291 0     0 0   my( $self, $channel, $on, $off ) = @_;
292            
293 0   0       $on //= 0;
294 0   0       $off //= 0;
295            
296 0           my $on_lsb = $on & 0xFF;
297 0           my $on_msb = ( $on & 0x1F00 ) >> 8;
298 0           my $off_lsb = $off & 0xFF;
299 0           my $off_msb = ( $off & 0x1F00 ) >> 8;
300            
301 0           $self->device->bus_write( CHAN_BASE + ( 4 * $channel ), $on_lsb, $on_msb, $off_lsb, $off_msb );
302             }
303              
304              
305             sub microseconds_to_duration {
306 0     0 0   my( $self, $us ) = @_;
307 0   0       $us ||= 100;
308 0           my $period_us = 1000000.0 / $self->frequency;
309 0           my $duration_percent = ( $us / $period_us ) * 100.0;
310 0           my $duration = 4096.0 * ( $duration_percent / 100.0 );
311 0           $duration = int( 0.5 + $duration ) - 1;
312 0 0         if( $self->debug ) {
313 0           warn qq($us microseconds converted to duration $duration);
314             }
315 0           return $duration;
316             }
317            
318             sub duration_to_microseconds {
319 0     0 0   my( $self, $duration ) = @_;
320 0 0         return 0 unless $duration;
321 0           $duration ++;
322 0           my $duration_percent = ( $duration / 4096.0 ) * 100.0;
323 0           my $period_us = 1000000.0 / $self->frequency;
324 0           my $us = int( 0.5 + (( $period_us /100 ) * $duration_percent));
325 0 0         if( $self->debug ) {
326 0           warn qq($us microseconds converted from duration $duration);
327             }
328 0           return $us;
329             }
330              
331             sub servo_degrees_to_pulse {
332 0     0 0   my ( $self, $servotype, $degrees) = @_;
333 0           my $svc = $self->servo_type_config( $servotype );
334 0   0       $degrees //= 90;
335 0 0         $degrees = $svc->{limit_min} if $degrees < $svc->{limit_min};
336 0 0         $degrees = $svc->{limit_max} if $degrees > $svc->{limit_max};
337             my $us = $svc->{pulse_min} +
338 0           int( 0.5 + ( ( $degrees - $svc->{degree_min} ) * $svc->{pw_per_degree} ) );
339            
340 0 0         if($self->debug) {
341 0           warn qq($degrees degrees converted to pulse $us);
342             }
343 0           return $us;
344             }
345              
346             sub servo_pulse_to_degrees {
347 0     0 0   my ( $self, $servotype, $us) = @_;
348 0           my $svc = $self->servo_type_config( $servotype );
349 0   0       $us ||= $svc->{pulse_mid};
350 0 0         $us = $svc->{pulse_min} if $us < $svc->{pulse_min};
351 0 0         $us = $svc->{pulse_max} if $us > $svc->{pulse_max};
352 0 0         return 90 if $us == $svc->{pulse_mid};
353 0           my $degrees = $svc->{degree_min} + int( 0.5 + ($us - $svc->{pulse_min}) / $svc->{pw_per_degree} );
354 0 0         if($self->debug) {
355 0           warn qq($us pulse converted to degrees $degrees);
356             }
357 0           return $degrees;
358             }
359              
360             sub servo_degrees_to_duration {
361 0     0 0   my($self, $servotype, $degrees ) = @_;
362 0           my $us = $self->servo_degrees_to_pulse($servotype, $degrees);
363 0           my $duration = $self->microseconds_to_duration($us);
364 0 0         if($self->debug) {
365 0           warn qq($degrees degrees converted to duration $duration);
366             }
367 0           return $duration;
368             }
369              
370             sub servo_duration_to_degrees {
371 0     0 0   my($self, $servotype, $duration) = @_;
372 0           my $us = $self->duration_to_microseconds($duration);
373 0           my $degrees = $self->servo_pulse_to_degrees($servotype, $us);
374 0 0         if($self->debug) {
375 0           warn qq($duration duration converted to degrees $degrees);
376             }
377 0           return $degrees;
378             }
379              
380             sub servo_type_config {
381 0     0 0   my ($self, $type) = @_;
382            
383 0   0       $type //= PCA_9685_SERVOTYPE_DEFAULT;
384            
385 0 0         if( exists($self->_servo_types->[$type] ) ) {
386 0           return { %{ $self->_servo_types->[$type] } };
  0            
387             } else {
388 0           carp 'unknown servo type specified';
389 0           return { %{ $self->_servo_types->[PCA_9685_SERVOTYPE_DEFAULT] } };
  0            
390             }
391             }
392              
393             sub register_servotype {
394 0     0 0   my($self, %params) = @_;
395 0           for my $param ( qw( pulse_min pulse_max degree_range ) ) {
396 0 0         unless(exists($params{$param})) {
397 0           carp(q(you must provide parameters pulse_min, pulse_max, and degree_range));
398 0           return undef;
399             }
400             }
401 0 0         unless($params{pulse_max} > $params{pulse_min}) {
402 0           carp(q(pulse_max must be greater than pulse_min));
403 0           return undef;
404             }
405            
406 0           my $index = scalar @{ $self->_servo_types };
  0            
407 0           my $pulse_band = $params{pulse_max} - $params{pulse_min};
408 0           my $pw_per_degree = $pulse_band / $params{degree_range};
409 0           my $degree_min = int( 90.5 - ( $params{degree_range} / 2.0 ));
410 0           my $degree_max = $degree_min + $params{degree_range};
411            
412             my $pulse_mid = $params{pulse_min} +
413 0           int( 0.5 + ( ( 90.0 - $degree_min ) * $pw_per_degree ) );
414            
415 0           my $limit_max = $degree_max;
416 0 0         if(exists($params{degree_max})) {
417 0 0         $limit_max = $params{degree_max} if $limit_max > $params{degree_max};
418             }
419 0           my $limit_min = $degree_min;
420 0 0         if(exists($params{degree_min})) {
421 0 0         $limit_min = $params{degree_min} if $limit_min < $params{degree_min};
422             }
423            
424             $self->_servo_types->[$index] = {
425             pulse_min => $params{pulse_min},
426             pulse_max => $params{pulse_max},
427             pulse_mid => $pulse_mid,
428             degree_range => $params{degree_range},
429 0           degree_min => $degree_min,
430             degree_max => $degree_max,
431             pw_per_degree => $pw_per_degree,
432             limit_min => $limit_min,
433             limit_max => $limit_max,
434             };
435 0           return $index;
436             }
437             1;
438              
439             __END__