import collections
import statistics
import time
import pigpio
#https://www.parallax.com/sites/default/files/downloads/900-00360-Feedback-360-HS-Servo-v1.1.pdf
#http://gpiozero.readthedocs.io/en/stable/remote_gpio.html
#https://gpiozero.readthedocs.io/en/stable/recipes.html#pin-numbering
[docs]class write_pwm:
"""
Steers a Parallax Feedback 360° High-Speed Servo `360_data_sheet`_ .
This class steers a Parallax Feedback 360° High-Speed Servo. Out of the speed range,
defined by ``min_speed`` and ``max_speed``, and the range of the pulsewidth, defined
by ``min_pw`` and ``max_pw``, the class allows setting the servo speed and automatically
calculates the appropriate pulsewidth for the chosen speed value.
.. note::
``min_pw`` and ``max_pw`` might needed to be interchanged, depending on if ``min_pw`` is
moving the servo max_forward/clockwise or max_backwards/counter-clockwise,
see methods :meth:`max_forward` and :meth:`max_backward`.
:meth:`max_forward` -> ``min_pw`` should let the servo rotate clockwise.
.. warning::
Be carefull with setting the min and max pulsewidth! Test carefully ``min_pw`` and ``max_pw``
before setting them. Wrong values can damage the servo, see set_servo_pulsewidth_ !!!
:param pigpio.pi pi:
Instance of a pigpio.pi() object.
:param int gpio:
GPIO identified by their Broadcom number, see elinux.org_ .
To this GPIO the control wire of the servo has to be connected.
:param int min_pw:
Min pulsewidth, see **Warning**, carefully test the value before!
**Default:** 1280, taken from the data sheet `360_data_sheet`_ .
:param int max_pw:
Max pulsewidth, see **Warning**, carefully test the value before!
**Default:** 1720, taken from the data sheet `360_data_sheet`_ .
:param int min_speed:
Min speed which the servo is able to move. **Default:** -1.
:param int max_speed:
Max speed which the servo is able to move. **Default:** 1.
.. _elinux.org: https://elinux.org/RPi_Low-level_peripherals#Model_A.2B.2C_B.2B_and_B2
.. _set_servo_pulsewidth: http://abyz.me.uk/rpi/pigpio/python.html#set_servo_pulsewidth
.. _`360_data_sheet`: https://www.parallax.com/sites/default/files/downloads/900-00360-Feedback-360-HS-Servo-v1.1.pdf
"""
def __init__(self, pi, gpio, min_pw = 1280, max_pw = 1720, min_speed = -1, max_speed = 1):
self.pi = pi
self.gpio = gpio
self.min_pw = min_pw
self.max_pw = max_pw
self.min_speed = min_speed
self.max_speed = max_speed
#calculate slope for calculating the pulse width
self.slope = (self.min_pw - ((self.min_pw + self.max_pw)/2)) / self.max_speed
#calculate y-offset for calculating the pulse width
self.offset = (self.min_pw + self.max_pw)/2
[docs] def set_pw(self, pulse_width):
"""
Sets pulsewidth of the PWM.
This method allows setting the pulsewidth of the PWM directly. This can be used to
test which ``min_pw`` and ``max_pw`` are appropriate. For this the ``min_pw`` and ``max_pw``
are needed to be set very small and very big, so that they do not limit the set pulsewidth.
Normally they are used to protect the servo by limiting the pulsewidth to a certain range.
.. warning::
Be carefull with setting the min and max pulsewidth! Test carefully ``min_pw`` and ``max_pw``
before setting them. Wrong values can damage the servo, see set_servo_pulsewidth_ !!!
:param int,float pulsewidth:
Pulsewidth of the PWM signal. Will be limited to ``min_pw`` and ``max_pw``.
.. _set_servo_pulsewidth: http://abyz.me.uk/rpi/pigpio/python.html#set_servo_pulsewidth
"""
pulse_width = max(min(self.max_pw, pulse_width), self.min_pw)
self.pi.set_servo_pulsewidth(user_gpio = self.gpio, pulsewidth = pulse_width)
def calc_pw(self, speed):
pulse_width = self.slope * speed + self.offset
return pulse_width
[docs] def set_speed(self, speed):
"""
Sets speed of the servo.
This method sets the servos rotation speed. The speed range is defined by
by ``min_speed`` and ``max_speed`` .
:param int,float speed:
Should be between ``min_speed`` and ``max_speed`` ,
otherwise the value will be limited to those values.
"""
speed = max(min(self.max_speed, speed), self.min_speed)
calculated_pw = self.calc_pw(speed = speed)
self.set_pw(pulse_width = calculated_pw)
[docs] def stop(self):
"""
Sets the speed of the servo to 0.
"""
pulse_width = (self.min_pw+self.max_pw)/2
self.set_pw(pulse_width = pulse_width)
[docs] def max_backward(self):
"""
Sets the speed of the servo to -1, so ``min_speed`` (max backwards,
counter-clockwise)
"""
self.set_pw(self.max_pw)
[docs] def max_forward(self):
"""
Sets the speed of the servo to 1, so ``max_speed`` (max forward,
clockwise)
"""
self.set_pw(self.min_pw)
[docs]class read_pwm:
"""
Reads position of a Parallax Feedback 360° High-Speed Servo `360_data_sheet`_ .
This class reads the position of a Parallax Feedback 360° High-Speed Servo. At the
moment, the period for a 910 Hz signal is hardcoded.
:param pigpio.pi pi:
Instance of a pigpio.pi() object.
:param int gpio:
GPIO identified by their Broadcom number, see elinux.org_ .
To this GPIO the feedback wire of the servo has to be connected.
.. todo::
Enable the class to be able to handle different signals, not just 910 Hz.
.. _elinux.org: https://elinux.org/RPi_Low-level_peripherals#Model_A.2B.2C_B.2B_and_B2
.. _`360_data_sheet`: https://www.parallax.com/sites/default/files/downloads/900-00360-Feedback-360-HS-Servo-v1.1.pdf
"""
def __init__(self, pi, gpio):
self.pi = pi
self.gpio = gpio
self.period = 1/910*1000000
self.tick_high = None
self.duty_cycle = None
self.duty_scale = 1000
#http://abyz.me.uk/rpi/pigpio/python.html#set_mode
self.pi.set_mode(gpio=self.gpio, mode=pigpio.INPUT)
#http://abyz.me.uk/rpi/pigpio/python.html#callback
self.cb = self.pi.callback(user_gpio=self.gpio, edge=pigpio.EITHER_EDGE, func=self.cbf)
#calculate the duty cycle
def cbf(self, gpio, level, tick):
#change to low (a falling edge)
if level == 0:
#if first edge is a falling one the following code will fail
#a try first time is faster than an if-statement every time
try:
#http://abyz.me.uk/rpi/pigpio/python.html#callback
# tick 32 bit The number of microseconds since boot
# WARNING: this wraps around from
# 4294967295 to 0 roughly every 72 minutes
#Tested: This is handled by the tickDiff function internally, if t1 (earlier tick)
#is smaller than t2 (later tick), which could happen every 72 min. The result will
#not be a negative value, the real difference will be properly calculated.
self.duty_cycle = self.duty_scale*pigpio.tickDiff(t1=self.tick_high, t2=tick)/self.period
except Exception:
pass
#change to high (a rising edge)
elif level == 1:
self.tick_high = tick
[docs] def read(self):
"""
Returns the recent measured duty cycle.
This method returns the recent measured duty cycle.
:return: Recent measured duty cycle
:rtype: float
"""
return self.duty_cycle
[docs] def cancel(self):
"""
Cancel the started callback function.
This method cancels the started callback function if initializing an object.
As written in the pigpio callback_ documentation, the callback function may be cancelled
by calling the cancel function after the created instance is not needed anymore.
.. _callback: http://abyz.me.uk/rpi/pigpio/python.html#callback
"""
self.cb.cancel()
[docs]class calibrate_pwm:
"""
Calibrates a Parallax Feedback 360° High-Speed Servo with the help of the :class:`read_pwm` class.
This class helps to find out the min and max duty cycle of the feedback signal of a
servo. This values ( ``dcMin`` / ``dcMax`` ) are then needed in :ref:`lib_motion`
to have a more precise measurement of the position. The experience has shown that each
servo has slightly different min/max duty cycle values, different than the once
provided in the data sheet 360_data_sheet_ . Values smaller and bigger than the
printed out once as "duty_cycle_min/duty_cycle_max" are outliers and should
therefore not be considered. This can be seen in the printouts of smallest/biggest
250 values. There are sometimes a few outliers. Compare the printouts of different
runs to get a feeling for it.
.. note::
The robot wheels must be able to rotate free in the air for calibration.
Rotating forward or backward might sometimes give slightly
different results for min/max duty cycle. Choose the smallest value and the
biggest value out of the forward and backward runs. Do both directions three
times for each wheel, with speed = 0.2 and -0.2. Then
chose the values. The speed has to be set manually, see :ref:`Examples`.
:param pigpio.pi pi:
Instance of a pigpio.pi() object.
:param int gpio:
GPIO identified by their Broadcom number, see elinux.org_ .
To this GPIO the feedback wire of the servo has to be connected.
:param int,float measurement_time:
Time in seconds for how long duty cycle values will be collected, so for how long the
measurement will be made. **Default:** 120.
:returns: Printouts of different measurements
At the moment, the period for a 910 Hz signal is hardcoded, as in :meth:`read_pwm` .
.. todo::
Enable the class to be able to handle different signals, not just 910 Hz.
.. _elinux.org: https://elinux.org/RPi_Low-level_peripherals#Model_A.2B.2C_B.2B_and_B2
.. _`360_data_sheet`: https://www.parallax.com/sites/default/files/downloads/900-00360-Feedback-360-HS-Servo-v1.1.pdf
"""
def __init__(self, pi, gpio, measurement_time = 120):
self.pi = pi
self.gpio = gpio
self.period = 1/910*1000000
self.tick_high = None
self.duty_cycle = None
self.duty_scale = 1000
self.list_duty_cycles = []
self.duty_cycle_min = None
self.duty_cycle_max = None
#http://abyz.me.uk/rpi/pigpio/python.html#set_mode
self.pi.set_mode(gpio=self.gpio, mode=pigpio.INPUT)
#http://abyz.me.uk/rpi/pigpio/python.html#callback
self.cb = self.pi.callback(user_gpio=self.gpio, edge=pigpio.EITHER_EDGE, func=self.cbf)
print('{}{}{}'.format('Starting measurements for: ', measurement_time, ' seconds.'))
print('----------------------------------------------------------')
time.sleep(measurement_time)
#stop callback before sorting list to avoid getting added new elements unintended
#http://abyz.me.uk/rpi/pigpio/python.html#callback
self.cb.cancel()
time.sleep(1)
self.list_duty_cycles = sorted(self.list_duty_cycles)
#some analyzis of the dc values
sorted_set = list(sorted(set(self.list_duty_cycles)))
print('{} {}'.format('Ascending sorted distinct duty cycle values:', sorted_set))
print('----------------------------------------------------------')
differences_list = [sorted_set[i+1]-sorted_set[i] for i in range(len(sorted_set)-1)]
rounded_differences_list = [round(differences_list[i],2) for i in range(len(differences_list)-1)]
counted_sorted_list = collections.Counter(rounded_differences_list)
print('{} {}'.format('Ascending counted, sorted and rounded distinct differences between duty cycle values:',counted_sorted_list))
print('----------------------------------------------------------')
#Median and median_high/median_low are chosen, because the biggest
#and smallest values are needed, and not an avarage of the smallest
#and biggest values of the selection.
#https://docs.python.org/3/library/statistics.html#statistics.median
print('{} {}'.format('Smallest 250 values:', self.list_duty_cycles[:250]))
self.duty_cycle_min = statistics.median_high(self.list_duty_cycles[:20])
print('----------------------------------------------------------')
print('{} {}'.format('Biggest 250 values:',self.list_duty_cycles[-250:]))
self.duty_cycle_max = statistics.median_low(self.list_duty_cycles[-20:])
print('----------------------------------------------------------')
print('duty_cycle_min:', round(self.duty_cycle_min,2))
print('duty_cycle_max:', round(self.duty_cycle_max,2))
def cbf(self, gpio, level, tick):
#change to low (a falling edge)
if level == 0:
#if first edge is a falling one the following code will not work
#a try first time is faster than an if-statement every time
try:
self.duty_cycle = self.duty_scale*pigpio.tickDiff(t1=self.tick_high, t2=tick)/self.period
self.list_duty_cycles.append(self.duty_cycle)
except Exception:
pass
#change to high (a rising edge)
elif level == 1:
self.tick_high = tick
def cancel(self):
self.cb.cancel()
if __name__ == "__main__":
#just continue
pass