def testConvergenceWithADisminishingResponse(self):
"""Test convergence with a model where the response disminishes
linearly over time. We ignore the integral component here as
it is misleading."""
class DisminishingResponseModel(Model):
def __init__(self, *args, **kwargs):
super(DisminishingResponseModel,
self).__init__(*args, **kwargs)
self.age = 0
def ApplyUpdate(self):
self.age += 1
self.state += self.correction / self.age
pid = pidcontroller.PID(1.0, 0.0, 0.6, time())
model = DisminishingResponseModel()
# We start in target state, verify that no corrections are necessary
self.assertAlmostEqual(model.Error(), 0.0)
self.assertAlmostEqual(pid.Update(model.Error(), time()), 0.0)
# Target a state of 10.0
model.SetTargetState(10.0)
self.assertAlmostEqual(model.Error(), 10.0)
for i in xrange(100):
correction = pid.Update(model.Error(), time())
model.SetCorrection(correction)
model.Update()
self.assertAlmostEqual(model.Error(), 0.0)
self.assertAlmostEqual(correction, 0.0)
def main():
print("!!!")
set_speed = 15
cur_speed = 0.0
dv = 0.0
omega = 0.0
pid = pidcontroller.PID(1, 0.1, 1)
gear = 1
# Init file
if not path.exists("set_speed.txt"):
with open('set_speed.txt', 'w') as f:
f.write("50")
while (1):
# Cruise control
with open('set_speed.txt', 'r') as opened_file:
set_speed = int(opened_file.read()) / 3.6
diff = set_speed - cur_speed
"""
if diff > 0:
throttle = min(diff / 10, 0.8)
"""
spd_control = pid.Update(diff, dt=10, ci_limit_L=-10,
ci_limit_H=200) / 20
# Engine control
if spd_control > 1.0:
throttle = 1.0
elif spd_control < 0.0:
throttle = 0.0
else:
throttle = spd_control
mapping = [1100, 3000] # Shift down & shift up RPM
if (throttle > 0.9):
mapping = [4500, 7500]
rpm = omega / ((2 * pi) / 60)
if rpm > mapping[1]:
gear = min(5, gear + 1)
elif rpm < mapping[0]:
gear = max(1, gear - 1)
# Vehicle simulation
dv, omega = vehicle_update(0, [cur_speed], [throttle, gear, 0])
cur_speed = cur_speed + (dv * 0.1)
print("Speed %d KMH, RPM %d, Gear %d, Throttle %.2f" %
(cur_speed * 3.6, rpm, gear, throttle))
sleep(0.1)
def testConvergenceWithADampenedResponse(self):
"""Test convergence with a model that reacts linearly to the correction."""
class DampenedResponseModel(Model):
def ApplyUpdate(self):
self.state += self.correction / 100
pid = pidcontroller.PID(1.0, 0.5, 0.1, time())
model = DampenedResponseModel(target_state=10.0)
self.assertAlmostEqual(model.Error(), 10.0)
for i in xrange(4000):
correction = pid.Update(model.Error(), time())
model.SetCorrection(correction)
model.Update()
self.assertAlmostEqual(model.Error(), 0.0)
self.assertAlmostEqual(correction, 0.0)
def testConvergenceWithImmediateResponse(self):
"""Test convergence with a model who reacts instantly to a correction."""
class InstantModel(Model):
def ApplyUpdate(self):
self.state += self.correction
pid = pidcontroller.PID(1.0, 0.5, 0.1, time())
model = InstantModel()
# We start in target state, verify that no corrections are necessary
self.assertAlmostEqual(model.Error(), 0.0)
self.assertAlmostEqual(pid.Update(model.Error(), time()), 0.0)
# Target a state of 10.0
model.SetTargetState(10.0)
self.assertAlmostEqual(model.Error(), 10.0)
for i in xrange(30):
correction = pid.Update(model.Error(), time())
model.SetCorrection(correction)
model.Update()
self.assertAlmostEqual(model.Error(), 0.0)
self.assertAlmostEqual(correction, 0.0)
def positive_control(self, x, y):
print(
'-----------------------------------hi_4------------------------------------------'
)
pid_pitch = pidcontroller.PID(50, 0.0, 0) #2.8, 1.8 level 2
pid_roll = pidcontroller.PID(50, 0.0, 0) #5.0, 2.0 level 2
#target_yaw = 0 # 0 is north
target_x = x
target_y = y
start_time_pure = time.gmtime()
start_time = start_time_pure.tm_sec
'''heading = ["X-coord", "Y-coord", "Z-coord", "x", "y", "z", "count", "position"]
with open('XYZ_data.csv', 'w', newline='') as csvFile:
writer = csv.writer(csvFile)
writer.writerow(heading)
csvFile.close()
heading = ["pitch", "roll", "yaw", "throttle"]
with open('Control_data.csv', 'w', newline='') as csvFile:
writer = csv.writer(csvFile)
writer.writerow(heading)
csvFile.close()
heading = ["pitch", "roll", "yaw", "throttle"]
with open('Control_raw_data.csv', 'w', newline='') as csvFile:
writer = csv.writer(csvFile)
writer.writerow(heading)
csvFile.close()
heading = ["error_x_forward", "error_y_sideward", "error_z_vertical", "error_yaw"]
with open('Error_data.csv', 'w', newline='') as csvFile:
writer = csv.writer(csvFile)
writer.writerow(heading)
csvFile.close()
'''
while (True):
#print('--------time----------',time.time(),'------------time--------------')
self.pos_feedback()
current_x = self.agent_pos[0]
current_y = self.agent_pos[1]
print('================================', current_x,
'=======================================')
print('================================', current_y,
'=======================================')
#current_altitude = -Y + bottom_to_checker_origin - bottom_to_drone_camera
error_x = target_x - current_x
error_y = target_y - current_y
print('================================', error_x,
'=======================================')
print('================================', error_y,
'=======================================')
pitch_correction = pid_pitch.Update(error_x)
roll_correction = -pid_roll.Update(error_y)
print('================================', pitch_correction,
'=======================================')
print('================================', roll_correction,
'=======================================')
if abs(error_x) < 0.05 and abs(error_y) < 0.05:
#self.drone(Landing())
#self.drone.disconnection()
self.drone(
PCMD(1, 0, 0, 0, 0, timestampAndSeqNum=0, _timeout=10))
break
else:
self.set_control_magnitude(0, int(round(pitch_correction)),
int(round(roll_correction)), 0)
time.sleep(1)
print('Current x: %f' % current_x)
print('Current y: %f' % current_y)
print('Error_x %f' % error_x)
print('Error_y %f' % error_y)
print('Setting the pitch command to %f' % pitch_correction)
print('Setting the roll command to %f' % roll_correction)
def main():
print("!!!")
set_speed = 15
cur_speed = 0.0
dv = 0.0
omega = 0.0
pid = pidcontroller.PID(1, 0.1, 1)
gear = 1
curr_speeds = []
prev_speeds = []
throttles = []
gears = []
df = None
if not path.exists("data.csv"):
df = pd.DataFrame(columns=[
"Current Speed", "Previous Speed", "Throttle", "Gear", "Class"
])
else:
df = pd.read_csv("data.csv")
# Init file
if not path.exists("set_speed.txt"):
with open('set_speed.txt', 'w') as f:
f.write("50")
my_class = 1
# good = 0
for i in range(4000):
# Cruise control
with open('set_speed.txt', 'r') as opened_file:
set_speed = int(opened_file.read()) / 3.6
diff = set_speed - cur_speed
"""
if diff > 0:
throttle = min(diff / 10, 0.8)
"""
spd_control = pid.Update(diff, dt=5, ci_limit_L=-10,
ci_limit_H=200) / 20
# Engine control
if spd_control > 1.0:
throttle = 1.0
elif spd_control < 0.0:
throttle = 0.0
else:
throttle = spd_control
mapping = [1100, 3000] # Shift down & shift up RPM
if (throttle > 0.9):
mapping = [4500, 7500]
rpm = omega / ((2 * pi) / 60)
if rpm > mapping[1]:
gear = min(5, gear + 1)
elif rpm < mapping[0]:
gear = max(1, gear - 1)
# Vehicle simulation
dv, omega = vehicle_update(0, [cur_speed], [throttle, gear, 0])
new_speed = cur_speed + (dv * 0.2)
if i % 4 == 0:
df = df.append(
{
"Current Speed": new_speed * 3.6,
"Previous Speed": cur_speed * 3.6,
"Throttle": throttle,
"Gear": gear,
"Class": my_class
},
ignore_index=True)
cur_speed = new_speed
print("Speed %d KMH, RPM %d, Gear %d, Throttle %.2f" %
(cur_speed * 3.6, rpm, gear, throttle))
sleep(0.2)
df.to_csv("data.csv")
def __init__(self, use_ros=True):
""" This init function sets the Raspberry ports to control the H-Bridge and initializes the ArUco interface """
en = 25 # PWM port
enb = 17 # PWM2 port
self.in1 = 24 # Motor A forward direction control
self.in2 = 23 # Motor A backwards direction control
# Escolhemos duas GPIOs proximas da do Motor A, aleatorio
self.inB1 = 22 # Motor B forward direction control
self.inB2 = 27 # Motor B backwards direction control
# Mode of refering to the ports from Raspberry
GPIO.setmode(GPIO.BCM)
# Motor A, setting the ports to control the direction of the motor
GPIO.setup(self.in1, GPIO.OUT) # Forward
GPIO.setup(self.in2, GPIO.OUT) # Backwards
# Motor B, setting the ports to control the direction of the motor
GPIO.setup(self.inB1, GPIO.OUT) # Forward
GPIO.setup(self.inB2, GPIO.OUT) # Backwards
# PWM is output
GPIO.setup(en, GPIO.OUT)
GPIO.setup(enb, GPIO.OUT)
# By default none of the motors should run
GPIO.output(self.in1, GPIO.LOW)
GPIO.output(self.in2, GPIO.LOW)
GPIO.output(self.inB1, GPIO.LOW)
GPIO.output(self.inB2, GPIO.LOW)
# PWM on port 25 and 17 with 1000Hz of frequency
pwm = GPIO.PWM(en, 1000)
pwm2 = GPIO.PWM(enb, 1000)
# On our experiment we will be working around 60% of the PWM, i.e 4.5V applied to the motors
pwm.start(80)
pwm2.start(80)
print("[INFO]: --- Initialized motors from the Robot ---")
self.pid = pidcontroller.PID(200, 300, 0)
# Setting the class variables to be 0 at start and then they will be updated as long the subscriber retrieves them
self.x = 0
self.z = 0
if use_ros:
# Retrieve the messages over ROS from the position
rospy.init_node("aruco_receiver")
self.positionSubscriber = rospy.Subscriber("marker_position",
Point,
self.positionCallback)
print("[INFO]: --- Retrieving position from ArUco Marker! ---")
# [Caso fosse usar Thread] Initializing the Thread
if use_ros != True:
self.ArucoInstance = pi_aruco_interface.ArucoInterface()
# The problem with the thread is because the opening of the camera is concurrent
self.t = threading.Thread(target=self.ArucoInstance.track_aruco,
args=("Thread sendo executada", ))
#self.runTest()
self.modelIdentification()
def testTargetState(self):
"""A system in the desired state should not have any correction."""
pid = pidcontroller.PID(1.0, 1.0, 1.0, time())
error = 0.0
correction = pid.Update(error, time())
self.assertAlmostEqual(correction, 0.0)
import socket
import sys
import select
import platform
import time
from math import pi
from time import sleep
import pidcontroller
from os import path
import fcntl
from common import *
ENG_CTL_HOST = "10.0.0.2"
ENG_CTL_PORT = 53599
pid = pidcontroller.PID(1, 0.1, 1)
connection_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
connection_socket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
eprint("\n Connecting to engine controller on %s:%s" %
(ENG_CTL_HOST, str(ENG_CTL_PORT)))
# Init file
if not path.exists("set_speed.txt"):
with open('set_speed.txt', 'w') as f:
f.write("50")
while(1):
try:
connection_socket.connect((ENG_CTL_HOST, ENG_CTL_PORT))
except ConnectionRefusedError:
sleep(0.5)
P= 0.5
I= 0.2
D= 0.5
LIMIT = 300
ADJUST = LIMIT / 10
FL=motor.Motor(motorFL)
FR=motor.Motor(motorFR)
RR=motor.Motor(motorRR)
RL=motor.Motor(motorRL)
targetAngle=[0,0,0]
#init classes
pid = pidcontroller.PID(P , I , D)
imu = imu.IMU6()
def getImuData():
return imu.getData(time.time())
def calculateDiff(presentAngle, targetAngle):
diff =[]
for n in range(3):
diff.append(targetAngle[n]-presentAngle[n])
try:
diffIndex = diff.index(max(list(map(abs,diff))))
except ValueError:
diffIndex = diff.index((-1)*max(list(map(abs,diff))))
return diff[diffIndex]
GPIO.cleanup()
print("cleaned")
GPIO.setmode(GPIO.BCM)
#constant init
MotorFrequency = 660
#port init
motorFL = 22 #Front Left
motorFR = 21 #Front Right
motorRL = 23 #Rear Left
motorRR = 24 #Rear Right
#constant
VARIANCE = 1
#control variables init
pid = pidcontroller.PID(0.05, 0.005, 0.005)
calibrationRate = 1000
ADD_DUTY = 3
LOW_DUTY = 72
def dutyCalibration(degree):
#return degree
return round(
((round(degree, 4) + calibrationRate) / (calibrationRate * 2)) * 100,
3)
def angleCalibration(degree):
#return degree