def fingerControl(self, finger, motorDir):
# TODO - TODO - While needs an output condition?
logging.info("IOUTILS::fingerControl")
""" With these three lines of code, the control of a single motor is achieved.
First the feedback value of the controller is obtained from a specific channel
of the ADC IC. Then, the control signal is computed with a PID. Finally, the
control signal is applied to the motor as a PWM signal generated by the PWM IC.
"""
# Initialize PID
pid = PID(PID_KP, PID_KI, PID_KD)
pid.setWindup = MOTOR_CTRL_MAX
pid.setSampleTime = 0.2
pid.SetPoint = 800
# Initialize buses
# Re-init will be necessary?
# self.adc = MCP3008.MCP3008(clk=CLK, cs=CS, miso=MISO, mosi=MOSI)
# Initialize PWM
# Re-init will be necessary?
#self.pwm = PCA9685.PCA9685(PCA_I2C_ADDR)
#self.pwm.set_pwm_freq(PWM_FRQUENCY)
logging.debug("IOUTILS::fingerControl - Reading from ADC Channel [%i]",
FINGER_CHANNELS_MATRIX[ADC][finger])
while True:
feedback = self.getPotentiometerValue(finger)
pid.update(feedback)
duty_cycle = int(pid.output)
self.motor_control(duty_cycle, finger, motorDir)
def test_pid(P=0.2, I=0.0, D=0.0, L=100):
"""Self-test PID class
.. note::
...
for i in range(1, END):
pid.update(feedback)
output = pid.output
if pid.SetPoint > 0:
feedback += (output - (1/i))
if i>9:
pid.SetPoint = 1
time.sleep(0.02)
---
"""
pid = PID.PID(P, I, D)
pid.SetPoint = 0.0
pid.setSampleTime(0.01)
END = L
feedback = 0
feedback_list = []
time_list = []
setpoint_list = []
for i in range(1, END):
pid.update(feedback)
output = pid.output
if pid.SetPoint > 0:
feedback += (output - (1 / i))
if i > 9:
pid.SetPoint = 1
time.sleep(0.02)
feedback_list.append(feedback)
setpoint_list.append(pid.SetPoint)
time_list.append(i)
time_sm = np.array(time_list)
time_smooth = np.linspace(time_sm.min(), time_sm.max(), 300)
feedback_smooth = spline(time_list, feedback_list, time_smooth)
plt.plot(time_smooth, feedback_smooth)
plt.plot(time_list, setpoint_list)
plt.xlim((0, L))
plt.ylim((min(feedback_list) - 0.5, max(feedback_list) + 0.5))
plt.xlabel('time (s)')
plt.ylabel('PID (PV)')
plt.title('TEST PID')
plt.ylim((1 - 0.5, 1 + 0.5))
plt.grid(True)
plt.show()
def test_pid(P = 0.2, I = 0.0, D= 0.0, L=100):
"""Self-test PID class
.. note::
...
for i in range(1, END):
pid.update(feedback)
output = pid.output
if pid.SetPoint > 0:
feedback += (output - (1/i))
if i>9:
pid.SetPoint = 1
time.sleep(0.02)
---
"""
pid = PID.PID(P, I, D)
pid.SetPoint=0.0
pid.setSampleTime(0.01)
END = L
feedback = 0
feedback_list = []
time_list = []
setpoint_list = []
for i in range(1, END):
pid.update(feedback)
output = pid.output
if pid.SetPoint > 0:
feedback += (output - (1/i))
if i>9:
pid.SetPoint = 1
time.sleep(0.02)
feedback_list.append(feedback)
setpoint_list.append(pid.SetPoint)
time_list.append(i)
time_sm = np.array(time_list)
time_smooth = np.linspace(time_sm.min(), time_sm.max(), 300)
feedback_smooth = spline(time_list, feedback_list, time_smooth)
plt.plot(time_smooth, feedback_smooth)
plt.plot(time_list, setpoint_list)
plt.xlim((0, L))
plt.ylim((min(feedback_list)-0.5, max(feedback_list)+0.5))
plt.xlabel('time (s)')
plt.ylabel('PID (PV)')
plt.title('TEST PID')
plt.ylim((1-0.5, 1+0.5))
plt.grid(True)
plt.show()
def run_controllers(main_d):
while main_d["running"]:
for i in range(0, 4):
# main_d["pid"][i]["current"] = loadcell.get_value(i)
pid.update(main_d["pid"][i])
# arduino.set_pwm(main_d["arduino"], i, main_d["pid"][i]["output"])
# gfx.log(main_d["pid"][0]["current"])
time.sleep(pid.DELTA_TIME)
def Pid_Adjast(move_speed, isforward):
angle = motion.getGyro()
pid.update(angle)
out = int(abs(pid.output * 10))
print "angle:" + str(angle)
print "out:" + str(out)
if angle >= 5:
motion.motion_wheelspeed(-move_speed, -move_speed - out)
elif angle <= -5:
motion.motion_wheelspeed(-move_speed - out, -move_speed)
else:
pass
import os, time
import pid
import sys
import Motion
# os.system('espeak -s 120 -v zh \'woyaoshiyong\' --stdout|aplay')
# os.system('espeak -s 120 -v en-us \'p i d\' --stdout|aplay')
# os.system('espeak -s 120 -v zh \'suanfa\' --stdout|aplay')
# pid= pid.PID(P=sys.argv[1],I=sys.argv[2],D=sys.argv[3])
pid = pid.PID(P=0.2, I=0.2, D=0.3)
motion = Motion.motion()
while True:
# time.sleep(1)
angle = motion.getGyro()
pid.update(angle)
out = int(pid.output * 10)
print "\n"
print angle
print out
print motion.getDist()
if out >= 1000:
out = 1000
elif out <= -1000:
out = -1000
if angle >= 0:
motion.motion_left(-out)
else:
motion.motion_right(out)
def main(self, raw_cap, gr_cap):
# Update loop count
# print(self.num_CW_detected)
# # print(self.crossing_count)
# print(self.no_ped_count)
print("----------------")
print("crosswalk stuff:")
print("crosswalk: " + str(self.detected_crosswalk))
print("pedestrian: " + str(self.detected_pedestrian))
print("no ped count:" + str(self.no_ped_count))
print("seen redline this many times: " + str(self.entering_cw))
print("has it passed crosswalk yet: " + str(self.passedCW))
print("has it seen corner yet: " + str(self.detected_corner))
print("has it found plate yet: " + str(self.foundPlate))
print("has it seen last corner yet:" + str(self.lastCorner))
print("this is 3rd corner count:" + str(self.thirdCorCount))
print("----------------")
print("plate stuff:")
print("this is count_detect_mode:" + str(self.count_detect_mode))
# print("this is count_loop_save:" + str(self.count_loop_save))
# print("has not successfully got a plate for:" + str(self.noPlateCount))
print("has it stopped to read plate yet?" + str(self.stopForPlate))
print("has image been saved yet?" + str(self.savedImage))
print("this many images have been saved:" +
str(my_plate_locator.numSavedImages))
# print("is it getting back out:" + str(self.getBackOut))
if my_plate_locator.numSavedImages == 5: # can be changed to 5
self.allDone = True
self.doneEarly = True
self.time_elapsed = rospy.get_time() - self.time_start
if my_plate_locator.numSavedImages == 4 and not self.secondCW:
self.passedCW = False
self.secondCW = True
self.foundPlate = False
self.stopRushing = False
if self.secondCW:
if not self.foundPlate and detection.path.corner(gr_cap):
self.countCWCorner += 1
if self.countCWCorner > 5:
print("found corner!!! now sweeping!!!")
self.move.linear.x = 0
self.move.angular.z = -1.8 * constants.CONST_ANG
self.detected_corner = True
if my_plate_locator.numSavedImages == 2:
if not self.firstCor:
if not self.foundPlate and detection.path.corner(gr_cap):
print("found corner!!! now sweeping!!!")
self.move.linear.x = 0
self.move.angular.z = -0.5 * constants.CONST_ANG
self.detected_corner = True
if self.firstCor and not self.passedCW:
self.foundPlate = False
if my_plate_locator.numSavedImages == 4 and self.passedCW:
if not self.thirdCor:
self.thirdCorCount += 1
self.move.linear.x *= RUSHING_FACTOR
if self.thirdCorCount > LAST_CORNER_COUNT_LIM and self.thirdCorCount < LAST_CORNER_COUNT_LIM + 15 and not self.lastCorner:
print("found corner!!! now sweeping!!!")
self.move.linear.x = 0
self.move.angular.z = -2.5 * constants.CONST_ANG
if self.thirdCorCount >= LAST_CORNER_COUNT_LIM + 15:
self.lastCorner = True
# !!!!!!! ONLY do these when not in the process of stopping and reading plate.
# Get crosswalk
# Only start crosswalk detection after at least 2 plates have been saved
if not self.stopForPlate:
if my_plate_locator.numSavedImages > 1 and not self.passedCW:
if not self.secondCW:
self.crosswalkFunc(raw_cap)
else:
if self.foundPlate:
self.crosswalkFunc(raw_cap)
# else:
# self.detected_crosswalk = [False, False]
# Get path state
if self.entering_cw == 0 and not self.passedCW and not self.secondCW:
state = detection.path.state(gr_cap, self.detected_crosswalk)
else:
print("manually change to False False")
state = detection.path.state(gr_cap, [False, False])
print(state)
if self.passedCW:
print(self.passedCW_count)
self.passedCW_count += 1
if self.passedCW_count > 50 and detection.path.corner(
gr_cap) and not self.foundPlate:
self.detected_corner = True
self.stopRushing = True
if self.detected_corner:
print("found corner! now sweeping!!!")
self.move.linear.x = 0
self.move.angular.z = -2.5 * constants.CONST_ANG
if self.detected_corner and state == [0, 1]:
if my_plate_locator.numSavedImages == 2 and self.passedCW:
print("found plate! stop sweeping")
self.foundPlate = True
self.detected_corner = False
self.stopForPlate = True
self.count_detect_mode = COUNT_DETECT_MODE_LIM + 1
self.move.linear.x = 0
self.move.angular.z = 0
if self.detected_corner and state == [0, 1]:
if my_plate_locator.numSavedImages == 4:
print("found plate! stop sweeping")
self.foundPlate = True
self.detected_corner = False
self.move.linear.x = 0
self.move.angular.z = 0
if self.passedCW:
self.lastCorner = True
if not self.firstCor and self.detected_corner and state == [
2, -1
] and my_plate_locator.numSavedImages == 2:
print("found plate! stop sweeping")
self.foundPlate = True
self.detected_corner = False
self.firstCor = True
self.move.linear.x = 0
self.move.angular.z = 0
if self.firstCor and my_plate_locator.numSavedImages == 4 and not self.thirdCor and self.detected_corner and state == [
2, -1
]:
print("found plate! stop sweeping")
self.foundPlate = True
self.detected_corner = False
self.thirdCor = True
self.thirdCorCount = 0
self.move.linear.x = 0
self.move.angular.z = 0
# getting back out of the bad orientation
if self.getBackOut_count == 0 and self.foundPlate and my_plate_locator.numSavedImages > 2:
self.move.linear.x = 0
self.getBackOut = True
self.move.angular.z = 0 * constants.CONST_ANG
print("getting back out!")
# if state == [0, 1]:
# self.canSweepNow = False
# print("Stop sweeping now")
# Get/set velocities only when crosswalk is not present
if not self.getBackOut and not self.stopForPlate and self.entering_cw < CROSSING_COUNT_LIM + 2 and not self.detected_pedestrian and not self.detected_corner:
if self.thirdCorCount <= LAST_CORNER_COUNT_LIM or self.thirdCorCount >= LAST_CORNER_COUNT_LIM + 15:
print("updating pid")
pid.update(self.move, state)
if self.getBackOut and self.getBackOut_count > 6:
print("finishing getting back out now")
self.getBackOut = False
self.move.angular.z = 0
if self.getBackOut:
self.getBackOut_count += 1
# Rush out of crosswalk
if my_plate_locator.numSavedImages != 4 and self.passedCW and not self.stopRushing:
self.move.linear.x *= RUSHING_FACTOR
# self.move.angular /= RUSHING_FACTOR
# Publish the state anytime
self.pub.publish(self.move)
#------------------------------------------------------------------------------------------------------------------------------------
if my_plate_locator.numSavedImages == 2 and self.foundPlate:
print("do not move until 3rd plate is found")
self.count_detect_mode = COUNT_DETECT_MODE_LIM + 1
# self.move.angular.z = -9 * pid.CONST_ANG
# self.pub.publish(self.move)
# only check for plate if wanted:
if (my_plate_locator.numSavedImages == 2 and not self.passedCW):
used_count_detect_mode_lim = CORRECTED_COUNT_DETECT_MODE_LIM
used_loop_count_lim = CORRECTED_LOOP_COUNT_LIM
elif (my_plate_locator.numSavedImages == 2
and self.foundPlate) or not self.passedCW:
used_count_detect_mode_lim = COUNT_DETECT_MODE_LIM
used_loop_count_lim = LOOP_COUNT_LIM
elif my_plate_locator.numSavedImages == 3:
used_count_detect_mode_lim = LESS_COUNT_DETECT_MODE_LIM
used_loop_count_lim = LESS_LOOP_COUNT_LIM
else:
used_count_detect_mode_lim = LAST_COUNT_DETECT_MODE_LIM
used_loop_count_lim = LAST_LOOP_COUNT_LIM
if (not self.passedCW
and not self.detected_pedestrian) or (self.passedCW
and self.foundPlate):
if self.thirdCorCount > LAST_PLATE_COUNT_LIM or (
not self.savedImage
and self.count_detect_mode > used_count_detect_mode_lim):
print("checking for plates")
if self.noPlateCount < NO_PLATE_MOVE_ON_LIM or self.foundPlate:
self.stopForPlate = True
# make it stop IMMEDIATELY, it will be too late if wait until method finishes
self.move.linear.x = 0
self.move.angular.z = 0
self.pub.publish(self.move)
self.count_detect_mode = 0
try:
print("entered try block!")
if my_plate_locator.locate_plate(
gr_cap, self.count_loop_save):
self.savedImage = True
self.stopForPlate = False # Resume
self.count_loop_save = 0
self.count_loop = 0
else:
self.count_loop_save += 1
self.count_detect_mode = used_count_detect_mode_lim + 1
if self.count_loop_save > 1:
self.noPlateCount += 1
self.loopcount += 1
except (ValueError, UnboundLocalError, IndexError,
AttributeError):
print("error caught")
self.noPlateCount += 1
self.loopcount += 1
else:
print("could not find plate, move on")
self.stopForPlate = False
self.noPlateCount = 0
self.loopcount = 0
self.count_loop_save = 0
self.count_detect_mode = 0
else:
print("not checking for plates")
self.stopForPlate = False
self.loopcount += 1
self.count_detect_mode += 1
if self.loopcount > used_loop_count_lim:
self.savedImage = False
self.loopcount = 0
cv2.imshow("robot cap", gr_cap)
cv2.waitKey(1)