def __init__(self, fnom):
super().__init__(fnom)
self.ctrl_heading = PIDController(
Kp=0, Ki=0, Kd=0, Kf=0,
source=self.drivetrain.getAngle,
output=self.correct_heading,
period=self.period,
)
self.ctrl_heading.setInputRange(-180, 180)
self.ctrl_heading.setOutputRange(-0.5, 0.5)
self.ctrl_heading.setContinuous(True)
self.max_velocity_fps = 11
self.max_velocity_encps = self.drivetrain.fps_to_encp100ms(self.max_velocity_fps)
self.ctrl_l = DriveController(
Kp=0, Kd=0,
Ks=1.293985, Kv=0.014172, Ka=0.005938,
get_voltage=self.drivetrain.getVoltage,
source=self.drivetrain.getLeftEncoderVelocity,
output=self.drivetrain.setLeftMotor,
period=self.period,
)
self.ctrl_l.setInputRange(-self.max_velocity_encps, self.max_velocity_encps)
self.ctrl_r = DriveController(
Kp=0, Kd=0,
Ks=1.320812, Kv=0.013736, Ka=0.005938,
get_voltage=self.drivetrain.getVoltage,
source=self.drivetrain.getRightEncoderVelocity,
output=self.drivetrain.setRightMotor,
period=self.period,
)
self.ctrl_r.setInputRange(-self.max_velocity_encps, self.max_velocity_encps)
def pid_action(self, pidvalue):
PID = PIDController(P=-40, I=1.0, D=5.0)
PIDx = PID.step(pidvalue)
if PIDx < 0.0:
backward(-float(PIDx))
elif PIDx > 0.0:
forward(float(PIDx))
else:
equilibrium()
def mainLoop():
inputSudut = round(angleMeter.get_kalman_roll(), 0) + round(
calibrationValue, 0)
PID = PIDController(P, I, D) #eight
PIDx = round(PID.step(inputSudut), 1)
if inputSudut < -setPoint:
forward(-(PIDx))
# print("forward")
elif inputSudut > setPoint:
backward((PIDx))
# print("back")
else:
equilibrium()
# print("equi")
print(round(inputSudut, 2), 'PID: ', PIDx)
def __init__(self, distance_ft):
super().__init__("ProfiledForward")
self.drivetrain = self.getRobot().drivetrain
self.requires(self.drivetrain)
self.dist_enc = distance_ft * self.drivetrain.ratio
self.period = 0.02
self.ctrl_heading = PIDController(
Kp=0, Ki=0, Kd=0, Kf=0,
source=self.drivetrain.getAngle,
output=self.correct_heading,
period=self.period,
)
self.ctrl_heading.setInputRange(-180, 180)
self.ctrl_heading.setOutputRange(-0.5, 0.5)
self.max_velocity = self.drivetrain.fps_to_encp100ms(3)
self.max_acceleration = self.drivetrain.fps2_to_encpsp100ms(3)
self.profiler_l = TrapezoidalProfile(
cruise_v=self.max_velocity,
a=self.max_acceleration,
target_pos=self.dist_enc,
tolerance=(3/12.)*self.drivetrain.ratio, # 3 inches
)
self.profiler_r = TrapezoidalProfile(
cruise_v=self.max_velocity,
a=self.max_acceleration,
target_pos=-self.dist_enc,
tolerance=(3/12.)*self.drivetrain.ratio, # 3 inches
)
self.ctrl_l = DriveController(
Kp=0, Kd=0,
Ks=1.293985, Kv=0.014172, Ka=0.005938,
get_voltage=self.drivetrain.getVoltage(),
source=self.drivetrain.getLeftEncoderVelocity,
output=self.drivetrain.setLeftMotor,
period=self.period,
)
self.ctrl_l.setInputRange(-self.max_velocity, self.max_velocity)
self.ctrl_r = DriveController(
Kp=0, Kd=0,
Ks=1.320812, Kv=0.013736, Ka=0.005938,
get_voltage=self.drivetrain.getVoltage(),
source=self.drivetrain.getRightEncoderVelocity,
output=self.drivetrain.setRightMotor,
period=self.period,
)
self.ctrl_r.setInputRange(-self.max_velocity, self.max_velocity)
def __init__(self, p, i, d, period=None, f=0.0, name=None):
"""Instantiates a PIDCommand that will use the given p, i and d values.
It will use the class name as its name unless otherwise specified.
It will also space the time between PID loop calculations to be equal
to the given period.
:param p: the proportional value
:param i: the integral value
:param d: the derivative value
:param period: the time (in seconds) between calculations (optional)
:param f: the feed forward value
:param name: the name (optional)
"""
super().__init__(name)
self.controller = PIDController(p, i, d, f, self.returnPIDInput,
self.usePIDOutput, period)
gyro_x_delta = (gyroX * time_diff)
gyro_y_delta = (gyroY * time_diff)
gyro_total_x += gyro_x_delta
gyro_total_y += gyro_y_delta
rotation_x = x_rotation(accelX, accelY, accelZ)
rotation_y = y_rotation(accelX, accelY, accelZ)
#Complementary Filter
# last_x = K * (last_x + gyro_x_delta) + (K1 * rotation_x)
last_y = K * (last_y + gyro_y_delta) + (K1 * rotation_y)
# last_y = last_y - 15
#setting the PID values. Here you can change the P, I and D values according to yiur needs
PID = PIDController(P=50, I=0.0, D=0.0)
# PIDx = PID.step(last_x)
PIDy = PID.step(last_y)
#if the PIDx data is lower than 0.0 than move appropriately backward
if PIDy < 0.0:
backward(float(PIDy))
#StepperFor(-PIDx)
#if the PIDx data is higher than 0.0 than move appropriately forward
elif PIDy > 0.0:
forward(float(PIDy))
#StepperBACK(PIDx)
#if none of the above statements is fulfilled than do not move at all
else:
equilibrium()
m1.rotate(power)
m2.rotate(power)
def botMoveBackward(power):
# power = power * 1.5
m1.rotate(power)
m2.rotate(power)
def botEquilibrium():
m1.rotate(0)
m2.rotate(0)
PID = PIDController(P=50, I=0.01, D=1)
#import pdb; pdb.set_trace()
try:
while True:
a, b, c = getMPUVals()
print(f'{a} {b} {c}')
PIDx = PID.step(b)
print(PIDx)
if PIDx < 0.0:
botMoveBackward(PIDx)
elif PIDx > 0.0:
botMoveForward(PIDx)
else:
botEquilibrium()
#m1.rotate(255)
def change_actuator(self, percent):
self.percent = percent
self.w_applied = self.actuator_power * percent / 100
def start(self):
if self.timer is None:
self.update()
if __name__ == "__main__":
cooler_system = System()
cooler_system.actuator_power = -50 # negative, because it is a cooler
cooler_system.start()
cooler_pid = PIDController(kp=-5, ki=-3,
kd=-3) # negative values because it is a cooler
cooler_pid.setpoint = 200
heater_system = System()
heater_system.actuator_power = 100
heater_system.start()
heater_pid = PIDController(kp=5, ki=3, kd=3)
heater_pid.setpoint = 300
def update():
cooler_system.change_actuator(cooler_pid.update(cooler_system.temp))
heater_system.change_actuator(heater_pid.update(heater_system.temp))
print(
'{0:.2f} - {1:.2f} ({2:.1f} + {3:.1f} + {4:.1f}) - {5:.2f} - {6:.2f} ({7:.1f} + {8:.1f} + {9:.1f})'
.format(cooler_system.temp, cooler_system.percent,
cooler_pid.out_components[0], cooler_pid.out_components[1],
import time
import signal
from picamera.array import PiRGBArray
from picamera import PiCamera
from xbox360controller import Xbox360Controller
from pidcontroller import PIDController
# Import the PCA9685 module.
import Adafruit_PCA9685
# local modules
from video import create_capture
from common import clock, draw_str
pidControllerXY = (PIDController(), PIDController())
isJoyPadConnected=True
try:
joy = Xbox360Controller(0, axis_threshold=0.2)
except:
isJoyPadConnected=False
pass
print('is Joy Pad connected %d' % (isJoyPadConnected))
"""
Proportional: 0.05
Integral: 0.15
Derivative: 0.042
"""
Kp = 10.0
Kd = 0.0
Ki = 3.0
a = -(B + k * k / R) / (J + m * r * r)
b = k / ((J + m * r * r) * R)
dt = 0.005
t = np.arange(0.0, 30.0, dt)
x = np.zeros(len(t))
w = np.zeros(len(t))
control = np.zeros(len(t))
w_ref = 1.0
pid = PIDController(Kp, Kd, Ki, w_ref, w[0])
if __name__ == "__main__":
for i in range(0, len(t) - 1):
u = limit_control(pid.getControl(w[i], dt), 12)
control[i] = u
dx = w[i] * r
dw = a * w[i] + b * u
x[i + 1] = x[i] + dx * dt
w[i + 1] = w[i] + dw * dt
plot_x = pp.subplot(311)
plot_x.title.set_text("Position")
class PIDCommand(Command):
"""This class defines a Command which interacts heavily with a PID loop.
It provides some convenience methods to run an internal PIDController.
It will also start and stop said PIDController when the PIDCommand is
first initialized and ended/interrupted.
"""
def __init__(self, p, i, d, period=None, f=0.0, name=None):
"""Instantiates a PIDCommand that will use the given p, i and d values.
It will use the class name as its name unless otherwise specified.
It will also space the time between PID loop calculations to be equal
to the given period.
:param p: the proportional value
:param i: the integral value
:param d: the derivative value
:param period: the time (in seconds) between calculations (optional)
:param f: the feed forward value
:param name: the name (optional)
"""
super().__init__(name)
self.controller = PIDController(p, i, d, f, self.returnPIDInput,
self.usePIDOutput, period)
def getPIDController(self):
"""Returns the PIDController used by this PIDCommand.
Use this if you would like to fine tune the pid loop.
Notice that calling setSetpoint(...) on the controller
will not result in the setpoint being trimmed to be in
the range defined by setSetpointRange(...).
:returns: the PIDController used by this PIDCommand
"""
return self.controller
def _initialize(self):
self.controller.enable()
def _end(self):
self.controller.disable()
def _interrupted(self):
self._end()
def setSetpointRelative(self, deltaSetpoint):
"""Adds the given value to the setpoint.
If :meth:`setRange` was used, then the bounds will still be honored by
this method.
:param deltaSetpoint: the change in the setpoint
"""
self.setSetpoint(self.getSetpoint() + deltaSetpoint)
def setSetpoint(self, setpoint):
"""Sets the setpoint to the given value. If :meth:`setRange` was called,
then the given setpoint will be trimmed to fit within the range.
:param setpoint: the new setpoint
"""
self.controller.setSetpoint(setpoint)
def getSetpoint(self):
"""Returns the setpoint.
:returns: the setpoint
"""
return self.controller.getSetpoint()
def getPosition(self):
"""Returns the current position
:returns: the current position
"""
return self.returnPIDInput()
def returnPIDInput(self):
"""Returns the input for the pid loop.
It returns the input for the pid loop, so if this command was based
off of a gyro, then it should return the angle of the gyro
All subclasses of PIDCommand must override this method.
This method will be called in a different thread then the :class:`.Scheduler`
thread.
:returns: the value the pid loop should use as input
"""
raise NotImplementedError
def usePIDOutput(self, output):
"""Uses the value that the pid loop calculated. The calculated value
is the "output" parameter.
This method is a good time to set motor values, maybe something along
the lines of `driveline.tankDrive(output, -output)`.
All subclasses of PIDCommand should override this method.
This method will be called in a different thread then the Scheduler
thread.
:param output: the value the pid loop calculated
"""
pass
def initSendable(self, builder):
self.controller.initSendable(builder)
super().initSendable(builder)
builder.setSmartDashboardType("PIDCommand")
class CsvTrajectoryCommand(_CsvTrajectoryCommand):
def __init__(self, fnom):
super().__init__(fnom)
self.ctrl_heading = PIDController(
Kp=0, Ki=0, Kd=0, Kf=0,
source=self.drivetrain.getAngle,
output=self.correct_heading,
period=self.period,
)
self.ctrl_heading.setInputRange(-180, 180)
self.ctrl_heading.setOutputRange(-0.5, 0.5)
self.ctrl_heading.setContinuous(True)
self.max_velocity_fps = 11
self.max_velocity_encps = self.drivetrain.fps_to_encp100ms(self.max_velocity_fps)
self.ctrl_l = DriveController(
Kp=0, Kd=0,
Ks=1.293985, Kv=0.014172, Ka=0.005938,
get_voltage=self.drivetrain.getVoltage,
source=self.drivetrain.getLeftEncoderVelocity,
output=self.drivetrain.setLeftMotor,
period=self.period,
)
self.ctrl_l.setInputRange(-self.max_velocity_encps, self.max_velocity_encps)
self.ctrl_r = DriveController(
Kp=0, Kd=0,
Ks=1.320812, Kv=0.013736, Ka=0.005938,
get_voltage=self.drivetrain.getVoltage,
source=self.drivetrain.getRightEncoderVelocity,
output=self.drivetrain.setRightMotor,
period=self.period,
)
self.ctrl_r.setInputRange(-self.max_velocity_encps, self.max_velocity_encps)
def initialize(self):
self.drivetrain.zeroEncoders()
self.drivetrain.zeroNavx()
self.ctrl_l.enable()
self.ctrl_r.enable()
self.ctrl_heading.enable()
self.logger = DataLogger("csv_trajectory1.csv")
self.drivetrain.init_logger(self.logger)
self.logger.add("profile_vel_r", lambda: self.target_v_r)
self.logger.add("profile_vel_l", lambda: self.target_v_l)
self.logger.add("pos_ft_l", lambda: self.pos_ft_l)
self.logger.add("i", lambda: self.i)
self.timer.start()
self.i = 0
#print ('pdf init')
def execute(self):
self.pos_ft_l = self.drivetrain.getLeftEncoder() / self.drivetrain.ratio
self.pos_ft_r = self.drivetrain.getRightEncoder() / self.drivetrain.ratio
(_, _, _, self.target_v_l, self.target_v_r, self.target_a_l,
self.target_a_r, self.target_heading) = self.get_trajectory_point_enc(self.i)
self.ctrl_l.setSetpoint(self.target_v_l)
self.ctrl_l.setAccelerationSetpoint(self.target_a_l)
self.ctrl_r.setSetpoint(self.target_v_r)
self.ctrl_r.setAccelerationSetpoint(self.target_a_r)
self.ctrl_heading.setSetpoint(self.target_heading)
self.drivetrain.feed()
self.logger.log()
self.i += 1
def end(self):
self.ctrl_l.disable()
self.ctrl_r.disable()
self.ctrl_heading.disable()
self.drivetrain.off()
self.logger.flush()
#print ('pdf end')
def correct_heading(self, correction):
pass
gy_total = (last_y) - gyro_offset_y
time_diff = 0.035
K = 0.98
#PID1 = PIDController(P=-40, I=-100, D=-2.5)
#PID1 = PIDController(P=-42, I=-10, D=-5)
#PID1 = PIDController(P=-32, I=-1, D=-5)
#PID1 = PIDController(P=-25, I=-4.5, D=-5)
#PID1 = PIDController(P=-25, I=-0.04, D=-8)
#PID1 = PIDController(P=-15, I=-1, D=-1)
PID1 = PIDController(P=-13, I=-1, D=-1)
with open("./target_value.txt", "r") as file:
targetvalue = float(file.read())
PID1.setTarget(targetvalue)
while True:
try:
# forward(30)
# time.sleep(2)
# backward(30)
# time.sleep(2)
# stand_still()
# time.sleep(2)
accel_data = sensor.get_accel_data()
gyro_data = sensor.get_gyro_data()
m1.rotate(0)
m2.rotate(0)
def map(x, in_min, in_max, out_min, out_max):
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
with open('P', 'r') as f:
kp = float(f.read())
with open('I', 'r') as f:
ki = float(f.read())
with open('D', 'r') as f:
kd = float(f.read())
PID = PIDController(P=kp, I=ki, D=kd)
# import pdb; pdb.set_trace()
try:
while True:
a, b, c = getMPUVals()
b = b - 1.5
PIDx = PID.step(b)
print(f'{b} {PIDx}')
if PIDx < 0.0:
#botMoveBackward(-map(-PIDx, 0, 1000, 40, 200))
botMoveBackward(PIDx)
elif PIDx > 0.0:
#botMoveForward(map(PIDx, 0, 1000, 40, 200))
botMoveForward(PIDx)
else:
botEquilibrium()
v = [0.0]
x = [0.0]
x0 = 0.0
dt = 0.01
time = np.arange(0, 10, dt)
Kp = 60.0
Kd = 14.0
Ki = 0.0
xKp = 2.0
xKd = 3.0
xKi = 0.0
pid_th = PIDController(Kp, Kd, Ki, 0.0, th[0])
pid_x = PIDController(xKp, xKd, xKi, 0.0, x[0])
for i in range(0, len(time) - 1):
u = pid_th.getControl(th[i], dt) + pid_x.getControl(x[i], dt)
f.append(-u)
d2th = (g * sin(th[i]) - f[i] * cos(th[i])) / L
dth1 = w[i] + d2th * dt
v.append(v[i] + f[i] * dt)
x.append(x[i] + v[i] * dt)
th.append(th[i] + dth1 * dt)
w.append(dth1)
class ProfiledForward(wpilib.command.Command):
def __init__(self, distance_ft):
super().__init__("ProfiledForward")
self.drivetrain = self.getRobot().drivetrain
self.requires(self.drivetrain)
self.dist_enc = distance_ft * self.drivetrain.ratio
self.period = 0.02
self.ctrl_heading = PIDController(
Kp=0, Ki=0, Kd=0, Kf=0,
source=self.drivetrain.getAngle,
output=self.correct_heading,
period=self.period,
)
self.ctrl_heading.setInputRange(-180, 180)
self.ctrl_heading.setOutputRange(-0.5, 0.5)
self.max_velocity = self.drivetrain.fps_to_encp100ms(3)
self.max_acceleration = self.drivetrain.fps2_to_encpsp100ms(3)
self.profiler_l = TrapezoidalProfile(
cruise_v=self.max_velocity,
a=self.max_acceleration,
target_pos=self.dist_enc,
tolerance=(3/12.)*self.drivetrain.ratio, # 3 inches
)
self.profiler_r = TrapezoidalProfile(
cruise_v=self.max_velocity,
a=self.max_acceleration,
target_pos=-self.dist_enc,
tolerance=(3/12.)*self.drivetrain.ratio, # 3 inches
)
self.ctrl_l = DriveController(
Kp=0, Kd=0,
Ks=1.293985, Kv=0.014172, Ka=0.005938,
get_voltage=self.drivetrain.getVoltage(),
source=self.drivetrain.getLeftEncoderVelocity,
output=self.drivetrain.setLeftMotor,
period=self.period,
)
self.ctrl_l.setInputRange(-self.max_velocity, self.max_velocity)
self.ctrl_r = DriveController(
Kp=0, Kd=0,
Ks=1.320812, Kv=0.013736, Ka=0.005938,
get_voltage=self.drivetrain.getVoltage(),
source=self.drivetrain.getRightEncoderVelocity,
output=self.drivetrain.setRightMotor,
period=self.period,
)
self.ctrl_r.setInputRange(-self.max_velocity, self.max_velocity)
def initialize(self):
self.drivetrain.zeroEncoders()
self.drivetrain.zeroNavx()
self.ctrl_l.enable()
self.ctrl_r.enable()
self.ctrl_heading.enable()
def execute(self):
pos_l = self.drivetrain.getLeftEncoder()
pos_r = self.drivetrain.getRightEncoder()
self.profiler_l.calculate_new_velocity(pos_l, self.period)
self.profiler_r.calculate_new_velocity(pos_r, self.period)
self.ctrl_l.setSetpoint(self.profiler_l.current_target_v)
self.ctrl_l.setAccelerationSetpoint(self.profiler_l.current_a)
self.ctrl_r.setSetpoint(self.profiler_r.current_target_v)
self.ctrl_r.setAccelerationSetpoint(self.profiler_r.current_a)
def isFinished(self):
return (
self.profiler_l.current_target_v == 0 and
self.profiler_l.current_a == 0 and
self.profiler_r.current_target_v == 0 and
self.profiler_r.current_a == 0)
def end(self):
self.ctrl_l.disable()
self.ctrl_r.disable()
self.ctrl_heading.disable()
self.drivetrain.off()
def correct_heading(self, correction):
correction = 1 + correction
self.profiler_l.setCruiseVelocityScale(correction)
self.profiler_r.setCruiseVelocityScale(-correction)
from matplotlib.figure import Figure
import Tkinter as tk
import ttk as ttk
from pidcontroller import PIDController
"""
Proportional: 0.05
Integral: 0.15
Derivative: 0.042
"""
DefaultProportional = 0.05
DefaultIntegral = 0.15
DefaultDerivative = 0.042
pidControl = PIDController()
class PIDGrapher(tk.Tk):
def __init__(self, *args, **kwargs):
tk.Tk.__init__(self, *args, **kwargs)
#tk.Tk.iconbitmap(self, default="clienticon.ico")
tk.Tk.wm_title(self, "PID Grapher")
self.mainFrame = tk.Frame(self)
self.mainFrame.grid_rowconfigure(0, weight=1)
self.mainFrame.grid_columnconfigure(0, weight=1)
self.mainFrame.pack(side="top", fill="both", expand=True)
Kp = 200.0
Kd = 20.0
Ki = 0.0
a = -(B + k * k / R) / (J + m * r * r)
b = k / ((J + m * r * r) * R)
dt = 0.05
t = np.arange(0.0, 30.0, dt)
x = np.zeros(len(t))
w = np.zeros(len(t))
control = np.zeros(len(t))
x[0] = 0.1
pid = PIDController(Kp, Kd, Ki, 0.0, x[0])
if __name__ == "__main__":
for i in range(0, len(t) - 1):
u = limit_control(pid.getControl(x[i], dt), 24)
control[i] = u
dx = w[i] * r
dw = a * w[i] + b * u
x[i + 1] = x[i] + dx * dt
w[i + 1] = w[i] + dw * dt
plot_x = pp.subplot(311)
plot_x.title.set_text("Position")
gyroY -= gyro_offset_y
gyro_x_delta = (gyroX * time_diff)
gyro_y_delta = (gyroY * time_diff)
gyro_total_x += gyro_x_delta
gyro_total_y += gyro_y_delta
rotation_x = x_rotation(accelX, accelY, accelZ)
rotation_y = y_rotation(accelX, accelY, accelZ)
#Complementary Filter
last_x = K * (last_x + gyro_x_delta) + (K1 * rotation_x)
#setting the PID values. Here you can change the P, I and D values according to yiur needs
PID = PIDController(P=-78.5, I=1.0, D=1.0)
PIDx = PID.step(last_x)
#if the PIDx data is lower than 0.0 than move appropriately backward
if PIDx < 0.0:
backward(-float(PIDx))
#StepperFor(-PIDx)
#if the PIDx data is higher than 0.0 than move appropriately forward
elif PIDx > 0.0:
forward(float(PIDx))
#StepperBACK(PIDx)
#if none of the above statements is fulfilled than do not move at all
else:
equilibrium()
# *** Helper Functions for math and trig ***
def distance(a, b):
# d^2 = a^2 + b^2
d = math.sqrt((a * a) + (b * b))
return d
while True:
# *** Setup and get sensor data ***
sensor_value = 0
# *** Setup and update PID feedback controller ***
PID = PIDController(P=set_Kp, I=set_Ki, D=set_Kd, theta=set_theta)
drive_value = PID.update(sensor_value)
if (drive_value < 0):
driveBack(drive_value)
elif (drive_value > 0):
driveForward(drive_value)
else:
driveStop()
time.sleep(0.1)
#the so called 'main loop' that loops around and tells the motors wether to move or not
while True:
gyro_data = sensor.euler #function to call raw gyroscope oriention data from IMU
gyroY = (gyro_data[1]) #variable created to only grab the y-axis gyro data
#variables for time for PID
time1 = time.process_time()
dt = time1 - last_time
last_time = time1
#Calling PIDcontroller class from pidcontroller, setting P, I, and D gains
PID = PIDController(P=10,
I=0.05,
D=0.09,
Time=dt,
lastI=lastI,
lastError=lastError)
PIDy, lastI, lastError = PID.step(gyroY)
if gyroY < -50: #if tile angle is greater than 50, shut off motors (dead-band)
equilibrium()
elif gyroY > 50: #if tile angle is greater than 50, shut off motors (dead-band)
equilibrium()
elif PIDy < 0: #if PID value is less than 0, drive motors backwards
backward(-PIDy)
elif PIDy > 0: #if PID value is more than 0, drive motors forwards
forward(PIDy)
def main():
sensor = mpu6050(0x68)
#K and K1 --> Constants used with the complementary filter
K = 0.98
K1 = 1 - K
time_diff = 0.02
ITerm = 0
#Calling the MPU6050 data
accel_data = sensor.get_accel_data()
gyro_data = sensor.get_gyro_data()
aTempX = accel_data['x']
aTempY = accel_data['y']
aTempZ = accel_data['z']
gTempX = gyro_data['x']
gTempY = gyro_data['y']
gTempZ = gyro_data['z']
last_x = x_rotation(aTempX, aTempY, aTempZ)
last_y = y_rotation(aTempX, aTempY, aTempZ)
gyro_offset_x = gTempX
gyro_offset_y = gTempY
gyro_total_x = (last_x) - gyro_offset_x
gyro_total_y = (last_y) - gyro_offset_y
while start:
accel_data = sensor.get_accel_data()
gyro_data = sensor.get_gyro_data()
accelX = accel_data['x']
accelY = accel_data['y']
accelZ = accel_data['z']
gyroX = gyro_data['x']
gyroY = gyro_data['y']
gyroZ = gyro_data['z']
gyroX -= gyro_offset_x
gyroY -= gyro_offset_y
gyro_x_delta = (gyroX * time_diff)
gyro_y_delta = (gyroY * time_diff)
gyro_total_x += gyro_x_delta
gyro_total_y += gyro_y_delta
rotation_x = x_rotation(accelX, accelY, accelZ)
rotation_y = y_rotation(accelX, accelY, accelZ)
#Complementary Filter
last_y = K * (last_y + gyro_y_delta) + (K1 * rotation_y)
#setting the PID values. Here you can change the P, I and D values according to your needs
PID = PIDController(P=slider1.get(), I=slider2.get(), D=slider3.get())
PIDx = PID.step(last_y)
#if the PIDx data is lower than 0.0 than move appropriately backward
if PIDx < 0.0:
if (PIDx < -100):
PIDx = -100
backward(-float(PIDx))
#StepperFor(-PIDx)
#if the PIDx data is higher than 0.0 than move appropriately forward
elif PIDx > 0.0:
if (PIDx > 100):
PIDx = 100
forward(float(PIDx))
#StepperBACK(PIDx)
#if none of the above statements is fulfilled than do not move at all
else:
equilibrium()
#update GUI
root.update()
class ProfiledForward(wpilib.command.Command):
def __init__(self, distance_ft):
super().__init__("ProfiledForward")
self.drivetrain = self.getRobot().drivetrain
self.requires(self.drivetrain)
self.dist_ft = distance_ft
self.dist_enc = distance_ft * self.drivetrain.ratio
self.timer = Timer()
self.period = self.getRobot().getPeriod()
self.ctrl_heading = PIDController(
Kp=0,
Ki=0,
Kd=0,
Kf=0,
source=self.drivetrain.getAngle,
output=self.correct_heading,
period=self.period,
)
self.ctrl_heading.setInputRange(-180, 180)
self.ctrl_heading.setOutputRange(-0.5, 0.5)
self.max_velocity_fps = 3 # ft/sec
self.max_v_encps = self.drivetrain.fps_to_encp100ms(
self.max_velocity_fps)
self.max_acceleration = 3 # ft/sec^2
self.profiler_l = TrapezoidalProfile(
cruise_v=self.max_velocity_fps,
a=self.max_acceleration,
target_pos=self.dist_ft,
tolerance=(3 / 12.), # 3 inches
)
self.profiler_r = TrapezoidalProfile(
cruise_v=self.max_velocity_fps,
a=self.max_acceleration,
target_pos=-self.dist_ft,
tolerance=(3 / 12.), # 3 inches
)
self.ctrl_l = DriveController(
Kp=0,
Kd=0,
Ks=1.293985,
Kv=0.014172,
Ka=0.005938,
get_voltage=self.drivetrain.getVoltage,
source=self.drivetrain.getLeftEncoderVelocity,
output=self.drivetrain.setLeftMotor,
period=self.period,
)
self.ctrl_l.setInputRange(-self.max_v_encps, self.max_v_encps)
self.ctrl_r = DriveController(
Kp=0,
Kd=0,
Ks=1.320812,
Kv=0.013736,
Ka=0.005938,
get_voltage=self.drivetrain.getVoltage,
source=self.drivetrain.getRightEncoderVelocity,
output=self.drivetrain.setRightMotor,
period=self.period,
)
self.ctrl_r.setInputRange(-self.max_v_encps, self.max_v_encps)
self.target_v_l = 0
self.target_v_r = 0
self.target_a_l = 0
self.target_a_r = 0
self.pos_ft_l = 0
self.pos_ft_r = 0
def initialize(self):
self.drivetrain.zeroEncoders()
self.drivetrain.zeroNavx()
self.ctrl_l.enable()
self.ctrl_r.enable()
self.ctrl_heading.enable()
self.logger = DataLogger("profiled_forward.csv")
self.drivetrain.init_logger(self.logger)
self.logger.add("profile_vel_r", lambda: self.target_v_r)
self.logger.add("profile_vel_l", lambda: self.target_v_l)
self.logger.add("pos_ft_l", lambda: self.pos_ft_l)
self.logger.add("target_pos_l", lambda: self.profiler_l.target_pos)
self.logger.add("adist_l", lambda: self.profiler_l.adist)
self.logger.add("err_l", lambda: self.profiler_l.err)
self.logger.add("choice_l", lambda: self.profiler_l.choice)
self.logger.add("adist_r", lambda: self.profiler_l.adist)
self.logger.add("err_r", lambda: self.profiler_l.err)
self.logger.add("choice_r", lambda: self.profiler_l.choice)
self.timer.start()
#print ('pdf init')
def execute(self):
self.pos_ft_l = self.drivetrain.getLeftEncoder(
) / self.drivetrain.ratio
self.pos_ft_r = self.drivetrain.getRightEncoder(
) / self.drivetrain.ratio
#print ('pdf exec ', self.timer.get())
self.profiler_l.calculate_new_velocity(self.pos_ft_l, self.period)
self.profiler_r.calculate_new_velocity(self.pos_ft_r, self.period)
self.target_v_l = self.drivetrain.fps_to_encp100ms(
self.profiler_l.current_target_v)
self.target_v_r = self.drivetrain.fps_to_encp100ms(
self.profiler_r.current_target_v)
self.target_a_l = self.drivetrain.fps2_to_encpsp100ms(
self.profiler_l.current_a)
self.target_a_r = self.drivetrain.fps2_to_encpsp100ms(
self.profiler_r.current_a)
self.ctrl_l.setSetpoint(self.target_v_l)
self.ctrl_l.setAccelerationSetpoint(self.target_a_l)
self.ctrl_r.setSetpoint(self.target_v_r)
self.ctrl_r.setAccelerationSetpoint(self.target_a_r)
#self.drivetrain.setLeftMotor(self.ctrl_l.calculateFeedForward())
#self.drivetrain.setRightMotor(self.ctrl_r.calculateFeedForward())
self.logger.log()
self.drivetrain.feed()
def isFinished(self):
return (abs(self.pos_ft_l - self.dist_ft) < 1 / .3
and self.profiler_l.current_target_v == 0
and self.profiler_l.current_a == 0
and self.profiler_r.current_target_v == 0
and self.profiler_r.current_a == 0)
def end(self):
self.ctrl_l.disable()
self.ctrl_r.disable()
self.ctrl_heading.disable()
self.drivetrain.off()
self.logger.flush()
#print ('pdf end')
def correct_heading(self, correction):
self.profiler_l.setCruiseVelocityScale(1 + correction)
self.profiler_r.setCruiseVelocityScale(1 - correction)