def __init__(
self, pi, width_robot = 92.74, diameter_wheels = 35, unitsFC = 360,
dcMin_1 = 27.3, dcMax_1 = 950.95,
dcMin_2 = 27.3, dcMax_2 = 960.96,
dcMin_3 = 27.3, dcMax_3 = 987.35,
wheel_1_gpio = 16, wheel_2_gpio = 20, wheel_3_gpio = 21,
servo_1_gpio = 17, min_pw_1 = 1280, max_pw_1 = 1720, min_speed_1 = -1, max_speed_1 = 1,
servo_2_gpio = 27, min_pw_2 = 1280, max_pw_2 = 1720, min_speed_2 = -1, max_speed_2 = 1,
servo_3_gpio = 22, min_pw_3 = 1280, max_pw_3 = 1720, min_speed_3 = -1, max_speed_3 = 1,
sampling_time = 0.01,
Kp_p = 0.1, # values not too big; otherwise output of position control would slow down too abruptly
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_1 = dcMin_1
self.dcMax_1 = dcMax_1
self.dcMin_2 = dcMin_2
self.dcMax_2 = dcMax_2
self.dcMin_3 = dcMin_3
self.dcMax_3 = dcMax_3
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.wheel_1 = lib_para_360_servo.read_pwm(pi = self.pi, gpio = wheel_1_gpio)
self.wheel_2 = lib_para_360_servo.read_pwm(pi = self.pi, gpio = wheel_2_gpio)
self.wheel_3 = lib_para_360_servo.read_pwm(pi = self.pi, gpio = wheel_3_gpio)
self.servo_l = lib_para_360_servo.write_pwm(pi = self.pi, gpio = servo_1_gpio, min_pw = min_pw_1, max_pw = max_pw_1, min_speed = min_speed_1, max_speed = max_speed_1)
self.servo_r = lib_para_360_servo.write_pwm(pi = self.pi, gpio = servo_2_gpio, min_pw = min_pw_2, max_pw = max_pw_2, min_speed = min_speed_2, max_speed = max_speed_2)
self.servo_r = lib_para_360_servo.write_pwm(pi = self.pi, gpio = servo_3_gpio, min_pw = min_pw_3, max_pw = max_pw_3, min_speed = min_speed_3, max_speed = max_speed_3)
# Needed time for initializing the four instances
time.sleep(1)
def __init__(
self,
unitsFC=360,
dcMin=31.85,
dcMax=956.41,
feedback_gpio=5,
servo_gpio=13,
min_pw=1280, # 1210
max_pw=1750, # 1750
min_speed=-1,
max_speed=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.05,
Kd_p=0.0,
Kp_s=0.3,
Ki_s=0,
Kd_s=0,
):
logger.info("Initialising servo motor")
self.stop_event = Event()
pi = pigpio.pi()
self.pi = pi
self.unitsFC = unitsFC
self.dcMin = dcMin
self.dcMax = dcMax
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.feedback = lib_para_360_servo.read_pwm(pi=self.pi,
gpio=feedback_gpio)
self.servo = lib_para_360_servo.write_pwm(
pi=self.pi,
gpio=servo_gpio,
min_pw=min_pw,
max_pw=max_pw,
min_speed=min_speed,
max_speed=max_speed,
)
self.target_setpoint = 0
import time import pigpio import lib_para_360_servo #define GPIO for each servo to write to gpio_l = 17 gpio_r = 27 pi = pigpio.pi() servo_l = lib_para_360_servo.write_pwm(pi = pi, gpio = gpio_l) servo_r = lib_para_360_servo.write_pwm(pi = pi, gpio = gpio_r) #buffer time for initializing everything time.sleep(1) servo_l.set_speed(0) servo_r.set_speed(0) #http://abyz.me.uk/rpi/pigpio/python.html#stop pi.stop()
# Emergency Stop import time import pigpio import lib_para_360_servo # Define GPIO pins for each servo to write to gpio_1 = 17 gpio_2 = 27 gpio_3 = 22 pi = pigpio.pi() servo_1 = lib_para_360_servo.write_pwm(pi=pi, gpio=gpio_1) servo_2 = lib_para_360_servo.write_pwm(pi=pi, gpio=gpio_2) servo_3 = lib_para_360_servo.write_pwm(pi=pi, gpio=gpio_3) #buffer time for initializing everything time.sleep(1) servo_1.set_speed(0) servo_2.set_speed(0) servo_3.set_speed(0) pi.stop()
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)
import time import pigpio import lib_para_360_servo #define GPIO for each servo to read from gpio_r_r = 14 #define GPIO for each servo to write to gpio_r_w = 18 pi = pigpio.pi() #### Calibrate servos, speed = 0.2 and -0.2 #choose gpio_l_w/gpio_l_r (left wheel), or gpio_r_w/gpio_r_r #(right wheel) accordingly servo = lib_para_360_servo.write_pwm(pi = pi, gpio = gpio_r_w) #buffer time for initializing everything time.sleep(1) servo.set_speed(0.2) wheel = lib_para_360_servo.calibrate_pwm(pi = pi, gpio = gpio_r_r) servo.set_speed(0) #http://abyz.me.uk/rpi/pigpio/python.html#stop pi.stop()
# Calibrate
import time
import pigpio
import lib_para_360_servo
# Define GPIO for each servo to read from
gpio_1_r = 16 # servo 1
gpio_2_r = 20 # servo 2
gpio_3_r = 21 # servo 3
# Define GPIO for each servo to write to
gpio_1_w = 17
gpio_2_w = 27
gpio_3_w = 22
pi = pigpio.pi()
#### Calibrate servos, speed = 0.2 and -0.2
servo = lib_para_360_servo.write_pwm(pi=pi, gpio=gpio_1_w) # do 1, 2, and 3
# Buffer time for initializing everything
time.sleep(1)
servo.set_speed(0.2) # do 0.2 and -0.2
wheel = lib_para_360_servo.calibrate_pwm(pi=pi,
gpio=gpio_1_r) # do 1, 2, and 3
servo.set_speed(0)
pi.stop()
def __init__(
self,
pi,
width_robot=92.74,
diameter_wheels=35,
unitsFC=360,
dcMin_l=27.3,
dcMax_l=950.95,
dcMin_r=27.3,
dcMax_r=960.96,
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,
#Don't make values too big (output of position control will slow down too abruptly)
# Outer PID controllers:
Kp_p=0.075, # 0.1 (BEST: 0.075)
Ki_p=0.133, # 0.1 (BEST: 0.133)
Kd_p=0, # 0
# Inner PID controllers:
Kp_s=0.5, # 0.5
Ki_s=0, # 0
Kd_s=0): # 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)