def test_auto_mode():
pid = PID(1, 0, 0, setpoint=10, sample_time=None)
# ensure updates happen by default
assert pid(0) == 10
assert pid(5) == 5
# ensure no new updates happen when auto mode is off
pid.auto_mode = False
assert pid(1) == 5
assert pid(7) == 5
# should reset when reactivating
pid.auto_mode = True
assert pid._last_input is None
assert pid._integral == 0
assert pid(8) == 2
# last update time should be reset to avoid huge dt
from simple_pid.PID import _current_time
pid.auto_mode = False
time.sleep(1)
pid.auto_mode = True
assert _current_time() - pid._last_time < 0.01
# check that setting last_output works
pid.auto_mode = False
pid.set_auto_mode(True, last_output=10)
assert pid._integral == 10
def get_pid():
print "pid is set"
pid = PID(0.001, 0.0000005, 0.0000005,setpoint=0.0)
pid.setpoint = 0.0
pid.sample_time = 0.01
pid.auto_mode = True
pid.output_limits = (-0.7, 0.7)
pid.proportional_on_measurement = True
return pid
def pid_process(output, p, i, d, objCoord, centerCoord):
## # signal trap to handle keyboard interrupt
## signal.signal(signal.SIGINT, signal_handler)
# create a PID and initialize it
pid = PID(p.value, i.value, d.value, setpoint=centerCoord.value)
pid.sample_time = 2.0
pid.auto_mode = True
## p.initialize()
# loop indefinitely
while True:
output.value = pid(objCoord.value)
def run(self):
# Get initial state
self.sensors.process()
#target_heading = -self.sensors.yaw().getValue()
# Set initial state of servos and motors
self.controls.stopAllMotors()
# Common controls
self.highPriorityProcesses.append(self.controls)
# Camera angle servo
cameraTiltServo = self.controls.servo(6)
currentPitch = Scaler(self.sensors.pitch(), scaling=-0.0075)
cameraUpDownButtons = ValueIntegrator(self.sensors.upDownButton(2, 0),
scaling=-0.005,
min=0.1,
max=0.85,
offset=0.725)
cameraLeveller = SimpleControlMediator( Scaler([currentPitch, cameraUpDownButtons], min=0.05, max=0.9 ), \
cameraTiltServo )
self.highPriorityProcesses.append(cameraLeveller)
# Grabber hand servo
grabberServo = self.controls.servo(5)
grabReleaseButtons = ValueIntegrator(self.sensors.upDownButton(1, 3),
min=0.1,
max=0.8,
scaling=0.1)
grabber = SimpleControlMediator( grabReleaseButtons, \
grabberServo )
self.medPriorityProcesses.append(grabber)
# Balance
pidL = PID(0.02, 0.001, 0.01, proportional_on_measurement=False)
pidR = PID(0.02, 0.001, 0.01, proportional_on_measurement=False)
pitchBalanceL = SimplePIDErrorValue(pidL, self.sensors.pitch())
pitchBalanceR = SimplePIDErrorValue(pidR,
self.sensors.pitch(),
scaling=-1.0)
# Motors
motorsStop = FixedValue(0.0)
motorEnable = self.sensors.button(4)
joystickForward = self.sensors.joystickAxis(1)
joystickLeftRight = self.sensors.joystickAxis(3)
motorL = SwitchingControlMediator( [ motorsStop, # Choice 0 = Stopped \
# Choice 1 = Controlled
[pitchBalanceL, Scaler(joystickForward, scaling = 0.8), Scaler(joystickLeftRight, scaling = -0.4)] # Joystick \
],
self.controls.motor(2), \
motorEnable )
self.highPriorityProcesses.append(motorL)
motorR = SwitchingControlMediator( [ motorsStop, # Choice 0 = Stopped \
# Choice 1 = Controlled
[pitchBalanceR, Scaler(joystickForward, scaling = -0.8), Scaler(joystickLeftRight, scaling = -0.4)] # Joystick \
],
self.controls.motor(1), \
motorEnable )
self.highPriorityProcesses.append(motorR)
# Set initial state of servos and motors
self.processAll(self.highPriorityProcesses)
self.processAll(self.medPriorityProcesses)
# Loop until the user clicks the close button.
done = False
running = False
while not done:
self.counter += 1
# Get current sensor state
self.sensors.process()
#
# EVENT PROCESSING STEP
#
for event in pygame.event.get(): # User did something.
if event.type == pygame.QUIT: # If user clicked close.
done = True # Flag that we are done so we exit this loop.
running = (motorEnable.getValue() > 0)
if running:
self.controls.setLightState(0, 1)
pidL.auto_mode = True
pidR.auto_mode = True
else:
self.controls.setLightState(0, 0)
# Reset balance state
pidL.setpoint = pidR.setpoint = self.sensors.pitch().getValue()
pidL.auto_mode = False
pidR.auto_mode = False
# Update everything
self.processAll(self.highPriorityProcesses)
if self.counter % 10 == 0:
self.processAll(self.medPriorityProcesses)
pygame.time.wait(10) # mS
#Calculate when one minute passed
oldEpoche = time.time()
def minutePassed():
global oldEpoche
currentEpoche = time.time()
if (currentEpoche - oldEpoche >=60):
oldEpoche = currentEpoche
return True
return False
#variables for while loop
switch1On = False
pid.auto_mode = False
lowestValue = 450
windAvg = 450
windValueArray = []
motorValueArray = []
numberOfReadings = 20
headers = {'Content-Type': 'application/json'}
url = 'https://kmet05demo.service-now.com/api/now/v1/clotho/put'
def mean(valueArray):
return float(sum(valueArray)) / max(len(valueArray),1)
while(1):
if GPIO.input(switch1) == GPIO.HIGH and switch1On == False:
def run_temperature_controller():
globals.log('info', 'Temperature Controller Started')
#Initialize Pid
pid = PID(Kp=aggressive_kp,
Ki=aggressive_ki,
Kd=aggressive_kd,
setpoint=globals.target_barrel_temp,
output_limits=(0, 100),
auto_mode=True,
proportional_on_measurement=True
) # See documentation on proportial on measurement flag
#Initialize temperature offset
temperature_offset = 0
if (simulated_mode == False):
#Initialize Fan
pwm = PWM.get_platform_pwm()
pwm.start(fan1_pin, globals.fan_speed, fan1_frequency)
#if a second fan is defined, initialize that as well
if (fan2_pin):
pwm.start(fan2_pin, globals.fan_speed, fan2_frequency)
#if lid switch pin is defined, set it up as input
if (lid_switch_pin):
gpio_platform.setup(lid_switch_pin, GPIO.IN)
#initialize thermocouples
TC1 = MAX6675.MAX6675(CLK_pin, TC1_CS_pin, DO_pin)
TC2 = MAX6675.MAX6675(CLK_pin, TC2_CS_pin, DO_pin)
TC3 = MAX6675.MAX6675(CLK_pin, TC3_CS_pin, DO_pin)
TC4 = MAX6675.MAX6675(CLK_pin, TC4_CS_pin, DO_pin)
TC5 = MAX6675.MAX6675(CLK_pin, TC5_CS_pin, DO_pin)
#Initialize Temperatures
#read sensors
TC1_temp = TC1.readTempC()
time.sleep(0.1)
TC2_temp = TC2.readTempC()
time.sleep(0.1)
TC3_temp = TC3.readTempC()
time.sleep(0.1)
TC4_temp = TC4.readTempC()
time.sleep(0.1)
TC5_temp = TC5.readTempC()
else:
#if in simulated mode, set to 20 degrees baseline
TC1_temp = 20
TC2_temp = 20
TC3_temp = 20
TC4_temp = 20
TC5_temp = 20
#and create empty pwm object
pwm = None
#set variables used in the loop
TC5_temp_last = TC5_temp
temp_weighted_avg_last = ((TC1_temp + TC2_temp + TC3_temp + TC4_temp) / 4)
#keep looping until instructed otherwise
while (globals.stop_threads == False):
#update setpoint if it differs
if (pid.setpoint != globals.target_barrel_temp):
globals.log(
'debug',
'Temperature Controller - PID Setpoint changed to: {}'.format(
globals.target_barrel_temp))
pid.setpoint = globals.target_barrel_temp
#read sensors and if in simulated mode, calculate based on fan speed
if (simulated_mode == False):
TC1_temp = TC1.readTempC()
time.sleep(0.1)
TC2_temp = TC2.readTempC()
time.sleep(0.1)
TC3_temp = TC3.readTempC()
time.sleep(0.1)
TC4_temp = TC4.readTempC()
time.sleep(0.1)
TC5_temp = TC5.readTempC()
else:
simulated_temp = simulate_temperature(TC1_temp)
TC1_temp = TC2_temp = TC3_temp = TC4_temp = simulated_temp
TC5_temp += 0.01
#Calculations
temp_weighted_avg = (
(TC1_temp + TC2_temp + TC3_temp + TC4_temp) / 4
) + globals.temperature_offset # + 45 From SmokeyTheBarrel: temperature compensation between outside and center of the barrel
temp_weighted_avg_last = (2 * temp_weighted_avg_last +
temp_weighted_avg) / 3
globals.current_barrel_temp = temp_weighted_avg_last
temperature_gap = globals.target_barrel_temp - temp_weighted_avg_last
globals.current_temp_gap = temperature_gap
TC5_temp_last = (2 * TC5_temp_last + TC5_temp) / 3
globals.current_meat_temp = TC5_temp_last
#use conservative pid profile for small temperature differences
if (temperature_gap >= -10 and temperature_gap <= 10):
set_pid_profile(pid, 'conservative')
#otherwise use the aggresive profile
else:
set_pid_profile(pid, 'aggressive')
#if lid switch pin is defined, check its state. If the lid is open, kill the fan. If the pin is not defined calculate and set the speed as required
if (lid_switch_pin):
lid_state = gpio_platform.input(lid_switch_pin)
if (lid_state == GPIO.HIGH):
set_fan_speed(pwm, 0)
globals.lid_open = True
else:
#calculate new fan speed and set the fan speed
#only do the pid calculation when the lid is closed to prevent confusing the pid
pid_output = pid(temp_weighted_avg_last)
set_fan_speed(pwm, pid_output)
globals.lid_open = False
else:
#calculate new fan speed and set the fan speed
pid_output = pid(temp_weighted_avg_last)
set_fan_speed(pwm, pid_output)
#if the calibrate option has been selected
if (globals.calibrate_temperature == True):
globals.log(
'info',
'Temperature Calibration Started, stopping PID controller')
#stop the pid
pid.auto_mode = False
#run the calibrate function
globals.temperature_offset = calibrate_temperature_offset(
TC1, TC2, TC3, TC4, TC5, pwm)
#reenable the pid
pid.auto_mode = True
globals.log(
'info',
'Temperature Calibration Finished, PID controller started')
#sleep for a second
time.sleep(1)
###### End of loop
if (simulated_mode == False):
pwm.stop(fan1_pin)
if (fan2_pin):
pwm.stop(fan2_pin)
globals.log('info', 'Temperature Controller Stopped')
xdiff = centerx - cx
if (abs(xdiff) < 50):
print('Center x - centroid x', xdiff)
GPIO.output(in1, True)
GPIO.output(in2, False)
GPIO.output(in3, False)
GPIO.output(in4, True)
pwm1.ChangeDutyCycle(25)
pwm2.ChangeDutyCycle(25)
#time.sleep(0.5)
else:
pid = PID(2.8, 0.2, 0.05, output_limits=(0, 60), setpoint=0)
pid2 = PID(2.8, 0.2, 0.05, output_limits=(0, 60), setpoint=0)
pid.auto_mode = True
pid2.auto_mode = True
output1 = pid(xdiff)
output2 = pid2((-xdiff))
if (output1 < 0):
GPIO.output(in1, False)
GPIO.output(in2, True)
if (output1 >= 0):
GPIO.output(in1, True)
GPIO.output(in2, False)
if (output2 < 0):
GPIO.output(in3, True)
GPIO.output(in4, False)
if (output2 >= 0):
GPIO.output(in3, False)
GPIO.output(in4, True)
__minimum_v = 0
__maximum_v = 0
__handle_dis = 0
__handle_ang = 0
Kp_v = 1.5
Ki_v = 0.0
Kd_v = 0.1
Cp_v = 0
Kp_w = 0.25
Ki_w = 0.0
Kd_w = 0.1
Cp_w = 0
pid_v = PID(Kp_v, Ki_v, Kd_v, setpoint=Cp_v)
pid_v.output_limits = (-1*__maximum_v, __maximum_v)
pid_v.auto_mode = True
pid_w = PID(Kp_w, Ki_w, Kd_w, setpoint=Cp_w)
pid_w.output_limits = (-1*__maximum_w, __maximum_w)
pid_w.auto_mode = True
def TuningVelocityContorller(self, p, i, d, cp = Cp_v):
self.pid_v.setpoint = cp
self.pid_v.tunings = (p, i, d)
def TuningAngularVelocityContorller(self, p, i, d, cp = Cp_w):
self.pid_w.setpoint = cp
self.pid_w.tunings = (p, i, d)
def ChangeVelocityRange(self, m, M):
self.__minimum_v = m
self.pid_v.output_limits = (-1*M, M)
def run(self):
# Get initial state
self.sensors.process()
# Set initial state of servos and motors
self.controls.stopAllMotors()
# Yaw control
yaw = self.sensors.yaw()
pidHeading = PID(0.3, 0.003, 0.05, sample_time=0.05)
target_heading = HeadingPIDErrorValue(yaw,
pidHeading,
yaw.getValue(),
min=-0.2,
max=0.2,
scaling=0.04)
# Initialise the PID
target_heading.getValue()
# Common controls
self.highPriorityProcesses.append(self.controls)
# Camera angle servo
cameraTiltServo = self.controls.servo(6)
currentPitch = Scaler(self.sensors.pitch(), scaling=-0.015)
cameraUpDownButtons = ValueIntegrator(self.sensors.upDownButton(2, 0),
scaling=-0.01,
min=-0.85,
max=0.85,
offset=0.5)
cameraLeveller = SimpleControlMediator( Scaler([currentPitch, cameraUpDownButtons], min=-0.9, max=0.85 ), \
cameraTiltServo )
self.highPriorityProcesses.append(cameraLeveller)
# Grabber hand servo
grabberServo = self.controls.servo(5)
grabReleaseButtons = ValueIntegrator(self.sensors.upDownButton(1, 3),
min=-0.8,
max=0.6,
scaling=0.2)
grabber = SimpleControlMediator( grabReleaseButtons, \
grabberServo )
self.medPriorityProcesses.append(grabber)
# Motors
motorsStop = FixedValue(0.0)
motorEnable = self.sensors.button(4)
joystickForward = self.sensors.joystickAxis(1)
joystickLeftRight = self.sensors.joystickAxis(3)
# Speed PID controller
## LEFT
speedSensorL = self.sensors.rateCounter(0) # Raw speed
speedSensorScaledL = Scaler(
speedSensorL, scaling=0.0005) # Roughly +/-1000 => +/1.0 max speed
speedRequestL = ValueAdder([
Scaler(joystickForward, scaling=0.5),
Scaler(joystickLeftRight, scaling=-0.2),
Scaler(target_heading, scaling=-1.0)
])
#pidL = PID(2.0,0.0,0.05, sample_time=0.05, output_limits=(-1.0, 1.0), proportional_on_measurement = False)
pidL = PID(0.3,
0.3,
0.05,
sample_time=0.05,
output_limits=(-1.0, 1.0),
proportional_on_measurement=True)
torqueErrorL = SimplePIDErrorValue(
pidL, Scaler(speedSensorScaledL, scaling=-1.0))
#torqueL = ValueAdder([speedRequestL, torqueErrorL])
torqueL = speedRequestL
motorL = SwitchingControlMediator( [ motorsStop, # Choice 0 = Stopped \
# Choice 1 = Controlled
[torqueL] # Speed control via PID \
],
self.controls.motor(2), \
motorEnable )
self.highPriorityProcesses.append(motorL)
## RIGHT
speedSensorR = self.sensors.rateCounter(1) # Raw speed
speedSensorScaledR = Scaler(
speedSensorR,
scaling=-0.0005) # Roughly +/-1000 => +/1.0 max speed
speedRequestR = ValueAdder([
Scaler(joystickForward, scaling=0.5),
Scaler(joystickLeftRight, scaling=0.2),
Scaler(target_heading, scaling=1.0)
])
#pidL = PID(2.0,0.0,0.05, sample_time=0.05, output_limits=(-1.0, 1.0), proportional_on_measurement = False)
pidR = PID(0.3,
0.3,
0.05,
sample_time=0.05,
output_limits=(-1.0, 1.0),
proportional_on_measurement=True)
torqueErrorR = SimplePIDErrorValue(
pidR, Scaler(speedSensorScaledR, scaling=-1.0))
#torqueR = ValueAdder([speedRequestR, torqueErrorR], scaling=-1.0)
torqueR = ValueAdder([speedRequestR], scaling=-1.0)
motorR = SwitchingControlMediator( [ motorsStop, # Choice 0 = Stopped \
# Choice 1 = Controlled
[torqueR] # Speed control via PID \
],
self.controls.motor(1), \
motorEnable )
self.highPriorityProcesses.append(motorR)
# Led Status
ledIndicator = self.controls.led(0)
# Set initial state of servos and motors
self.processAll(self.highPriorityProcesses)
self.processAll(self.medPriorityProcesses)
# Loop until the user clicks the close button.
done = False
running = False
while not done:
self.counter += 1
# Get current sensor state
self.sensors.process()
#
# EVENT PROCESSING STEP
#
for event in pygame.event.get(): # User did something.
if event.type == pygame.QUIT: # If user clicked close.
done = True # Flag that we are done so we exit this loop.
running = (motorEnable.getValue() > 0)
if running:
ledIndicator.setValue(0x04)
if not pidHeading.auto_mode:
pidHeading.auto_mode = True
targetL = speedRequestL.getValue()
print(f"targetL: {targetL}")
torqueErrorL.setTarget(targetL)
if not pidL.auto_mode:
pidL.auto_mode = True
targetR = speedRequestR.getValue()
#print(f"targetR: {targetR}")
torqueErrorR.setTarget(targetR)
if not pidR.auto_mode:
pidR.auto_mode = True
else:
ledIndicator.setValue(0x02)
torqueErrorL.setTarget(0.0)
pidL.auto_mode = False
torqueErrorR.setTarget(0.0)
pidR.auto_mode = False
target_heading.setTarget(yaw.getValue())
pidHeading.auto_mode = False
# Update everything
self.processAll(self.highPriorityProcesses)
if self.counter % 10 == 0:
self.processAll(self.medPriorityProcesses)
#speedSensorValL = speedSensorL.getCounter()
rateOfChangeL = speedSensorL.getValue()
speedL = speedSensorScaledL.getValue()
print(f"scaled speedL: {speedL}, speed: {rateOfChangeL}")
pidLcomponents = pidL.components
print(f"pidL: {pidLcomponents}")
#speedSensorValR = speedSensorR.getCounter()
#rateOfChangeR = speedSensorR.getValue()
#print(f"speedSensorValR: {speedSensorValR}, speed: {rateOfChangeR}")
#if running:
# currentTorqueL = torqueL.getValue()
# torqueLprev.setValue(currentTorqueL)
# currentTorqueR = torqueR.getValue()
# torqueRprev.setValue(currentTorqueR)
#else:
# torqueLprev.setValue(0.0)
# torqueRprev.setValue(0.0)
pygame.time.wait(10) # mS
# Keep motors alive if sensors also alive
if self.sensors.checkWatchdog() > 0:
self.controls.resetWatchdog()
else:
motorEnable.setValue(0, status=0)
temp = (res.replace("temp=", "").replace("'C\n", ""))
#print("temp is {0}".format(temp)) #Uncomment here for testing
return temp
PWM_FREQ = 25 #change this if fan stutters or else behave strange
fanPin = 21 # The pin ID, edit here to change it
GPIO.setmode(GPIO.BCM)
GPIO.setup(fanPin, GPIO.OUT)
GPIO.setwarnings(False)
myPWM = GPIO.PWM(fanPin, PWM_FREQ)
myPWM.start(40)
pid = PID(-2, -0.8, -2)
pid.setpoint = 45
output = 0
pid.auto_mode = True
# assume we have a system we want to control in controlled_system
pid.sample_time = 0.5 # update every 0.01 seconds
pid.output_limits = (0, 100) # output value will be between 0 and 10
while True:
input_temp = float(getCPUtemperature())
print(input_temp)
# compute new output from the PID according to the systems current value
output = int(pid(input_temp))
# feed the PID output to the system and get its current value
#v = getCPUtemperature().update(0)
myPWM.ChangeDutyCycle(output)
#PID
P_pitch = 1.48
I_pitch = 28.0
D_pitch = 0.05
P_roll = 0.1
I_roll = 30.0
D_roll = 0.1
pid_pitch = PID(P_pitch, I_pitch, D_pitch, setpoint=0.0)
pid_roll = PID(P_roll, I_roll, D_roll, setpoint=0.0)
pid_pitch.sample_time = 0.01
pid_roll.sample_time = pid_pitch.sample_time
pid_pitch.auto_mode = True
pid_roll.auto_mode = True
pid_pitch.output_limits = (-100.0, 100.0)
pid_roll.output_limits = (-100.0, 100.0)
##############################################################################
#Thread functions
def ReadSerial():
global mode, tilt
data = []
while 1:
data = ser.readline()
DATA = list(data)
def run(self):
# Get initial state
self.sensors.process()
#target_heading = -self.sensors.yaw().getValue()
# Set initial state of servos and motors
self.controls.stopAllMotors()
#pidLeft = PID(0.4, 0.8, 0.02, setpoint=0, sample_time=0.01, proportional_on_measurement=True)
#pidLeft.output_limits = (-1.0, 1.0)
#pidRight = PID(0.4, 0.8, 0.02, setpoint=0, sample_time=0.01, proportional_on_measurement=True)
#pidRight.output_limits = (-1.0, 1.0)
pidHeadingL = PID(0.1,
0.000,
0.003,
setpoint=0,
sample_time=0.01,
proportional_on_measurement=False,
output_limits=(-1.0, 1.0))
pidHeadingR = PID(0.1,
0.000,
0.003,
setpoint=0,
sample_time=0.01,
proportional_on_measurement=False,
output_limits=(-1.0, 1.0))
# Value generatators for speed control based on heading direction
headingErrorValueL = HeadingPIDErrorValue(self.sensors.yaw(),
pidHeadingL, 0.0)
headingErrorValueR = HeadingPIDErrorValue(self.sensors.yaw(),
pidHeadingR,
0.0,
scaling=-1.0)
# Common controls
self.highPriorityProcesses.append(self.controls)
# Camera angle servo
cameraTiltServo = self.controls.servo(6)
currentPitch = Scaler(self.sensors.pitch(), scaling=-0.0075)
cameraUpDownButtons = ValueIntegrator(self.sensors.upDownButton(2, 0),
scaling=-0.005,
min=0.1,
max=0.85,
offset=0.725)
cameraLeveller = SimpleControlMediator( Scaler([currentPitch, cameraUpDownButtons], min=0.05, max=0.9 ), \
cameraTiltServo )
self.highPriorityProcesses.append(cameraLeveller)
# Grabber hand servo
grabberServo = self.controls.servo(5)
grabReleaseButtons = ValueIntegrator(self.sensors.upDownButton(1, 3),
min=0.1,
max=0.8,
scaling=0.1)
grabber = SimpleControlMediator( grabReleaseButtons, \
grabberServo )
self.medPriorityProcesses.append(grabber)
# Motors
motorsStop = FixedValue(0.0)
motorEnable = self.sensors.button(4)
joystickForward = self.sensors.joystickAxis(1)
joystickLeftRight = self.sensors.joystickAxis(3)
motorL = SwitchingControlMediator( [ motorsStop, # Choice 0 = Stopped \
# Choice 1 = Controlled
[Scaler(joystickForward, scaling = 0.8), Scaler(joystickLeftRight, scaling = -0.5)] # Joystick \
],
self.controls.motor(2), \
motorEnable )
self.highPriorityProcesses.append(motorL)
motorR = SwitchingControlMediator( [ motorsStop, # Choice 0 = Stopped \
# Choice 1 = Controlled
[Scaler(joystickForward, scaling = -0.8), Scaler(joystickLeftRight, scaling = -0.5)] # Joystick \
],
self.controls.motor(1), \
motorEnable )
self.highPriorityProcesses.append(motorR)
# Vision
imageProcessor = LineFollowerImageProcessor()
# Set initial state of servos and motors
self.processAll(self.highPriorityProcesses)
self.processAll(self.medPriorityProcesses)
# Loop until the user clicks the close button.
done = False
running = False
while not done:
self.counter += 1
# Get current sensor state
self.sensors.process()
#
# EVENT PROCESSING STEP
#
for event in pygame.event.get(): # User did something.
if event.type == pygame.QUIT: # If user clicked close.
done = True # Flag that we are done so we exit this loop.
heading = -self.sensors.yaw().getValue()
button_rotate = rotateLeft.getValue() - rotateRight.getValue()
running = (motorEnable.getValue() > 0)
if running:
self.controls.setLightState(0, 1)
pidHeadingL.auto_mode = True
pidHeadingR.auto_mode = True
# Get the offset for the white line
#_,lineAngle = imageProcessor.captureAndAssess()
lineAngle = imageProcessor.getValue()
# clamp rotate to prevent sudden changes
if lineAngle > 10.0:
lineAngle = 10.0
elif lineAngle < -10.0:
lineAngle = -10.0
target_heading = self.normaliseHeading(heading - lineAngle)
print(
f"White line indicate rotate from {heading} to {target_heading}"
)
target_heading = self.normaliseHeading(target_heading +
button_rotate)
headingErrorValueL.setTarget(target_heading)
headingErrorValueR.setTarget(target_heading)
else:
self.controls.setLightState(0, 0)
# Just idle to a standstill
pidHeadingL.auto_mode = False
pidHeadingR.auto_mode = False
#target_heading = heading # So doesn't spin when button is released
headingErrorValueL.setTarget(heading)
headingErrorValueR.setTarget(heading)
#print("L: {0:6.3f} => {2:6.3f}, R: {1:6.3f} => {3:6.3f}".format(leftThrottle, rightThrottle, pidLeft.setpoint, pidRight.setpoint ))
# Update everything
self.processAll(self.highPriorityProcesses)
if self.counter % 10 == 0:
self.processAll(self.medPriorityProcesses)
#imageProcessor.captureAndDisplay()
#time.sleep(0.1)
pygame.time.wait(10) # mS
def main():
drone = tellopy.Tello()
tracker = Tracker()
control_y = PID(-0.08, -0.007, -0.003, setpoint=0)
control_z = PID(-0.15, -0.01, -0.005, setpoint=0)
control_yaw = PID(-0.08, -0.007, -0.003, setpoint=0)
control_y.sample_time = 1 / 60
control_z.sample_time = 1 / 60
control_yaw.sample_time = 1 / 60
control_y.output_limits = (-MAX_SPEED, MAX_SPEED)
control_z.output_limits = (-MAX_SPEED, MAX_SPEED)
control_yaw.output_limits = (-MAX_SPEED, MAX_SPEED)
reticle = calc_gluideslope(-5)
try:
drone.subscribe(drone.EVENT_FLIGHT_DATA, flight_data_handler)
drone.subscribe(drone.EVENT_LOG_DATA, flight_data_handler)
drone.connect()
drone.wait_for_connection(60.0)
retry = 3
container = None
while container is None and 0 < retry:
retry -= 1
try:
container = av.open(drone.get_video_stream())
except av.AVError as ave:
print(ave)
print('retry...')
# Start without the autopilot
autopilot_on = False
control_y.auto_mode = False
control_z.auto_mode = False
control_yaw.auto_mode = False
frame_skip = 300 # Skip first frames
while True:
for frame in container.decode(video=0):
# If transforms are taking too long then start skipping frames
if frame_skip > 0:
frame_skip -= 1
continue
start_time = time.time()
image = cv2.cvtColor(np.array(frame.to_image()),
cv2.COLOR_RGB2BGR)
# Key presses give the drone a speed, and not a distance to move. Press x to stop all movement
key = cv2.waitKey(1) & 0xFF
fly_with_keyboard(drone, key)
# Toggle autopilot
if key == ord('p'):
autopilot_on = not autopilot_on
if autopilot_on:
#control_y.auto_mode = True
control_z.auto_mode = True
control_yaw.auto_mode = True
else:
#control_y.auto_mode = False
control_z.auto_mode = False
control_yaw.auto_mode = False
tracker.update(image)
image = tracker.draw_markers(image)
image = draw_reticle(image, reticle)
error_yaw, error_z, error_y = tracker.calc_error(2, reticle)
#print('Errors:', error_yaw, error_z, error_y)
v_y = control_y(error_y)
v_z = control_z(error_z)
v_yaw = control_yaw(error_yaw)
#print('error y', error_y, 'v_y', v_y, 'PID', control_y.components)
#print('error z', error_z, 'v_z', v_z, 'PID', control_z.components)
#print('error yaw', error_y, 'v_yaw', v_yaw, 'PID', control_yaw.components)
#print(log_data.imu)
#print(type(log_data.imu))
imu = log_data.imu
#print(quat2euler(imu.q0, imu.q1, imu.q2, imu.q3)) # roll, pitch, yaw
tracker.draw_axes(image)
try:
if tracker.distances[0] < 1000:
control_y.auto_mode = True
else:
control_y.auto_mode = False
except KeyError:
pass
if autopilot_on:
if v_z is None:
pass
elif v_z > 0:
drone.up(v_z)
else:
drone.down(abs(v_z))
if v_y is None:
pass
elif v_y > 0:
drone.right(v_y)
else:
drone.left(abs(v_y))
if v_yaw is None:
pass
elif v_yaw > 0:
drone.clockwise(v_yaw)
else:
drone.counter_clockwise(abs(v_yaw))
# Display an image with edge detection. Make smaller so can fit on screen with the HUD
#img = cv2.resize(image, (300, 225))
#cv2.imshow('Canny', cv2.Canny(img, 100, 200))
# Display full image with HUD
image = draw_hud(image, autopilot_on)
cv2.imshow('Drone', image)
# Determine if need to skip any frames
if frame.time_base < 1.0 / 60:
time_base = 1.0 / 60
else:
time_base = frame.time_base
frame_skip = int((time.time() - start_time) / time_base)
except Exception as ex:
exc_type, exc_value, exc_traceback = sys.exc_info()
traceback.print_exception(exc_type, exc_value, exc_traceback)
print(ex)
finally:
drone.quit()
cv2.destroyAllWindows()
joystick = pygame.joystick.Joystick(0)
joystick.init()
#
# EVENT PROCESSING STEP
#
# Possible joystick actions: JOYAXISMOTION, JOYBALLMOTION, JOYBUTTONDOWN,
# JOYBUTTONUP, JOYHATMOTION
for event in pygame.event.get(): # User did something.
print(event.type)
if event.type == pygame.QUIT: # If user clicked close.
done = True # Flag that we are done so we exit this loop.
elif not running and event.type == pygame.JOYBUTTONDOWN and (
joystick.get_button(4) == 1 or joystick.get_button(5) == 1):
running = True
pidHeading.auto_mode = True
print("Motor Joystick enabled...")
elif running and event.type == pygame.JOYBUTTONDOWN and joystick.get_button(
15) == 1:
button_rotate = -22.5
print("Rotate -22.5 deg...")
elif running and event.type == pygame.JOYBUTTONDOWN and joystick.get_button(
16) == 1:
button_rotate = 22.5
print("Rotate +22.5 deg...")
elif running and event.type == pygame.JOYBUTTONUP and joystick.get_button(
4) == 0 and joystick.get_button(5) == 0:
running = False
pidHeading.auto_mode = False
print("Motors off.")