def __init__(self):
rospy.init_node("joint_controller_node")
rospy.on_shutdown(self.shutdown)
rospy.loginfo("Setting up joint_controller_node")
self.pitch_pub = rospy.Publisher("roboclaw/pitch_vel",Twist,queue_size=10)
self.height_pub = rospy.Publisher("roboclaw/height_vel",Twist,queue_size=10)
self.joint_pub = rospy.Publisher('arm_joint_state',JointState,queue_size=10)
self.pitch_sub = rospy.Subscriber('roboclaw/pitch',JointState,self.pitch_pos_callback)
self.height_sub = rospy.Subscriber('roboclaw/height',JointState,self.height_pos_callback)
self.joint_status_pub = rospy.Subscriber('joint_states',JointState,self.joint_states_callback)
self.rate = rospy.Rate(5)
self.height_min_length = 15.71
self.height_max_length = 25.55
self.height_stroke_length = self.height_max_length - self.height_min_length
self.height_angle_offset = 369.69
self.pitch_min_length = 11.69
self.pitch_max_length = 17.6
self.pitch_stroke_length = self.pitch_max_length - self.pitch_min_length
self.start = False
self.h1_pid = PID(5,0,0,output_limits=(-127,127),auto_mode=False,sample_time=1/5)
self.h2_pid = PID(5,0,0,output_limits=(-127,127),auto_mode=False,sample_time=1/5)
self.p1_pid = PID(5,0,0,output_limits=(-127,127),auto_mode=False,sample_time=1/5)
self.p2_pid = PID(5,0,0,output_limits=(-127,127),auto_mode=False,sample_time=1/5)
self.MAX_ENC = 2047.0
self.quantize_div = 12.0
self.deadband = 11.0
self.h_params = {"m1":50.0,"m2":50.0}
self.p_params = {"m1":50.0,"m2":50.0}
self.p1_vals = [0.0,0.0,0.0,0.0,0.0]
self.p2_vals = [0.0,0.0,0.0,0.0,0.0]
self.h1_vals = [0.0,0.0,0.0,0.0,0.0]
self.h2_vals = [0.0,0.0,0.0,0.0,0.0]
self.height_pos = {"m1":0.0,"m2":0.0}
self.pitch_pos = {"m1":0.0,"m2":0.0}
def setupPid(self):
SAMPLE_TIME = TIMER_TICK / 1000.
self.pid = PID(Kp=self.Kp,
Ki=self.Ki,
Kd=self.Kd,
setpoint=self.angle_setpoint,
sample_time=SAMPLE_TIME,
output_limits=(-MAX_SPEED, MAX_SPEED))
self.pi = pigpio.pi()
def move_mouse(self):
mouse = Controller()
screenCenter = [2560 / 2, 1080 / 2]
# mouse.position = tuple(screenCenter)
scaleFactor = 1.2
pid = PID(.2, .2, 0.05, setpoint=1)
eyeStateLIST = []
scaledChange = [0, 0]
webcam = cv2.VideoCapture(0)
while True:
# set pid controls
controlChangeX = pid((mouse.position[0] - screenCenter[0]) -
scaledChange[0])
controlChangeY = pid((screenCenter[1] - mouse.position[1]) -
scaledChange[1])
_, frame = webcam.read()
FaceTracker()
face_center = FaceTracker().trackface(frame)
print(face_center)
if face_center is not None:
############# YAAAAAAASSSSSS THE PID WORKS ON MULTIPLE FILES ###############33
coarse_newCoord = face_center
changeX = self.imageCenter[0] - coarse_newCoord[0]
changeY = coarse_newCoord[1] - self.imageCenter[1]
# if changex > changeBuffer or changey > changeBuffer:
change = [changeX, changeY]
# else:
scaledChange = np.average([[controlChangeX, controlChangeY],
[change[0] * 35, change[1] * 20]],
axis=0)
newPos = np.add(screenCenter, np.multiply(scaledChange, 1))
# print(newPos)
if newPos[0] > 10 and newPos[0] < 2550 and newPos[
1] > 10 and newPos[1] < 1070:
mouse.position = newPos
else:
break
# show full view
cv2.imshow('full', frame)
#close up
if cv2.waitKey(1) & 0xFF == ord('q'):
break
webcam.release()
cv2.destroyAllWindows()
def frentee():
client = mqtt.Client()
Conectar(client)
sleep(0.5)
descerr()
KP = 12
TP = 210.0
KIO = 0
KD = 0
SP = 3
V_MAX_MOTOR = 1000
VALOR_MAX_CONTROL = V_MAX_MOTOR - TP
for i in range(0, 30):
frente()
pid = PID(KP, KIO, KD, setpoint=SP)
control = pid(carga[1])
if (control > VALOR_MAX_CONTROL):
control = VALOR_MAX_CONTROL
elif (control < -VALOR_MAX_CONTROL):
control = -VALOR_MAX_CONTROL
motor_esq.run_forever(speed_sp=TP - control)
motor_dir.run_forever(speed_sp=TP + control)
if (carga[1] >= 18):
Sound.beep()
stop()
frente()
frente()
frente()
sleep(0.5)
descerr()
sleep(0.5)
vertiE()
sleep(0.5)
mre()
sleep(0.5)
frente()
sleep(0.5)
mre()
sleep(0.5)
frente()
sleep(0.5)
mre()
sleep(0.5)
frente()
stop()
stop()
Desconectar(client)
def __init__(self, robot):
"""
Default C'tor
@param[in] robot The handle to the robot
"""
from simple_pid import PID
super(StateMachineFollow, self).__init__('Follow', robot)
# The initial pointing position
self._init_pt = None
# Our PID controller
self._pitch_pid = PID(0.5, 0.5, 0, setpoint=0)
self._yaw_pid = PID(0.5, 0.5, 0, setpoint=0)
# Line up our states
self._state_funcs = [self._state_init, self._state_follow]
def __init__(self, ds, sett):
print("[FANC] Initialized")
# we are cooling, so more fan if we are over temp
self.pid = PID(Kp=-1.0, output_limits=(0, 100))
self.ds = ds
self.sett = sett
self.lastValue = None
self.fan = Fan()
def handleZumoDeltaPos(deltaPosData):
global motorFwdPID, motorHeadingPID, currentHeading, requiredDist
currentHeading += deltaPosData.angular.z
requiredDist += deltaPosData.linear.x
motorHeadingPID = PID(4,
0.2,
0,
setpoint=currentHeading,
sample_time=None,
output_limits=(-motorMax, motorMax))
motorFwdPID = PID(800,
0.5,
0,
setpoint=requiredDist,
sample_time=None,
output_limits=(-motorMax, motorMax))
def setPid(self, const, sp):
'''
sets pid thread object
:param const: list of pid constants [kp,ki,kd]
:param sp: pid set point
:return: nothing
'''
self.pid = PID(const['kp'], const['ki'], const['kd'], sp, 0.01,
[0, 1023])
def __init__(self, bot: DriveBase, gyro_sensor: GyroSensor):
self.bot = bot
self.gyro_sensor = gyro_sensor
self.pid = PID(TurnToAngleBehavior.PROPORTIONAL,
TurnToAngleBehavior.INTEGRAL,
TurnToAngleBehavior.DERIVATIVE, 0,
TurnToAngleBehavior.UPDATE_INTERVAL)
self.__configure_pid()
super().__init__()
def __init__(self, trajectory, currPos, theta, canvas, vMax = 2):
self.trajectory = np.array(trajectory)
self.currPos = currPos
self.theta = theta
self.extPos = currPos + Pos(-15*np.cos(theta*np.pi/180), 15*np.sin(theta*np.pi/180))
self.vMax = vMax
self.pid = PID(1, 0.1, 0.5, setpoint=0)
self.pid.sample_time = 0.01
self.canvas = canvas
def __init__(self, shape, init_theta=1e-5, set_point = 5):
super(ErrorLayerBipolar, self).__init__()
self.theta = torch.nn.Parameter(torch.ones(shape)*init_theta, requires_grad = False)
self.err_count = torch.zeros(shape)
self.t_count = torch.tensor(0)
self.relu = nn.ReLU()
self.set_point = set_point
self.pid = PID(.5e-6, .0, .0, setpoint = set_point)
self.last_err_rate = 0
def __init__(self, delay_weight, static_term):
self.delay_weight = delay_weight
self.static_term = static_term
self.steer_controller = PID(
Kp=2.88,
Ki=0.0,
Kd=0.0818,
output_limits=(-1, 1),
)
self.base_speed = 1
self.speed_controller = PID(
Kp=1.0,
Ki=0.0,
Kd=0.125,
output_limits=(-1, 1),
)
self.detector = LaneDetector()
def add_goal(self, goal):
"""
Goal is of type Goal()
"""
self.goal.insert(0, goal)
self.pid_vec_x = PID(1,
0,
0,
setpoint=0,
sample_time=None,
output_limits=(-0.2, 0.2))
self.pid_rot_z = PID(7,
0.001,
1,
setpoint=0,
sample_time=None,
output_limits=(-1.5, 1.5))
print(f'New goal set {self.get_current_goal()}')
def test_separate_components():
pid = PID(1, 0, 1, setpoint=10, sample_time=0.1)
assert pid(0) == 10
assert pid.components == (10, 0, 0)
time.sleep(0.1)
assert round(pid(5)) == -45
assert tuple(round(term) for term in pid.components) == (5, 0, -50)
def __init__(self):
self.cont_temp = 0 #Variable globale pour les consignes de température
self.cont_hum = 0 # Variable pour les consignes d'humidité
self.pid_HeatMat = PID(2,1,5, setpoint=self.cont_temp)
self.pid_HeatMat.output_limits = (0, 255)
GPIO.setmode(GPIO.BOARD)
GPIO.setup(40, GPIO.OUT)
self.soft_pwm = GPIO.PWM(40,500)
self.soft_pwm.start(50)
def LandAruco(self, req):
''' Land on the aruco marker '''
rospy.loginfo('Landing on the aruco marker')
timeOut = req.timeOut
new_sp = TwistStamped()
while self.UAV_state.mode != "OFFBOARD" :
rospy.sleep(0.1)
self.set_mode(0, 'OFFBOARD')
# Publish something to activate the offboard mode
self.cmd_vel_pub.publish(new_sp)
if not mavros.command.arming(True) :
mavros.command.arming(True)
ts = rospy.Time.now()
xPID = PID(.4, 0.075, 0.02, output_limits=(-.5, 0.5), setpoint=0.0)
yPID = PID(.4, 0.075, 0.02, output_limits=(-.5, 0.5), setpoint=0.0)
zPID = PID(.5, 0.0, 0.05, output_limits=(-0.75, 0.75), setpoint=0.0)
yawPID = PID(.2, 0.0, 0.0, output_limits=(-1.0, 1.0), setpoint=0.0)
prev_height = self.markerPos[2]
ts2 = rospy.Time.now()
while (rospy.Time.now() - ts < rospy.Duration(timeOut)):
new_sp = TwistStamped()
new_sp.twist.linear.x = xPID(-self.markerPos[0])
new_sp.twist.linear.y = yPID(-self.markerPos[1])
new_sp.twist.linear.z = zPID(-self.markerPos[2])
new_sp.twist.angular.z = yawPID(self.markerPos[3])
if self.markerPos[2] != prev_height:
ts2 = rospy.Time.now()
prev_height = self.markerPos[2]
if rospy.Time.now() - ts2 > rospy.Duration(1.0):
break
#print(np.linalg.norm(self.markerPos[0:3] - np.array([0.0, 0.0, -self.markerHeight])))
self.cmd_vel_pub.publish(new_sp)
return land_arucoResponse(np.linalg.norm(self.markerPos[0:3] - np.array([0.0, 0.0, 0.0])))
def __init__(self):
self.previousPowerLevel = 0
self.powerLevel = 0
self.rollAngleDeflection = 5
self.pitchAngleDeflection = 5
self.mpu = KalmanMPU()
self.mpu.start()
self.pidSampletime = 0.01
self.rollPID = PID(0.01 * 32, 0.01 * 67, 0.01 * 7, 0)
self.rollPID.sample_time = self.pidSampletime
self.rollPID.output_limits = (-20, 20)
self.pitchPID = PID(0.01 * 93, 0.01 * 21, 0.01 * 16, 0)
self.pitchPID.sample_time = self.pidSampletime
self.pitchPID.output_limits = (-20, 20)
self.rollPID.auto_mode = False
self.pitchPID.auto_mode = False
self.ended = False
self.s = socket.socket()
self.s.bind(("", 5005))
self.s.listen()
self.i2c = busio.I2C(board.SCL, board.SDA)
self.pca = adafruit_pca9685.PCA9685(self.i2c)
self.pca.frequency = 2000
self.kit = ServoKit(channels=8)
self.m1 = self.kit.servo[0]
self.m2 = self.kit.servo[1]
self.m3 = self.kit.servo[2]
self.m4 = self.kit.servo[3]
self.m1Val = 0
self.m2Val = 0
self.m3Val = 0
self.m4Val = 0
self.arm()
threading.Thread(target=self.pidCorrectionThread).start()
self.activeClient = False
self.awaitClients()
def __init__(self):
self.sys = System()
self.VERBOSITY = 2
# Set the visual paramiters.
self.scale = 600 # scaling factor between simulated system and pixels
boarder = 0.05 # Width of the canvas edge border as a portion of the
# canvas dimensions
self.width = self.sys.el * (1 + boarder) # width of canvas.
self.height = self.sys.el * (1 + boarder) # height of canvas.
self.initial_state = [
-0.1, self.height * 0.8, -np.pi / 4, np.pi, 0, 0, 0, 0
]
self.state = self.initial_state
self.dt = 0.005 # how long to jump the simulation each time step.
# Transform to move from simulation coordinates to gui coordinates.
self.g_cw = self.sys.build_G(
self.sys.build_R(-np.pi),
[self.width / 2, self.height - self.height * boarder * 0.5, 0])
# Vars relating to the closed loop control
self.forces = [0, 0]
self.control_enable = IntVar()
self.inner = PID(0, 0, 0, 0) # These are not safe defaults, change
self.outer = PID(0, 0, 0, 0) # during system bringup.
self.outer2 = PID(0, 0, 0, 0) # during system bringup.
self.outer.output_limits = (-np.pi / 8, np.pi / 8)
self.wind_scale = -0.1
self.plate_goal = 0
self.velocity_goal = 0
self.control_defaults = [1500, 120, -.25, -.04, 0.5]
root = self.create_gui()
stop = threading.Event()
draw = threading.Thread(target=self.animate_canvas, args=(stop, ))
simulate = threading.Thread(target=self.simulate_system, args=(stop, ))
self.run = False # Should the simulation continue
try:
draw.start()
simulate.start()
root.mainloop()
finally:
stop.set()
def controlTemp(mean_Temp):
if (mean_Temp > 0):
datetime_Now = datetime.strptime(
datetime.now().strftime("%m/%d/%Y, %H:%M:%S"), "%m/%d/%Y, %H:%M:%S"
) #Fait comme ca pour avoir la meme forme que les autres datetimes. Sinon des erreurs de precisions arrivent.
datetime_On_Heat = datetime.strptime(obj_Control.heat_On_Time,
"%m/%d/%Y, %H:%M:%S")
datetime_Off_Heat = datetime.strptime(obj_Control.heat_Off_Time,
"%m/%d/%Y, %H:%M:%S")
if (datetime_Now > datetime_Off_Heat
and datetime_Now > datetime_On_Heat):
obj_Control.heat_On_Time = datetime_Now.strftime(
"%m/%d/%Y, %H:%M:%S")
obj_Control.heat_Off_Time = (
datetime_Now + timedelta(seconds=obj_Control.PERIOD_HEAT)
).strftime("%m/%d/%Y, %H:%M:%S")
elif (datetime_Now >= datetime_Off_Heat
): #Lorsque tempsoff est depassé, allume et set le temps max On
try:
pid_Heating = PID(10, 5, 3, setpoint=obj_Control.setpoint_Temp)
pid_Heating.output_limits = (
0, obj_Control.PERIOD_HEAT
) #Fait un output de 0 a 5 qui sera changé en secondes
temp_Control = pid_Heating(mean_Temp)
except:
print("Erreur Heating Control")
if (obj_Control.heating_On == False):
obj_Control.heating_On = True
obj_Control.heat_On_Time = (
datetime_Off_Heat +
timedelta(seconds=(int(temp_Control)),
milliseconds=(temp_Control * 1000) %
1000)).strftime("%m/%d/%Y, %H:%M:%S")
obj_Control.heat_Off_Time = (
datetime_Off_Heat +
timedelta(seconds=obj_Control.PERIOD_HEAT)
).strftime("%m/%d/%Y, %H:%M:%S")
obj_Control.toggle(obj_Control.PIN_CHAUFFAGE)
#print("Heating On - Time : {} -> Heat Shutoff : {} - New Cycle : {}".format(datetime_Now.strftime("%m/%d/%Y, %H:%M:%S"),obj_Control.heat_On_Time, obj_Control.heat_Off_Time))
elif (datetime_Now >= datetime_On_Heat):
if (obj_Control.heating_On == True):
#print("Shutting off Heat. - Time : {}".format(datetime_Now.strftime("%m/%d/%Y, %H:%M:%S")))
obj_Control.heating_On = False
obj_Control.toggle(obj_Control.PIN_CHAUFFAGE)
def main():
mainProcess = VrepProcess()
mainProcess.simConnect()
mainProcess.simSetup(0.01, 200)
baseName = 'Body'
jointName = 'hip_joint'
mainProcess.simGetHandle(baseName, jointName)
mainProcess.simStart()
"""
控制环
"""
hip_joint_handle = mainProcess.m_jointHandle[0]
theta, theta_dot = mainProcess.simGetJointInfo(hip_joint_handle)
pid = PID(50,0,5,theta) #期望设置到当前
time_hidtory = []
theta_history = []
theta_dot_history = []
for idx in range(mainProcess.m_N):
time = mainProcess.simGetTime()
#currentTime, basePos, jointConfig = mainProcess.simGetInfo()
hip_joint_handle = mainProcess.m_jointHandle[0]
theta, theta_dot = mainProcess.simGetJointInfo(hip_joint_handle)
time_hidtory.append(0.01*time)
theta_history.append(theta)
theta_dot_history.append(theta_dot)
u = pid(theta)
mainProcess.simSetJointCmd(hip_joint_handle, u)
#print(currentTime)
#print(basePos)
#print(theta)
#print(theta_dot)
if idx%10 == 0:
print('running')
print(u)
mainProcess.simNextStep()
mainProcess.simStop()
print('simulaiton stopped')
plt.figure()
plt.plot(time_hidtory,theta_history)
plt.plot(time_hidtory,theta_dot_history)
plt.show()
def __init__(self,
scope,
piezo,
p=1.,
i=0.1,
d=0.05,
sample_time=0.01,
mode='frame'):
"""
Parameters
----------
scope: PYME.Acquire.Hardware.microscope.microscope
piezo: PYME.Acquire.Hardware.Piezos.offsetPiezoREST.OffsetPiezoClient
p: float
i: float
d: float
sample_time: float
See simple_pid.PID, but this servo does not have a tolerance on the lock position, but rather a dead-time
of-sorts by only updating at ~regular time intervals. The correction is only changed once per sample_time.
mode: str
flag, where 'frame' queries on each frame and 'time' queries at fixed times by polling at sample_time
"""
self.scope = scope
self.piezo = piezo
# self._last_offset = self.piezo.GetOffset()
self._fitter = GaussFitter1D()
self.fit_roi_size = 75
self.peak_position = 512 # default to half of the camera size
self.subtraction_profile = None
PID.__init__(self,
p,
i,
d,
setpoint=self.peak_position,
auto_mode=False,
sample_time=sample_time)
self._mode = mode
self._polling = False
def setup_feed_back(self, setpoint=1300, output_limits=[]):
self.fb_pid = PID(
1,
0,
0,
setpoint=setpoint,
output_limits=output_limits,
sample_time=1,
proportional_on_measurement=True,
)
def learn(self, batch_state, batch_next_state, batch_reward, batch_action):
outputs = self.model(batch_state).gather(
1, batch_action.unsqueeze(1)).squeeze(1)
next_outputs = self.model(batch_next_state).detach().max(1)[0]
target = self.gamma * next_outputs + batch_reward
td_loss = F.smooth_l1_loss(outputs, target)
target = PID(target, 0.1, 0.05, setpoint=1)
self.optimizer.zero_grad()
td_loss.backward(retain_variables=True)
self.optimizer.step()
def __init__(self):
self.running =False
self.freq=1
self.gpio_num=HLT_heater_cspin
self.power=0
self.pid=PID(Kp=112.344665712, Ki=0.840663751375, Kd=12.5112685197)
self.pid.output_limits = (0, 100)
self.pid.sample_time=5
self.running=False
self.pid.proportional_on_measurement = False
def pitch_hold(self, pitch_comm: float) -> None:
"""
Maintains a commanded pitch attitude [radians] using a PI controller with a rate component
:param pitch_comm: commanded pitch attitude [radians]
:return: None
"""
error = pitch_comm - self.sim[prp.pitch_rad]
kp = 1.0
ki = 0.0
kd = 0.03
controller = PID(kp, ki, 0.0)
output = controller(error)
# self.sim[prp.elevator_cmd] = output
rate = self.sim[prp.q_radps]
rate_controller = PID(kd, 0.0, 0.0)
rate_output = rate_controller(rate)
output = output + rate_output
self.sim[prp.elevator_cmd] = output
def main():
global quit, out, control, err
fourcc = cv2.VideoWriter_fourcc(*'XVID')
out = cv2.VideoWriter('.\CV_DS\debug_1.avi', fourcc, fps, frame_size)
keyboard.on_press_key("1", toggle_quit, suppress=True)
pid = PID(1, 0.1, 0.05, setpoint=1)
pid.sample_time = 1
print("Press 1 to start")
while quit:
sleep(1)
print("Press 1 to quit")
while not quit:
frame = np.array(ImageGrab.grab())
err = get_err(frame)
control = pid(err)
update(control, err)
cv2.destroyAllWindows()
keyboard.unhook_all()
out.release()
def __init__(self, max_angle=45, max_setpoint_angle=12.5, scaling_factor=1000, max_collinear_offset=120000):
self.MAX_ANGLE = math.radians(max_angle)
self.MAX_SETPOINT_ANGLE = math.radians(max_setpoint_angle)
self.MAX_COLLINEAR_OFFSET = max_collinear_offset
self.MAX_ROTATIONAL_OFFSET = 100000
self.SCALING_FACTOR = scaling_factor
#self.pid = pid_class.PID()
self.pid = PID(400, 10, 0, setpoint=0)
#self.pid.update_constants()
self.pid_output = 0
self.imu = mpu.Sensors(1, 0x68)
self.remote = remote.RemoteController()
self.update_user_angle()
self.odrv = drive.OdriveController()
self.setup_odrive()
self.angle = self.dt = 0 # Values set in update_pid
self.running = False
self.sample_time = 0.001
self.update_pid()
def __init__(self):
self.client = base.Client(('localhost', 11211))
self.pid = PID(float(self.client.get("p")),
float(self.client.get("i")),
float(self.client.get("d")),
setpoint=0)
self.pid.output_limits = (-90, 90)
self.pid.sample_time = .5
self.steps = 0
self.i = float(self.client.get("i"))
def __init__(self):
# ROS setup
rospy.init_node('control')
self.rate = rospy.Rate(60)
self.running = False
# ROS Parameters
self.vel_topic = rospy.get_param("/vel_topic")
#self.vel_topic = "/tello/cmd_vel"
self.pose_topic = rospy.get_param("/pose_topic")
#self.pose_topic = "/orb_slam2_mono/pose"
# self.pid_config_file = rospy.get_param("~pid_config_file")
# self.calibrate_pid = rospy.get_param('~calibrate_pid',False)
# Publishers
self.vel_pub = rospy.Publisher(self.vel_topic, Twist, queue_size=1)
# Subscribers
self.running_sub = rospy.Subscriber('/control/set_running_state', Bool,
self.running_cb)
self.slam_pose_sub = rospy.Subscriber(self.pose_topic, PoseStamped,
self.pose_callback)
self.cmd_pose_sub = rospy.Subscriber('/viscon/set_position', Pose,
self.set_pose_callback)
self.last_time = time.time()
self.delay = 0
# Servers
#self.cfg_srv = Server(ControllerConfig, self.cfg_callback)
# Attributes
self.goal_pose = PoseStamped()
self.current_pose = PoseStamped()
self.velocity = Twist()
self.scale_factor = 1
self.is_losted = True
# PIDs
self.pid_x = PID(0.4, 0.01, 0.05)
self.pid_y = PID(0.4, 0.01, 0.05)
self.pid_z = PID(0.3, 0.008, 0.01)
self.pid_w = PID(0.5, 0, 0.01)
self.pid_x.output_limits = self.pid_y.output_limits = (
-0.8, 0.8) # output value will be between -1 and 1
self.pid_z.output_limits = (
-0.5, 0.5) # output value will be between -0.8 and 0.8
def __init__(self):
self.motorhat = Adafruit_MotorHAT(0x60, i2c_bus=1)
self.left_motor = self.motorhat.getMotor(1)
self.right_motor = self.motorhat.getMotor(2)
self.pid_r = PID(1500.0, 500.0, 100.0,
sample_time=0.01) # P/I/D for right wheel
self.pid_l = PID(1500.0, 500.0, 100.0,
sample_time=0.01) # P/I/D for left wheel
self.pid_r.output_limits = (-255, 255)
self.pid_l.output_limits = (-255, 255) # PWM limit
self.port = rospy.get_param(
"~port", '/dev/ttyACM0') # Arduino port from parameter server
self.pub_tf = rospy.get_param(
"~pub_tf", False) # If true, broadcasr transform from
# odom to car_base
self.ard = serial.Serial(self.port, 57600)
# Flush serial data
for i in range(0, 20):
_ = self.ard.readline()
# Subscriber and publisher
self.pub_odom = rospy.Publisher('/wheel_odom', Odometry, queue_size=10)
self.sub_cmd = rospy.Subscriber('/cmd_vel',
Twist,
self.cmd_cb,
queue_size=1)
if self.pub_tf:
self.tf_br = tf.TransformBroadcaster()
# Service
self.reset_odom = rospy.Service('reset_wheel_odom', Empty,
self.reset_odom)
rospy.Timer(rospy.Duration(1 / 100.), self.read_data) # 100Hz
# Left and right wheel velocities
self.v_r = None
self.v_l = None
# Position variables
self.heading = 0
self.x = 0
self.y = 0
# Desired velocities
self.v_d = 0
self.w_d = 0
self.time = rospy.Time.now()
rospy.loginfo("[%s] Initialized" % (rospy.get_name()))
def reload(self):
config.read('settings.ini')
settings = config['SETTINGS']
self.setpoint = float(settings[self.name.lower()])
self.pid = PID(
self.pid_params['P'],
self.pid_params['I'],
self.pid_params['D'],
self.setpoint,
)
class Node:
def __init__(self):
rospy.init_node("joint_controller_node")
rospy.on_shutdown(self.shutdown)
rospy.loginfo("Setting up joint_controller_node")
self.pitch_pub = rospy.Publisher("roboclaw/pitch_vel",Twist,queue_size=10)
self.height_pub = rospy.Publisher("roboclaw/height_vel",Twist,queue_size=10)
self.joint_pub = rospy.Publisher('arm_joint_state',JointState,queue_size=10)
self.pitch_sub = rospy.Subscriber('roboclaw/pitch',JointState,self.pitch_pos_callback)
self.height_sub = rospy.Subscriber('roboclaw/height',JointState,self.height_pos_callback)
self.joint_status_pub = rospy.Subscriber('joint_states',JointState,self.joint_states_callback)
self.rate = rospy.Rate(5)
self.height_min_length = 15.71
self.height_max_length = 25.55
self.height_stroke_length = self.height_max_length - self.height_min_length
self.height_angle_offset = 369.69
self.pitch_min_length = 11.69
self.pitch_max_length = 17.6
self.pitch_stroke_length = self.pitch_max_length - self.pitch_min_length
self.start = False
self.h1_pid = PID(5,0,0,output_limits=(-127,127),auto_mode=False,sample_time=1/5)
self.h2_pid = PID(5,0,0,output_limits=(-127,127),auto_mode=False,sample_time=1/5)
self.p1_pid = PID(5,0,0,output_limits=(-127,127),auto_mode=False,sample_time=1/5)
self.p2_pid = PID(5,0,0,output_limits=(-127,127),auto_mode=False,sample_time=1/5)
self.MAX_ENC = 2047.0
self.quantize_div = 12.0
self.deadband = 11.0
self.h_params = {"m1":50.0,"m2":50.0}
self.p_params = {"m1":50.0,"m2":50.0}
self.p1_vals = [0.0,0.0,0.0,0.0,0.0]
self.p2_vals = [0.0,0.0,0.0,0.0,0.0]
self.h1_vals = [0.0,0.0,0.0,0.0,0.0]
self.h2_vals = [0.0,0.0,0.0,0.0,0.0]
self.height_pos = {"m1":0.0,"m2":0.0}
self.pitch_pos = {"m1":0.0,"m2":0.0}
def run(self):
rospy.loginfo("waiting for data")
while self.height_pos['m1'] == 0 and self.pitch_pos['m1'] == 0:
self.rate.sleep()
rospy.sleep(1)
rospy.loginfo("got data")
self.h1_pid.set_auto_mode(True,last_output=0.0)
self.h2_pid.set_auto_mode(True,last_output=0.0)
self.p1_pid.set_auto_mode(True,last_output=0.0)
self.p2_pid.set_auto_mode(True,last_output=0.0)
h1_old, h2_old, p1_old, p2_old = self.call_pids()
while not rospy.is_shutdown():
h1,h2,p1,p2 = self.call_pids()
h1,h2,p1,p2 = self.apply_deadband(h1,h2,p1,p2)
h1,h2,p1,p2 = self.safety_check(h1,h2,p1,p2,14,20)
if h1_old!=h1 or h2_old!=h2:
rospy.loginfo("pid output h1 %d, h2 %d", h1,h2)
if p1_old!=p1 or p2_old!=p2:
rospy.loginfo("pid output p1 %d, p2 %d", p1,p1)
h1_old = h1
h2_old = h2
p1_old = p1
p2_old = p2
self.publish_height_vel(-h1,h2)
self.publish_pitch_vel(-p1,-p2)
self.publish_arm_state()
self.rate.sleep()
def shutdown(self):
rospy.loginfo("Shutting down joint_controller_node")
def call_pids(self):
h1 = self.h1_pid(self.quantize(self.height_pos['m1']))
h2 = self.h2_pid(self.quantize(self.height_pos['m2']))
p1 = self.p1_pid(self.quantize(self.pitch_pos['m1']))
p2 = self.p2_pid(self.quantize(self.pitch_pos['m2']))
return h1,h2,p1,p2
def quantize(self, val):
return int(val/self.quantize_div)
def get_avg(self,lst):
return sum(lst) / len(lst)
def apply_deadband(self,h1,h2,p1,p2):
if abs(h1) < self.deadband: h1 = 0.0
if abs(h2) < self.deadband: h2 = 0.0
if abs(p1) < self.deadband: p1 = 0.0
if abs(p2) < self.deadband: p2 = 0.0
return h1,h2,p1,p2
def safety_check(self, h1, h2, p1, p2, thresh_p, thresh_o):
had_error = False
if abs(self.quantize(self.height_pos['m1'])-self.quantize(self.height_pos['m2'])) > thresh_p:
had_error = True
h1 = 0
h2 = 0
if abs(self.quantize(self.pitch_pos['m1'])-self.quantize(self.pitch_pos['m2'])) > thresh_p:
had_error = True
p1 = 0
p2 = 0
if abs(h1 - h2)>thresh_o:
had_error = True
h1 = 0
h2 = 0
if abs(p1 - p2)>thresh_o:
had_error = True
p1 = 0
p2 = 0
if had_error:
rospy.loginfo("ERROR!!!")
return h1, h2, p1, p2
def pitch_add_meas(self, p1, p2):
self.p1_vals.insert(0,p1)
self.p1_vals.pop()
self.p2_vals.insert(0,p2)
self.p2_vals.pop()
def height_add_meas(self, h1, h2):
self.h1_vals.insert(0,h1)
self.h1_vals.pop()
self.h2_vals.insert(0,h2)
self.h2_vals.pop()
def pitch_pos_callback(self, msg):
if msg.position[0] < 0.001 or msg.position[1] < 0.001:
self.pitch_add_meas(self.p1_vals[0],self.p2_vals[0])
else:
self.pitch_add_meas(msg.position[0],msg.position[1])
self.pitch_pos['m1'] = self.get_avg(self.p1_vals)
self.pitch_pos['m2'] = self.get_avg(self.p2_vals)
def height_pos_callback(self, msg):
if msg.position[0] < 0.001 or msg.position[1] < 0.001:
self.height_add_meas(self.h1_vals[0],self.h2_vals[0])
else:
self.height_add_meas(msg.position[0],msg.position[1])
self.height_pos['m1'] = self.get_avg(self.h1_vals)
self.height_pos['m2'] = self.get_avg(self.h2_vals)
def joint_states_callback(self,msg):
height_angle_target = msg.position[2]
height_inch_target = self.height_angle_to_dist(math.radians(self.height_angle_offset)-height_angle_target)
height_enc_target = self.height_inch_to_enc(height_inch_target,self.h_params['m1'])
height_enc_target = self.height_inch_to_enc(height_inch_target,self.h_params['m2'])
self.h1_pid.setpoint = self.quantize(height_enc_target)
self.h2_pid.setpoint = self.quantize(height_enc_target)
pitch_angle_target = msg.position[3]
pitch_inch_target = self.pitch_angle_to_dist(pitch_angle_target)
pitch_enc_target = self.pitch_inch_to_enc(pitch_inch_target,self.p_params['m1'])
pitch_enc_target = self.pitch_inch_to_enc(pitch_inch_target,self.p_params['m2'])
self.p1_pid.setpoint = self.quantize(pitch_enc_target)
self.p2_pid.setpoint = self.quantize(pitch_enc_target)
def publish_arm_state(self):
arm_msg = JointState()
arm_msg.header = Header()
arm_msg.header.stamp = rospy.Time.now()
arm_msg.name = ['base_to_lever_arm', 'lever_arm_to_digging_arm']
height_pos_inch = self.height_enc_to_inch(self.height_pos['m1'],self.h_params['m1'])
pitch_pos_inch = self.pitch_enc_to_inch(self.pitch_pos['m1'],self.p_params['m1'])
height_angle = math.radians(self.height_angle_offset) - self.height_dist_to_angle(height_pos_inch)
pitch_angle = self.pitch_dist_to_angle(pitch_pos_inch)
arm_msg.position = [height_angle, pitch_angle]
self.joint_pub.publish(arm_msg)
def publish_pitch_vel(self,m1,m2):
vel = Twist()
vel.linear.x = m1
vel.linear.y = m2
vel.linear.z = 0
self.pitch_pub.publish(vel)
def publish_height_vel(self,m1,m2):
vel = Twist()
vel.linear.x = m1
vel.linear.y = m2
vel.linear.z = 0
self.height_pub.publish(vel)
def height_enc_to_inch(self,enc,enc_min):
percent = float(enc-enc_min)/float(self.MAX_ENC-enc_min)
return self.height_min_length + self.height_stroke_length * percent
def height_inch_to_enc(self,inch,enc_min):
percent = float(inch-self.height_min_length)/float(self.height_stroke_length)
return float(enc_min) + float(self.MAX_ENC-enc_min) * percent
def pitch_enc_to_inch(self,enc,enc_min):
percent = float(enc-enc_min)/float(self.MAX_ENC-enc_min)
return self.pitch_min_length + self.pitch_stroke_length * percent
def pitch_inch_to_enc(self,inch,enc_min):
percent = float(inch-self.pitch_min_length)/float(self.pitch_stroke_length)
return enc_min + (self.MAX_ENC-enc_min) * percent
def height_angle_to_dist(self, theta):
return math.sqrt(793.5625 - 708*math.cos(math.radians(90)-theta))
def height_dist_to_angle(self, pos): #pos in inches
return math.radians(90 - math.degrees(math.acos((793.5625-(pos*pos))/708)))
def pitch_angle_to_dist(self, theta):
return math.sqrt(((30-11*math.cos(theta))-14.875)**2 + ((11*math.sin(theta)-3)-6.75)**2)
def pitch_dist_to_angle(self, pos):
return math.asin(0.05557*pos*math.sin(math.acos((pos*pos - 202.828125)/(22*pos*pos))))+math.radians(32.807)
