def rotate(self, angle_change): yaw_pid = pid(p=1, i=1, d=0.00, i_max=50, output_max=90) current_roll, current_pitch, current_yaw = self.gyro.get_angle() target_yaw = current_yaw + angle_change # yaw_error=target_yaw-current_yaw # if yaw_error>=180: # yaw_error-=360 # elif yaw_error<=-180: # yaw_error+=360 yaw_error = angle_change while abs(yaw_error) > 4.5: current_roll, current_pitch, current_yaw = self.gyro.get_angle() yaw_error = target_yaw - current_yaw if yaw_error >= 180: yaw_error -= 360 elif yaw_error <= -180: yaw_error += 360 # yaw_output_angle+=yaw_pid.calculate_pid(yaw_error) # print('Current:{:4}, Error:{:4}'.format(current_yaw,yaw_error)) time.sleep(0.01) # if yaw_error > 0: # left gpio.output(self.front_left_dir_pin, True) gpio.output(self.front_right_dir_pin, True) else: # right gpio.output(self.front_left_dir_pin, False) gpio.output(self.front_right_dir_pin, False) gpio.output(self.front_left_step_pin, True) gpio.output(self.front_right_step_pin, True) time.sleep(0.0015) gpio.output(self.front_left_step_pin, False) gpio.output(self.front_right_step_pin, False) time.sleep(0.0015)
pot = ADC(Pin(28)) pot_filter_count = 256 filter_count = 1 cycle = 0 duty = 65000 freq = 1000 dz_min = -.08 dz_max = .08 motor = Motor_Driver(6, 7, 8, None, 26, None, True) #(self, dir_pin, speed_pin, bk_pin, sleep_pin, motor_sense, current_sense, indicator): motor2 = Motor_Driver(16, 17, None, None, 27, None, None) motor.speed(freq, duty) motor2.speed(freq, duty) pid_1 = pid() pid_2 = pid() while True: filtered_pot = 0 for i in range(pot_filter_count): filtered_pot += pot.read_u16() filtered_pot = filtered_pot / pot_filter_count filtered_motor = 0 for i in range(filter_count): filtered_motor += motor.pot_read() filtered_motor = filtered_motor / filter_count # print(filtered_pot) # print(motor.pot_read()) # print("break")
from std_msgs.msg import String import geometry_msgs.msg import time import serial import sys from PID import pid from numpy import genfromtxt import tf import math from random import randint # yaw = pid(9,0,100) yaw = pid(3.4, 0, 2) # velo = pid(18500,0,10000) #velo = pid(9800,0,5000) velo = pid(1500, 0.09, 9.5) yaw_callback_time = 0.015 #15 ms yaw_callback_last_time = 0 ser = None __yaw = 0 __velo = 0 def open_connection(): global ser
from PID import pid import numpy as np import matplotlib.pyplot as plt dt = 0.05 f_sample = 1 / dt controlador_pid = pid(Kp=1, Ki=1, Kd=0.1) f = np.logspace(-2, 1.6, num=300) moduloy = np.array([]) modulox = np.array([]) ''' freq = 0.08 t = np.arange(0, 2*np.pi/freq, dt) x = np.sin(freq*t) y = np.array([]) for sample in x: c = controlador_pid(sample, dt=dt) y = np.append(y, c) plt.step(t,y) plt.step(t,x) plt.legend(['PID','Setpoint']) plt.show()''' for freq in f: print(freq) t = np.arange(0, 2 * np.pi / freq, dt) x = np.sin(freq * t) y = np.array([]) for sample in x:
led = Pin(15, Pin.OUT) button = Pin(14, Pin.IN, Pin.PULL_UP) pot = ADC(Pin(28)) filter_count = 256 cycle = 0 duty = 65000 freq = 1000 dz_min = -.08 dz_max = .08 motor = Motor_Driver(6, 7, 8, None, 26, 27, True) #(self, dir_pin, speed_pin, bk_pin, sleep_pin, motor_sense, current_sense, indicator): motor.speed(freq, duty) pid = pid() while True: filtered_pot = 0 for i in range(filter_count): filtered_pot += pot.read_u16() filtered_pot = filtered_pot / filter_count # print(filtered_pot) # print(motor.pot_read()) # print("break") print(pot.read_u16()) print(pid.comp_pid(motor.pot_read(), filtered_pot)) status = pid.comp_pid(motor.pot_read(), filtered_pot) if status < dz_min: led.off()
import serial import sys from PID import pid from numpy import genfromtxt import tf import math from random import randint from random import shuffle import threading import os yaw = pid(3.4, 0, 2) #velo = pid(9800,50,5000) velo = pid(1550, 0.095, 9.7) base_path = os.path.dirname(os.path.realpath(__file__)) yaw_callback_time = 0.015 #15 ms yaw_callback_last_time = 0 ser = None __yaw = 0 __velo = 0 time_frame_num = 0 map_index = None
import geometry_msgs.msg from sensor_msgs.msg import sensor_msgs, Image import math from cv_bridge import CvBridge import cv2 from PID import pid bridge = CvBridge() center = [0, 0] position = [0, 0, 0] controller_values = ["", "", ""] #TODO tune kp ki kd values xpid = pid(0, 0, 0) ypid = pid(0, 0, 0) zpid = pid(0, 0, 0) def get_center(data): h = data.height / 2 w = data.width / 2 global center center = [h, w] def read_position(data): x_pos = data.pose.position.x * 100 # millimeter