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)