class MotorController():
def __init__(self):
rospy.init_node('motor_controller',log_level=rospy.INFO)
# ------ PARAMETRI ------
# frequenza del loop
self.freq = rospy.get_param('/eod/loop_freq/motor_ctrl')
self.rate = rospy.Rate(self.freq)
# parametri pid dx
self.kp_dx = rospy.get_param('/eod/pid_dx/Kp')
self.ki_dx = rospy.get_param('/eod/pid_dx/Ki')
self.kd_dx = rospy.get_param('/eod/pid_dx/Kd')
self.vMin_dx = rospy.get_param('/eod/pid_dx/v_min')
self.vMax_dx = rospy.get_param('/eod/pid_dx/v_max')
# parametri pid sx
self.kp_sx = rospy.get_param('/eod/pid_sx/Kp')
self.ki_sx = rospy.get_param('/eod/pid_sx/Ki')
self.kd_sx = rospy.get_param('/eod/pid_sx/Kd')
self.vMin_sx = rospy.get_param('/eod/pid_sx/v_min')
self.vMax_sx = rospy.get_param('/eod/pid_sx/v_max')
# ------ VARIABILI ------
self.old_time = rospy.Time.now()
self.pid_dx = PID(self.kp_dx, self.ki_dx, self.kd_dx, self.vMin_dx, self.vMax_dx)
self.pid_sx = PID(self.kp_sx, self.ki_sx, self.kd_sx, self.vMin_sx, self.vMax_sx)
self.v_dx = 0.0
self.v_sx = 0.0
self.ref_dx = 0.0
self.ref_sx = 0.0
# ------ TOPIC ------
rospy.Subscriber('v_target', Wheels_Vel, self.v_target_callback)
rospy.Subscriber('wheels_velocity', Wheels_Vel, self.wheels_velocity_callback)
# il messaggio Wheels_Vel contiene due valori di tensione
self.cmd_pub = rospy.Publisher('motor_cmd', Wheels_Vel, queue_size=10)
def loop(self):
while not rospy.is_shutdown():
# calcolo dt e aggiornamento
dt = self.calculate_dt()
# calcolo della legge di controlo
v_input_dx = self.pid_dx.calculate(self.v_dx, self.ref_dx, dt)
v_input_sx = self.pid_sx.calculate(self.v_sx, self.ref_sx, dt)
# controllo tra segno della tensione e direzione di marcia
v_input_dx = self.saturation(v_input_dx, numpy.sign(self.ref_dx))
v_input_sx = self.saturation(v_input_sx, numpy.sign(self.ref_sx))
# normalizzazione input tra [0,1]
v_input_dx = v_input_dx/self.vMax_dx
v_input_sx = v_input_sx/self.vMax_sx
# pubblicazione dei segnali di controllo
self.publish_motor_cmd(v_input_dx, v_input_sx)
self.rate.sleep()
def calculate_dt(self):
current_time = rospy.Time.now()
dt = (current_time - self.old_time).to_sec()
self.old_time = current_time
return dt
def saturation(self, v_input, sign):
# se l'input da fornire al motore e' opposto alla direzione di marcia
# si preferisce fornire una tensione nulla
if ((sign > 0) and (v_input < 0)) or ((sign < 0) and (v_input > 0)):
return 0.0
else:
return v_input
def publish_motor_cmd(self, v_input_dx, v_input_sx):
# anche se il messaggio e' di tipo Wheels_Vel stiamo
# pubblicando il valore di due tensioni
msg_cmd = Wheels_Vel()
msg_cmd.right = v_input_dx
msg_cmd.left = v_input_sx
self.cmd_pub.publish(msg_cmd)
def v_target_callback(self, msg):
self.ref_dx = msg.right
self.ref_sx = msg.left
def wheels_velocity_callback(self, msg):
self.v_dx = msg.right
self.v_sx = msg.left
def main():
#Variables for PID
kp = 1
kd = 0.5
ki = 0
min = 0
max = 255
#Variables for cv2
center = 320
source = 2
red = (0, 0, 255)
green = (0, 255, 0)
thinkness = 3
center_x = (center, 480)
center_end = (center, 0)
'''
base = Base()
if base.find_arduino() is False:
print("Cannot connect to arduino")
return
'''
cam = cv2.VideoCapture(source)
if cam is None or not cam.isOpened():
print("Warning: unable to open video source:")
return
pid = PID(kp, kd, ki, min, max)
while True:
ret, frame = cam.read()
if not ret:
break
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
index = get_signature(gray)
frame = cv2.line(frame, center_x, center_end, green, thinkness)
if index == None:
print("Black line is not detected")
#base.set_motor(ac.MOTOR_L, ac.FORWARD, ac.MINPOWER)
#base.set_motor(ac.MOTOR_R, ac.FORWARD, ac.MINPOWER)
else:
start_point = (index, frame.shape[0])
end_point = (index, 0)
frame = cv2.line(frame, start_point, end_point, red, thinkness)
err = center - index
if err > 0:
result = pid.calculate(err)
print("Left: ", result)
#base.set_motor(ac.MOTOR_L, ac.FORWARD, ac.MAXPOWER - result)
#base.set_motor(ac.MOTOR_R, ac.FORWARD, ac.MAXPOWER)
else:
result = pid.calculate(-err)
print("Right:", result)
#base.set_motor(ac.MOTOR_L, ac.FORWARD, ac.MAXPOWER)
#base.set_motor(ac.MOTOR_R, ac.FORWARD, ac.MAXPOWER - result)
cv2.imshow("Video", frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
cv2.destroyAllWindows()
class controller(threading.Thread):
# Instantiate the motor controller
def __init__(self, pwm_name, dir_a, dir_b, eqep):
# Initialize the thread base
super(controller, self).__init__()
# Create stop event object
self.stop_event = threading.Event()
self.stop_confirm = threading.Event()
# Store parameters
self.pwm = pwm_name
self.dir_a = dir_a
self.dir_b = dir_b
# Create a PID controller
self.pid = PID(0.01, 0.01, 0.0, -100.0, 100.0, 0.0)
# Load the eQEP interface
self.eqep = eQEP(eqep, eQEP.MODE_RELATIVE)
self.eqep.set_period(10000000)
# Setup the GPIO
gpio.setup(self.dir_a, gpio.OUT)
gpio.setup(self.dir_b, gpio.OUT)
gpio.output(self.dir_a, gpio.LOW)
gpio.output(self.dir_b, gpio.LOW)
# Setup the PWM
pwm.start(self.pwm, 100.0)
# Process of the running thing
def run(self):
while (self.stop_event.is_set() == False):
current = float(self.eqep.poll_position())
output = self.pid.calculate(current)
# Calculate the output duty cycle
duty = 100.0 - abs(output)
# Setup the IO lines correctly
if(output > 0.0):
gpio.output(self.dir_a, gpio.HIGH)
gpio.output(self.dir_b, gpio.LOW)
else:
gpio.output(self.dir_a, gpio.LOW)
gpio.output(self.dir_b, gpio.HIGH)
# Set the pwm
pwm.set_duty_cycle(self.pwm, duty)
# Log
#print "Current = " + str(current) + "; Output = " + str(output)
# Confirm stop
self.stop_confirm.set()
# Stop the motor
def stop(self):
self.stop_event.set()
self.stop_confirm.wait()
self.stop_event.clear()
self.stop_confirm.clear()
gpio.output(self.dir_a, gpio.LOW)
gpio.output(self.dir_b, gpio.LOW)
pwm.set_duty_cycle(self.pwm, 100.0)
encoder = Encoder(21, 20) #port nums for encoder
pid = PID(0.001, 0, 0, 2000) # p , i , d , setpoint
pid.setMinMax(-0.6, 0.6)
IO.setwarnings(False)
IO.setmode(IO.BCM)
IO.setup(19,IO.OUT)
p = IO.PWM(19,50)
p.start(0)
def setPWM(pwm):
return (pwm - 7)/3 #7 is mid value, 3 is range / 2
def setSpeed(speed):
return (speed * 3.0) + 7.0
encoder.start()
while True:
#print(setSpeed(pid.calculate(encoder.position())))
print(pid.error(encoder.position()))
p.ChangeDutyCycle(setSpeed(pid.calculate(encoder.position())))
if (abs(pid.error(encoder.position())) < 30):
encoder.stop()
encoder.join()
break
# print("working")
# for x in range(0, 40):
# p.ChangeDutyCycle(7 - (x/10.0))
# print(7 - (x/10.0))
# time.sleep(0.05)
while self.controlEnabled:
if self.stopped():
self.motor.ChangeDutyCycle(0)
return
self.motor.ChangeDutyCycle(self.pwmSpeed)
def stop(self):
self._stop.set()
def stopped(self):
return self._stop.isSet()
def enableController(self, enable):
self.controlEnabled = enable
def setSpeed(self, speed):
self.speed = speed
self.pwmSpeed = (self.speed * 3.0) + 7.0
p = PWMMotorControl(19)
p.start()
encoder.start()
while True:
print(pid.error(encoder.position()))
p.setSpeed(pid.calculate(encoder.position()))
if (abs(pid.error(encoder.position())) < 10):
p.stop()
encoder.stop()
break
