import math
import statistics
import time
import pigpio
import lib_para_360_servo
[docs]class control:
"""
Controls the robot movement.
This class controls the robots movement by controlling the two Parallax Feedback 360°
High-Speed Servos `360_data_sheet`_ . The robots local coordinate system is defined in
the following way. X-axis positive is straight forward, y-axis positive is perpendicular
to the x-axis in the direction to the left wheel. The center (0/0) is where the middle
of the robots chassi is cutting across a imaginary line through both wheels/servos.
Angle phi is the displacement of the local coordinate system to the real world
coordinate system. See :ref:`Used_local_coordinate_system` for a picture of it.
.. 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 width_robot:
Width of the robot in mm, so distance between middle right wheel and middle
left wheel.
**Default:** 102 mm, measured.
:param diameter_wheels:
Diameter of both wheels.
**Default:** 66 mm, measured and taken from the products website `wheel_robot`_ .
:param int unitsFC:
Units in a full circle, so each wheel is divided into X sections/ticks.
This value should not be changed.
**Default:** 360
:param float dcMin_l:
Min duty cycle of the left wheel.
**Default:** 27.3, measured with method :meth:`lib_para_360_servo.calibrate_pwm` ,
see :ref:`Examples` .
:param float dcMax_l:
Max duty cycle of the left wheel.
**Default:** 969.15, measured with method :meth:`lib_para_360_servo.calibrate_pwm` ,
see :ref:`Examples` .
:param float dcMin_r:
Min duty cycle of the right wheel.
**Default:** 27.3, measured with method :meth:`lib_para_360_servo.calibrate_pwm` ,
see :ref:`Examples` .
:param float dcMax_r:
Max duty cycle of the left wheel.
**Default:** 978.25, measured with method :meth:`lib_para_360_servo.calibrate_pwm` ,
see :ref:`Examples` .
:param int l_wheel_gpio:
GPIO identified by their Broadcom number, see elinux.org_ .
To this GPIO the feedback wire of the left servo has to be connected.
:param int r_wheel_gpio:
GPIO identified by their Broadcom number, see elinux.org_ .
To this GPIO the feedback wire of the right servo has to be connected.
:param int servo_l_gpio:
GPIO identified by their Broadcom number, see elinux.org_ .
To this GPIO the control wire of the left servo has to be connected.
:param int min_pw_l:
Min pulsewidth, see **Warning**, carefully test the value before!
**Default:** 1280, taken from the data sheet `360_data_sheet`_ .
:param int max_pw_l:
Max pulsewidth, see **Warning**, carefully test the value before!
**Default:** 1720, taken from the data sheet `360_data_sheet`_ .
:param int min_speed_l:
Min speed which the servo is able to move. **Default:** -1, so that
the speed range is also scaled between -1 and 1 as the output of
the inner control loop.
:param int max_speed_l:
Max speed which the servo is able to move. **Default:** 1, so that
the speed range is also scaled between -1 and 1 as the output of
the inner control loop.
:param int servo_r_gpio:
GPIO identified by their Broadcom number, see elinux.org_ .
To this GPIO the control wire of the right servo has to be connected.
:param int min_pw_r:
Min pulsewidth, see **Warning**, carefully test the value before!
**Default:** 1280, taken from the data sheet `360_data_sheet`_ .
:param int max_pw_r:
Max pulsewidth, see **Warning**, carefully test the value before!
**Default:** 1720, taken from the data sheet `360_data_sheet`_ .
:param int min_speed_r:
Min speed which the servo is able to move. **Default:** -1, so that
the speed range is also scaled between -1 and 1 as the output of
the inner control loop.
:param int max_speed_r:
Max speed which the servo is able to move. **Default:** 1, so that
the speed range is also scaled between -1 and 1 as the output of
the inner control loop.
:param float sampling_time:
Sampling time of the four PID controllers in seconds.
**Default:** 0.01.
1. PWM of motor feedback is 910Hz (0,001098901 s), so position changes cannot
be recognized faster than 1.1 ms. Therefore, it is not needed to run the outer control
loop more often and update the speed values which have a 50 Hz (20ms) PWM.
2. Tests of the runtime of the code including the controller part have shown that
writing the pulsewidth (pi.set_servo_pulsewidth()) in :meth:`lib_para_360_servo.write_pwm.set_pw` is
the bottleneck which drastically slows down the code by the factor ~400
(0,002 seconds vs 0,000005 seconds; runtime with vs without writing pulsewidth).
3. For recognizing the RPMs of the wheels 10ms is needed to have enough changes in the
position. This was found out by testing. See method :meth:`move` for more informations.
:param int,float Kp_p:
Kp value of the outer PID controllers, see method :meth:`move`
for more informations.
**Default:** 0.1.
:param int,float Ki_p:
Ki value of the outer PID controllers, see method :meth:`move`
for more informations.
**Default:** 0.1.
:param int,float Kd_p:
Kd value of the outer PID controllers, see method :meth:`move`
for more informations.
**Default:** 0.
:param int,float Kp_s:
Kp value of the inner PID controllers, see method :meth:`move`
for more informations.
**Default:** 0.5.
:param int,float Ki_s:
Ki value of the inner PID controllers, see method :meth:`move`
for more informations.
**Default:** 0.
:param int,float Kd_s:
Kd value of the inner PID controllers, see method :meth:`move`
for more informations.
**Default:** 0.
.. _`360_data_sheet`: https://www.parallax.com/sites/default/files/downloads/900-00360-Feedback-360-HS-Servo-v1.1.pdf
.. _`wheel_robot`: https://www.parallax.com/product/28114
.. _elinux.org: https://elinux.org/RPi_Low-level_peripherals#Model_A.2B.2C_B.2B_and_B2
"""
def __init__(
self, pi, width_robot = 102, diameter_wheels = 66, unitsFC = 360,
dcMin_l = 27.3, dcMax_l = 969.15,
dcMin_r = 27.3, dcMax_r = 978.25,
l_wheel_gpio = 16, r_wheel_gpio = 20,
servo_l_gpio = 17, min_pw_l = 1280, max_pw_l = 1720, min_speed_l = -1, max_speed_l = 1,
servo_r_gpio = 27, min_pw_r = 1280, max_pw_r = 1720, min_speed_r = -1, max_speed_r = 1,
sampling_time = 0.01,
Kp_p = 0.1, #not too big values, otherwise output of position control would slow down too abrupt
Ki_p = 0.1,
Kd_p = 0,
Kp_s = 0.5,
Ki_s = 0,
Kd_s = 0):
self.pi = pi
self.width_robot = width_robot
self.diameter_wheels = diameter_wheels
self.unitsFC = unitsFC
self.dcMin_l = dcMin_l
self.dcMax_l = dcMax_l
self.dcMin_r = dcMin_r
self.dcMax_r = dcMax_r
self.sampling_time = sampling_time
self.Kp_p = Kp_p
self.Ki_p = Ki_p
self.Kd_p = Kd_p
self.Kp_s = Kp_s
self.Ki_s = Ki_s
self.Kd_s = Kd_s
self.l_wheel = lib_para_360_servo.read_pwm(pi = self.pi, gpio = l_wheel_gpio)
self.r_wheel = lib_para_360_servo.read_pwm(pi = self.pi, gpio = r_wheel_gpio)
self.servo_l = lib_para_360_servo.write_pwm(pi = self.pi, gpio = servo_l_gpio, min_pw = min_pw_l, max_pw = max_pw_l, min_speed = min_speed_l, max_speed = max_speed_l)
self.servo_r = lib_para_360_servo.write_pwm(pi = self.pi, gpio = servo_r_gpio, min_pw = min_pw_r, max_pw = max_pw_r, min_speed = min_speed_r, max_speed = max_speed_r)
#needed time for initializing the four instances
time.sleep(1)
#angular position in units full circle
def get_angle_l(self):
#driving forward will increase the angle
angle_l = (self.unitsFC - 1) - ((self.l_wheel.read() - self.dcMin_l) * self.unitsFC) / (self.dcMax_l - self.dcMin_l + 1)
angle_l = max(min((self.unitsFC - 1), angle_l), 0)
return angle_l
#angular position in units full circle
def get_angle_r(self):
#driving forward will increase the angle
angle_r = (self.r_wheel.read() - self.dcMin_r) * self.unitsFC / (self.dcMax_r - self.dcMin_r + 1)
angle_r = max(min((self.unitsFC - 1), angle_r), 0)
return angle_r
def set_speed_l(self, speed):
#the value of the speed of the left wheel will always be multiplied
#by minus 1 before setting it.
#this ensures that e.g. positive passed speed values/arguments
#will let the robot drive forward, which means for the left servo
#that it has to rotate counter-clockwise/negative in its local
#orientation system as it is defined in the lib_para_360_servo module.
self.servo_l.set_speed(-speed)
return None
def set_speed_r(self, speed):
self.servo_r.set_speed(speed)
return None
def get_total_angle(self, angle, unitsFC, prev_angle, turns):
#### counting number of rotations
#If 4th to 1st quadrant
if((angle < (0.25*unitsFC)) and (prev_angle > (0.75*unitsFC))):
turns += 1
#If in 1st to 4th quadrant
elif((prev_angle < (0.25*unitsFC)) and (angle > (0.75*unitsFC))):
turns -= 1
####total angle measurement from zero
if(turns >= 0):
total_angle = (turns*unitsFC) + angle
elif(turns < 0):
total_angle = ((turns + 1)*unitsFC) - (unitsFC - angle)
return turns, total_angle
def get_target_angle(self, number_ticks, angle):
#positiv number_ticks will be added, negativ number_ticks substracted
target_angle = angle + number_ticks
return target_angle
def tick_length(self):
tick_length_mm = math.pi * self.diameter_wheels / self.unitsFC
return tick_length_mm
def arc_circle(self, degree):
arc_circle_mm = degree * math.pi * self.width_robot / 360.0
return arc_circle_mm
[docs] def turn(self, degree):
"""
Turns the robot about x degree.
This method turns the robot x degree to the left or to the right.
Positive degree values turn the robot to the left,
negative degree values to the right, see picture in :ref:`Used_local_coordinate_system` ,
where the local coordinate system of the robot is shown. This method calls
:meth:`lib_motion.control.move` which controls the movement of the robot.
:param int,float degree:
Degree the robot has to turn.
"""
number_ticks = self.arc_circle(degree)/self.tick_length()
self.move(number_ticks = number_ticks, turn = True)
return None
[docs] def straight(self, distance_in_mm):
"""
Moves the robot about x mm forward or backward.
This method moves the robot x mm forward or backward. Positive distance
values move the robot forward (regarding the local x-axis), negative distance
values backward (regarding the local x-axis), see picture in :ref:`Used_local_coordinate_system` ,
where the local coordinate system of the robot is shown. This method calls
:meth:`lib_motion.control.move` which controls the movement of the robot.
:param int,float distance_in_mm:
Distance the robot has to move.
"""
number_ticks = distance_in_mm/self.tick_length()
self.move(number_ticks = number_ticks, straight = True)
return None
[docs] def move(
self,
number_ticks = 0,
straight = False,
turn = False):
"""
Core of motion control.
This method controls the movement of the robot. It is called from :meth:`lib_motion.control.turn`
or :meth:`lib_motion.control.straight` and is not ment to be called directly. Four
digital PID controllers are used to make two cascade control loops, one cascade control loop
for each wheel. Each cascade control loop has the same parameters (P/I/D parameters), so that
both wheels are controlled in the same way. Chosen default: Outer control loop is a PI
controller, inner control loop is a P controller. The outer loop is a position controller,
the inner loop a speed controller. After both wheels have reached their set-point (position),
the method waits one second before the movement is marked as finished. This ensures that
overshoots/oscillations are possible and that both wheels can independently reach their
set-point (position). The I part of each PID controller is limited to -1 and 1, so that
the sum of the errors is not integrated till infinity which means to very high or low values
which might cause problems. The output value of each inner PID controller is scaled between -1
and 1 and the output value of each outer PID controller is limited to -1 and 1.
This ensures that no scaling factors are introduced in the P/I/D parameters and also that
the output of each PID controller matches the speed range of the servos, defined in
:meth:`lib_para_360_servo.write_pwm.set_speed` . A sliding median window is used to filter out
the noise in the rotation speed measurement (ticks/s) which is done indirectly by measuring the
position of the servo. Also a deadband filter after the error calculation of the outer control
loop is implemented. This adjustments help to make the controllers more stable, e.g. filter out
outliers while calculating the rotation speed and therefore avoid high value changes/jumps or
avoid oscillations after reaching the set-point (position). The sample time of the digital PID
controllers can also be freely chosen and does not influence the P/I/D parameters, the rotation
speed measurement or the time before the movement is marked as finished.
:param int,float number_ticks:
Number of ticks the wheels have to move.
:param bool straight:
True or False, if robot should move straight.
**Default:** False.
:param bool turn:
True or False, if robot should turn.
**Default:** False.
"""
turns_l = 0
turns_r = 0
angle_l = self.get_angle_l()
angle_r = self.get_angle_r()
target_angle_r = self.get_target_angle(number_ticks = number_ticks, angle = angle_r)
if straight == True:
#speed and number_ticks of servo_l must rotate in
#SAME direction to servo_r while driving straight
target_angle_l = self.get_target_angle(number_ticks = number_ticks, angle = angle_l)
elif turn == True:
#speed and number_ticks of servo_l must rotate in
#OPPOSITE direction to servo_r while turning
target_angle_l = self.get_target_angle(number_ticks = -number_ticks, angle = angle_l)
#initial values sum_error_*
sum_error_r_p = 0
sum_error_r_s = 0
sum_error_l_p = 0
sum_error_l_s = 0
#initial values error_*_old
error_l_p_old = 0
error_l_s_old = 0
error_r_p_old = 0
error_r_s_old = 0
#empty lists for ticks_*
list_ticks_l = []
list_ticks_r = []
position_reached = False
reached_sp_counter = 0
#position must be reached for one second to allow
#overshoots/oscillations before stopping control loop
wait_after_reach_sp = 1/self.sampling_time
#start time of the control loop
start_time = time.time()
#control loop:
while not position_reached:
#DEBUGGING OPTION:
#printing runtime of loop , see end of while true loop
#start_time_each_loop = time.time()
angle_l = self.get_angle_l()
angle_r = self.get_angle_r()
#try needed, because:
#- first iteration of the while loop prev_angle_* is missing and the
#method self.get_total_angle() will throw an exception.
#- second iteration of the while loop prev_total_angle_* is missing,
#which will throw another exception
try:
turns_l, total_angle_l = self.get_total_angle(angle_l, self.unitsFC, prev_angle_l, turns_l)
turns_r, total_angle_r = self.get_total_angle(angle_r, self.unitsFC, prev_angle_r, turns_r)
#### cascade control right wheel
## Position Control
#Er = SP - PV
error_r_p = target_angle_r - total_angle_r
#Deadband-Filter to remove ocillating forwards and backwards after reaching set-point
if error_r_p <= 2 and error_r_p >= -2:
error_r_p = 0
#I-Part
sum_error_r_p += error_r_p
#limit I-Part to -1 and 1
#try needed, because Ki_p can be zero
try:
sum_error_r_p = max(min(1/self.Ki_p, sum_error_r_p), -1/self.Ki_p)
except Exception:
pass
#PID-Controller
output_r_p = self.Kp_p * error_r_p + self.Ki_p * self.sampling_time * sum_error_r_p + self.Kd_p / self.sampling_time * (error_r_p - error_r_p_old)
#limit output of position control to speed range
output_r_p = max(min(1, output_r_p), -1)
error_r_p_old = error_r_p
## Speed Control
#convert range output_r_p from -1 to 1 to ticks/s
#full speed of a wheel forward and backward = +-650 ticks/s
output_r_p_con = 650 * output_r_p
#ticks per second (ticks/s), calculated from a moving median window with 5 values
ticks_r = (total_angle_r - prev_total_angle_r) / self.sampling_time
list_ticks_r.append(ticks_r)
list_ticks_r = list_ticks_r[-5:]
ticks_r = statistics.median(list_ticks_r)
#Er = SP - PV
error_r_s = output_r_p_con - ticks_r
#I-Part
sum_error_r_s += error_r_s
#limit I-Part to -1 and 1
#try needed, because Ki_s can be zero
try:
sum_error_r_s = max(min(650/self.Ki_s, sum_error_r_s), -650/self.Ki_s)
except Exception:
pass
#PID-Controller
output_r_s = self.Kp_s * error_r_s + self.Ki_s * self.sampling_time * sum_error_r_s + self.Kd_s / self.sampling_time * (error_r_s - error_r_s_old)
error_r_s_old = error_r_s
#convert range output_r_s fom ticks/s to -1 to 1
output_r_s_con = output_r_s / 650
self.set_speed_r(output_r_s_con)
#### cascade control left wheel
## Position Control
#Er = SP - PV
error_l_p = target_angle_l - total_angle_l
#Deadband-Filter to remove ocillating forwards and backwards after reaching set-point
if error_l_p <= 2 and error_l_p >= -2:
error_l_p = 0
#I-Part
sum_error_l_p += error_l_p
#limit I-Part to -1 and 1
#try needed, because Ki_p can be zero
try:
sum_error_l_p = max(min(1/self.Ki_p, sum_error_l_p), -1/self.Ki_p)
except Exception:
pass
#PID-Controller
output_l_p = self.Kp_p * error_l_p + self.Ki_p * self.sampling_time * sum_error_l_p + self.Kd_p / self.sampling_time * (error_l_p - error_l_p_old)
#limit output of position control to speed range
output_l_p = max(min(1, output_l_p), -1)
error_l_p_old = error_l_p
## Speed Control
#convert range output_l_p from -1 to 1 to ticks/s
#full speed of a wheel forward and backward = +-650 ticks/s
output_l_p_con = 650 * output_l_p
#ticks per second (ticks/s), calculated from a moving median window with 5 values
ticks_l = (total_angle_l - prev_total_angle_l) / self.sampling_time
list_ticks_l.append(ticks_l)
list_ticks_l = list_ticks_l[-5:]
ticks_l = statistics.median(list_ticks_l)
#Er = SP - PV
error_l_s = output_l_p_con - ticks_l
#I-Part
sum_error_l_s += error_l_s
#limit I-Part to -1 and 1
#try needed, because Ki_s can be zero
try:
sum_error_l_s = max(min(650/self.Ki_s, sum_error_l_s), -650/self.Ki_s)
except Exception:
pass
#PID-Controller
output_l_s = self.Kp_s * error_l_s + self.Ki_s * self.sampling_time * sum_error_l_s + self.Kd_s / self.sampling_time * (error_l_s - error_l_s_old)
error_l_s_old = error_l_s
#convert range output_l_s fom ticks/s to -1 to 1
output_l_s_con = output_l_s / 650
self.set_speed_l(output_l_s_con)
except Exception:
pass
prev_angle_l = angle_l
prev_angle_r = angle_r
#try needed, because first iteration of the while loop prev_angle_* is
#missing and the method self.get_total_angle() will throw an exception,
#and therefore no total_angle_* gets calculated
try:
prev_total_angle_l = total_angle_l
prev_total_angle_r = total_angle_r
except Exception:
pass
try:
if error_l_p == 0 and error_r_p == 0:
reached_sp_counter += 1
if reached_sp_counter >= wait_after_reach_sp:
self.set_speed_r(0.0)
self.set_speed_l(0.0)
position_reached = True
else:
pass
except Exception:
pass
#Pause control loop for chosen sample time
#https://stackoverflow.com/questions/474528/what-is-the-best-way-to-repeatedly-execute-a-function-every-x-seconds-in-python/25251804#25251804
time.sleep(self.sampling_time - ((time.time() - start_time) % self.sampling_time))
#DEBUGGING OPTION:
#printing runtime of loop, see beginning of while true loop
#print('{:.20f}'.format((time.time() - start_time_each_loop)))
return None
[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.l_wheel.cancel()
self.r_wheel.cancel()
if __name__ == '__main__':
#just continue
pass