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