def __init__(self, frequency):
self.FEEDBACK_TIMEOUT = 2.0
self.wrench_input = WrenchStamped()
self.setpoint_valid = False
self.feedback_received = False
self.enabled = False
self.last_feedback = Range()
self.enable_server = rospy.Service('~enable',
auv_control_msgs.srv.EnableControl,
self.enable)
self.pid = Pid(0.0, 0.0, 0.0) # the right constants will
self.k_f = 0.0 # be set through dynamic reconfigure
self.server = dynamic_reconfigure.server.Server(
AltitudeControllerConfig, self.reconfigure)
self.pub = rospy.Publisher('~wrench_output', WrenchStamped)
rospy.Subscriber('~wrench_input', WrenchStamped,
self.wrenchInputCallback)
rospy.Subscriber('~altitude_request', Float32, self.setpointCallback)
period = rospy.rostime.Duration.from_sec(1.0 / frequency)
self.timer = rospy.Timer(period, self.updateOutput)
rospy.Subscriber('altitude', Range, self.altitudeCallback)
rospy.loginfo(
'Listening for altitude feedback to be published on '
'%s...', rospy.resolve_name('altitude'))
rospy.loginfo('Waiting for setpoint to be published on '
'%s...', rospy.resolve_name('~altitude_request'))
def start(self, Rover, target_deg, spin):
Tip.target_deg = target_deg
Tip.total_deg = abs(Rover.yaw - target_deg)
Tip.spin = spin
Tip.started = True
Tip.did_start_brake = False
Tip.start_yaw = Rover.yaw
Tip.Pid = Pid(Tip.kp, Tip.ki, Tip.kd, target_deg)
Rover.throttle = 0
print("Set rover steer to %d" % Rover.steer)
def start(self, Rover, target_pt):
Goto.target_pt = target_pt
Goto.start_pt = Rover.pos
Goto.total_dist = euclidean_heuristic(Goto.start_pt, Goto.target_pt)
Goto.last_dist = 0
Goto.started = True
Goto.quit_counter = 0
Goto.do_finishing_brake = False
Goto.Pid = Pid(Goto.kp, Goto.ki, Goto.kd, 0.0)
Rover.steer = 0
def __init__(self):
super().__init__()
self.running = True
self.target_lat = 39
self.target_lon = 70
self.target_yaw = 0
self.target_alt = 0
self.pid_left = Pid(0.1, 0, 1.0, max_integral=1)
self.pid_forward = Pid(0.1, 0, 1.0, max_integral=1)
self.pid_yaw = Pid(0.01, 0.005, 0, max_integral=10)
self.pid_up = Pid(0.001, 0.0005, 0.01, max_integral=1)
self.prev_yaw_error = 0
self.aux_switch_was_flipped = False
self.last_debug_print = time.time()
self.last_gps_update = 0
self.enabled = False
def test_pid(mass=3,
velocity=1,
start_point=2,
iterations=2000,
time_step=0.01,
wind_force=0.1,
max_force=5):
pid = Pid(1.5, 0.5, 0.5)
current_position = start_point
times = []
positions = []
winds = []
wind_f = [(random.random(), random.random()) for _ in range(0, 10)]
def wind(time_step):
result = 0
for x, y in wind_f:
result += wind_force * (x - 0.5) * math.sin(y * 0.5 * time_step)
return result
for i in range(0, iterations):
force = min(
max(pid.update(current_position, delta_time=time_step),
-max_force), max_force)
force += wind(i * time_step)
acceleration = force / mass
velocity += acceleration
current_position += velocity * time_step
times.append(i * time_step)
positions.append(current_position)
winds.append(wind(i * time_step))
pyplot.plot(times, positions)
pyplot.plot(times, [0] * len(times), linestyle="--")
pyplot.plot(times, winds, linestyle=":")
pyplot.show()
def __init__(self):
# Velocity PID object setup
self.vel_pid_output_a = VelocityPidOutput()
self.vel_pid_output_b = VelocityPidOutput()
self.vel_pid_output_c = VelocityPidOutput()
self.vel_pid_a = Pid(self.vel_pid_output_a, Robot.VEL_P, Robot.VEL_I, Robot.VEL_D, Robot.VEL_F)
self.vel_pid_b = Pid(self.vel_pid_output_b, Robot.VEL_P, Robot.VEL_I, Robot.VEL_D, Robot.VEL_F)
self.vel_pid_c = Pid(self.vel_pid_output_c, Robot.VEL_P, Robot.VEL_I, Robot.VEL_D, Robot.VEL_F)
# Qep encoder object setup
self.qep_a = Qep(Robot.MOTOR_A_QEP)
self.qep_b = Qep(Robot.MOTOR_B_QEP)
self.qep_c = Qep(Robot.MOTOR_C_QEP)
# Pwm object setup
self.pwm_a = Pwm(Robot.MOTOR_A_PWM)
self.pwm_b = Pwm(Robot.MOTOR_B_PWM)
self.pwm_c = Pwm(Robot.MOTOR_C_PWM)
self.pwms = [self.pwm_a, self.pwm_b, self.pwm_c]
# Create MotorController objects
self.motor_a = MotorController(self.pwm_a, self.vel_pid_a, self.vel_pid_output_a, self.qep_a, self.VEL_PID_ENABLED)
self.motor_b = MotorController(self.pwm_b, self.vel_pid_b, self.vel_pid_output_b, self.qep_b, self.VEL_PID_ENABLED)
self.motor_c = MotorController(self.pwm_c, self.vel_pid_c, self.vel_pid_output_c, self.qep_c, self.VEL_PID_ENABLED)
self.motors = [self.motor_a, self.motor_b, self.motor_c]
# set up pid
self.yaw_pid_output = YawPidOutput()
self.yaw_pid = Pid(self.yaw_pid_output, Robot.YAW_P, Robot.YAW_I, Robot.YAW_D, izone=1.0)
# initialise mpu/imu server module
self.mpu = Mpu()
self.yaw_pid_thread = YawPidThread(self.yaw_pid, self.mpu)
self.current_command = Robot.INIT_COMMAND
# pid *enabled* by default
self.yaw_pid_enabled = False
# pid not in *control* by default, this is automatically set by drive
self.pid_in_control = False
self.field_centered = True
self.enabled = False
self.interrupted = False
self.last_input_time = time.time()
def __init__(self, frequency):
self.FEEDBACK_TIMEOUT = 1.0
self.setpoint_valid = False
self.feedback_received = False
self.enabled = False
self.last_feedback = Odometry()
self.enable_server = rospy.Service('~enable', auv_control_msgs.srv.EnableControl, self.enable)
self.pids = []
for i in range(6):
self.pids.append(Pid(0.0, 0.0, 0.0))
self.server = dynamic_reconfigure.server.Server(TwistControllerConfig, self.reconfigure)
self.pub = rospy.Publisher('~wrench_output', WrenchStamped)
rospy.Subscriber('~twist_request', TwistStamped, self.setpointCallback)
period = rospy.rostime.Duration.from_sec(1.0/frequency)
self.timer = rospy.Timer(period, self.updateOutput)
rospy.Subscriber('odometry', Odometry, self.odometryCallback)
rospy.loginfo('Listening for altitude feedback to be published on '
'%s...', rospy.resolve_name('altitude'))
rospy.loginfo('Waiting for setpoint to be published on '
'%s...', rospy.resolve_name('~twist_request'))
def __init__(self):
self.state = defState
self.pos = position(0, 0, 0, False, False, False)
self.output = {
"roll": midOutput,
"pitch": midOutput,
"yaw": midOutput,
"throttle": minOutput
}
self.speeds = {
"roll": defSpeed,
"pitch": defSpeed,
"yaw": defSpeed,
"throttle": defSpeed
}
self.xPid = Pid(pidDefKp, pidDefKi, pidDefKd, 0, pidMinOut, pidMaxOut)
self.yPid = Pid(pidDefKp, pidDefKi, pidDefKd, 0, pidMinOut, pidMaxOut)
self.zPid = Pid(pidDefKp, pidDefKi, pidZKd, 0, pidMinOut, pidMaxOut)
self.setPoints = {"x": 0, "y": 0, "z": 0}
class fControl:
def __init__(self):
self.state = defState
self.pos = position(0, 0, 0, False, False, False)
self.output = {
"roll": midOutput,
"pitch": midOutput,
"yaw": midOutput,
"throttle": minOutput
}
self.speeds = {
"roll": defSpeed,
"pitch": defSpeed,
"yaw": defSpeed,
"throttle": defSpeed
}
self.xPid = Pid(pidDefKp, pidDefKi, pidDefKd, 0, pidMinOut, pidMaxOut)
self.yPid = Pid(pidDefKp, pidDefKi, pidDefKd, 0, pidMinOut, pidMaxOut)
self.zPid = Pid(pidDefKp, pidDefKi, pidZKd, 0, pidMinOut, pidMaxOut)
self.setPoints = {"x": 0, "y": 0, "z": 0}
def tune(self, controller, P, I, D):
if controller == "x":
self.xPid.tune(P, I, D)
elif controller == "y":
self.yPid.tune(P, I, D)
elif controller == "z":
self.zPid.tune(P, I, D)
def setSpeed(self, direction, value):
self.speeds[direction] = value
def startHover(self, pos):
self.setPoints["x"] = pos.x
self.setPoints["y"] = pos.y
self.setPoints["z"] = pos.z
self.state = "hover"
def update(self, state, pos):
self.state = state
self.pos = pos
if self.state == "landed":
return
# Put setpoints over/under current to move in z direction
if self.state == "moveUP":
self.setPoints["z"] = pos.z - self.speeds["throttle"]
elif self.state == "moveDown":
self.setPoints["z"] = pos.z + self.speeds["throttle"]
#Feed forward control if position info is invalid
if pos.zValid == False:
if state == "moveUp":
self.output["throttle"] += throttleIncrement
elif state == "moveDown":
self.output["throttle"] -= throttleIncrement
#run altitude controller
elif pos.zValid == True:
self.zPid.set(self.setPoints["z"])
self.zPid.step(pidDt, pos.z)
self.output["throttle"] -= self.zPid.get(
) #negative because of NED
# Run X/Y-controllers in hover mode
if self.state == "hover":
if pos.xValid:
self.xPid.set(self.setPoints["x"])
self.xPid.step(pidDt, pos.x)
self.output["pitch"] = midOutput - self.xPid.get()
#negative because low elevator is forward
else:
self.output["pitch"] = midOutput
if pos.yValid:
self.yPid.set(self.setPoints["y"])
self.yPid.step(pidDt, pos.y)
self.output["roll"] = midOutput + self.yPid.get()
else:
self.output["roll"] = midOutput
return
# Moving states. Feed forward attitude setpoints to FC
if self.state == "turnLeft":
self.output["yaw"] = midOutput + self.speeds["yaw"]
elif self.state == "turnRight":
self.output["yaw"] = midOutput - self.speeds["yaw"]
elif self.state == "moveRight":
self.output["roll"] = midOutput + self.speeds["roll"]
elif self.state == "moveLeft":
self.output["roll"] = midOutput - self.speeds["roll"]
elif self.state == "moveForward":
self.output["pitch"] = midOutput - self.speeds["pitch"]
elif self.state == "moveBackward":
self.output["pitch"] = midOutput + self.speeds["pitch"]
def create_components(mqtt_broker):
# Get settings
availability_topic = env_bool('HA_AVAILABLE', False)
auto_discovery = env_bool('HA_AUTO', False)
# Setup registry
reg = ComponentRegistry()
state = MqttSharedTopic(mqtt_broker,
id_from_name("/{}".format(with_prefix('state'))))
reg.add_shared_topic(state)
# Setup standard config
standard_config = {
'mqtt': mqtt_broker,
'state_topic': state,
'availability_topic': availability_topic,
'auto_discovery': auto_discovery
}
# Create error sensor
errors = ErrorSensor(with_prefix('Errors'), **standard_config)
reg.add_component(errors)
# Create last update sensor
last_update = create_last_update_sensor(with_prefix('Last Update'),
**standard_config)
reg.add_component(last_update)
# Setup temperature sensors
func_limit = create_func_call_limiter()
ha_sensors = []
for s in create_temp_sensors():
logging.info("Found temp sensor: {}".format(s.get_id()))
# Wrap state function to catch errors
ha_sensors.append(
Sensor(with_prefix('Temp ' + env_name(s.get_id())),
'°C',
state_func=wrap_sensor(errors, func_limit, s),
**standard_config))
reg.add_component(ha_sensors)
# Setup temp sensor counter
reg.add_component(
Sensor(with_prefix('Temp Sensor Count'),
'',
state_func=lambda: len(ha_sensors),
state_formatter_func=lambda x: x,
icon='mdi:magnify-plus',
**standard_config))
# Create average temperature
avg = create_average_sensor(with_prefix('Temp Average'),
'°C',
ha_sensors,
icon='mdi:thermometer-lines',
**standard_config)
reg.add_component(avg)
# Create weighted average temperature
avg_w, weights = create_weighted_average_sensor(
with_prefix('Temp Average Weighted'),
'°C',
0,
100,
1,
50,
ha_sensors,
icon='mdi:thermometer-lines',
**standard_config)
reg.add_component(avg_w)
reg.add_component(weights, send_updates=False)
# Create SSR
ssr = Switch(with_prefix('SSR'),
state_change_func=create_ssr(errors),
**standard_config)
reg.add_component(ssr)
# Create PID sensor
pid_sensor = PidSensor()
s_p = Sensor(with_prefix('P'),
' ',
sensor_id=id_from_name(with_prefix('pid_p')),
state_func=lambda: pid_sensor.p,
**standard_config)
s_i = Sensor(with_prefix('I'),
' ',
sensor_id=id_from_name(with_prefix('pid_i')),
state_func=lambda: pid_sensor.i,
**standard_config)
s_d = Sensor(with_prefix('D'),
' ',
sensor_id=id_from_name(with_prefix('pid_d')),
state_func=lambda: pid_sensor.d,
**standard_config)
s_o = Sensor(with_prefix('Percent On'),
'%',
icon='mdi:power-socket-eu',
state_func=lambda: float(pid_sensor.control_percent),
**standard_config)
reg.add_component([s_p, s_i, s_d, s_o])
# Create PID controller
s_def = {
'mqtt': mqtt_broker,
'availability_topic': availability_topic,
'auto_discovery': auto_discovery,
'icon': 'mdi:cursor-move'
}
pid_controller = Pid(avg_w, ssr, pid_sensor)
reg.add_component(SettableSensor('P',
'',
0,
10,
0.1,
pid_controller.p,
lambda v: pid_controller.set_p(v),
sensor_id=id_from_name(
with_prefix('pid_p_gain')),
**s_def),
send_updates=False)
reg.add_component(SettableSensor('I',
'',
0,
10,
0.1,
pid_controller.i,
lambda v: pid_controller.set_i(v),
sensor_id=id_from_name(
with_prefix('pid_i_gain')),
**s_def),
send_updates=False)
reg.add_component(SettableSensor('D',
'',
0,
10,
0.1,
pid_controller.d,
lambda v: pid_controller.set_d(v),
sensor_id=id_from_name(
with_prefix('pid_d_gain')),
**s_def),
send_updates=False)
reg.add_component(SettableSensor(
'Out Limit',
'',
0,
10,
1,
pid_controller.output_limit,
lambda v: pid_controller.set_output_limit(v),
sensor_id=id_from_name(with_prefix('pid_output_limit')),
**s_def),
send_updates=False)
reg.add_component(SettableSensor(
'Time',
'',
0,
10,
1,
pid_controller.sample_time,
lambda v: pid_controller.set_sample_time(v),
sensor_id=id_from_name(with_prefix('pid_sample_time')),
**s_def),
send_updates=False)
# Create Time On Target sensor
tat = TimeOnTarget(pid_controller)
reg.add_component(
Sensor(with_prefix('Time On Target'),
'',
state_func=errors.wrap_function(tat),
icon='mdi:clock',
device_class='timestamp',
state_formatter_func=lambda s: s,
**standard_config))
# Create HA climate
climate = Climate(
with_prefix(),
avg_w,
state_change_func=lambda mode, target: pid.state_change(mode, target),
mqtt=mqtt_broker,
availability_topic=availability_topic,
auto_discovery=auto_discovery,
# state_topic=state
)
reg.add_component(climate)
if not SIMULATE:
# RPI temp sensor
reg.add_component(
Sensor(with_prefix('Temp CPU'),
'°C',
state_func=cpu_temp,
**standard_config))
cfg = reg.create_config()
file = '/tmp/ha-cfg.yml'
logging.info("Writing HA config to file: {}".format(file))
with open(file, 'w') as f:
f.write(cfg)
if env_bool('HA_PRINT_CONFIG', False):
logging.info("HA config:\n{}".format(cfg))
return func_limit, pid_controller, reg, climate, errors
import cv2
import sys
from gui import render_crosshairs
from pid import Pid, print_graph
from red_blob_detection import RedBlobDetector
# Set to false if you don't want to show any windows
showGUI = True
controller = Pid(kp=0.2, ki=0.05, kd=0.2)
vision_algorithm = RedBlobDetector()
def move_camera(vehicle, pwm):
if vehicle:
pwm = max(pwm,
1) # Ensure we never ask for negative or zero pwm values
msg = vehicle.message_factory.rc_channels_override_encode(
1, 1, 0, 0, 0, 0, 0, pwm, 0, 0)
vehicle.send_mavlink(msg)
vehicle.flush()
def disable_camera(vehicle):
if vehicle:
msg = vehicle.message_factory.rc_channels_override_encode(
1, 1, 0, 0, 0, 0, 0, 0, 0, 0)
vehicle.send_mavlink(msg)
vehicle.flush()
def main():
rospy.init_node('pid_controller', anonymous=True)
long_control = Pid()
# lat_control.angle_pred()
rospy.spin()
class AltitudeControllerNode():
"""
Node for calculating the wrench needed in order to keep the altitude to the
seabed constant. As only one DOF -namely z- is controlled, the
other values have to be given by some other input, e.g. via joystick.
This node subscribes to 'altitude', expecting a sensor_msgs/Range message
and to 'wrench_input' which should be of type geometry_msgs/WrenchStamped.
The node also receives std_msgs/Float32 messages on the topic
'altitude_request' to define the required altitude (setpoint).
The node then calculates the force in z needed to control the AUV to
have the desired altitude, replaces this value within wrench_input and
publishes the result to 'wrench_output'.
Internally a simple PID is used for control, its variables can be modified
using dynamic_reconfigure.
A service is provided to enable and disable the altitude control, by default
it is disabled. Enabling sets the setpoint to the current altitude.
"""
def __init__(self, frequency):
self.FEEDBACK_TIMEOUT = 2.0
self.wrench_input = WrenchStamped()
self.setpoint_valid = False
self.feedback_received = False
self.enabled = False
self.last_feedback = Range()
self.enable_server = rospy.Service('~enable',
auv_control_msgs.srv.EnableControl,
self.enable)
self.pid = Pid(0.0, 0.0, 0.0) # the right constants will
self.k_f = 0.0 # be set through dynamic reconfigure
self.server = dynamic_reconfigure.server.Server(
AltitudeControllerConfig, self.reconfigure)
self.pub = rospy.Publisher('~wrench_output', WrenchStamped)
rospy.Subscriber('~wrench_input', WrenchStamped,
self.wrenchInputCallback)
rospy.Subscriber('~altitude_request', Float32, self.setpointCallback)
period = rospy.rostime.Duration.from_sec(1.0 / frequency)
self.timer = rospy.Timer(period, self.updateOutput)
rospy.Subscriber('altitude', Range, self.altitudeCallback)
rospy.loginfo(
'Listening for altitude feedback to be published on '
'%s...', rospy.resolve_name('altitude'))
rospy.loginfo('Waiting for setpoint to be published on '
'%s...', rospy.resolve_name('~altitude_request'))
def enable(self, request):
"""
Handles ROS service requests for enabling/disabling control.
Returns current enabled status and setpoint.
"""
response = auv_control_msgs.srv.EnableControlResponse()
if request.enable:
if self.isFeedbackValid():
self.enabled = True
self.pid.setSetpoint(self.last_feedback.range)
self.setpoint_valid = True
response.enabled = True
else:
rospy.logerr(
"Cannot enable altitude control without valid feedback!")
response.enabled = False
else:
self.enabled = False
response.enabled = False
response.current_setpoint = self.pid.getSetpoint()
return response
def reconfigure(self, config, level):
"""
Handles dynamic reconfigure requests.
"""
rospy.loginfo("Reconfigure request...")
self.pid.k_p = config['Kp']
self.pid.k_i = config['Ki']
self.pid.k_d = config['Kd']
self.k_f = config['Kf']
rospy.loginfo("Reconfigured to (Kp, Ki, Kd, Kf) = (%f, %f, %f, %f)",
self.pid.k_p, self.pid.k_i, self.pid.k_d, self.k_f)
return config # Returns the updated configuration.
def setpointCallback(self, setpoint):
"""
Change the setpoint of the controller.
"""
if not self.setpoint_valid:
rospy.loginfo("First setpoint received.")
self.setpoint_valid = True
rospy.loginfo('Changed setpoint to: %s', setpoint.data)
self.pid.setSetpoint(setpoint.data)
def altitudeCallback(self, range_msg):
self.last_feedback = range_msg
def wrenchInputCallback(self, wrench):
self.wrench_input = wrench
def isFeedbackValid(self):
feedback_in_time = (rospy.Time.now() - self.last_feedback.header.stamp
).to_sec() < self.FEEDBACK_TIMEOUT
return feedback_in_time and (self.last_feedback.range > 0)
def updateOutput(self, event):
if self.setpoint_valid and self.enabled:
wrench_output = copy.deepcopy(self.wrench_input)
if self.isFeedbackValid():
dt = (event.current_real - event.last_real).to_sec()
wrench_output.wrench.force.z = -self.pid.update(
self.last_feedback.range, dt) + self.k_f
if wrench_output.wrench.force.z > 1.0:
wrench_output.wrench.force.z = 1.0
if wrench_output.wrench.force.z < -1.0:
wrench_output.wrench.force.z = -1.0
else:
rospy.logwarn(
"Altitude feedback is invalid, setting force in z to zero."
)
wrench_output.wrench.force.z = 0.0
wrench_output.header.stamp = rospy.Time.now()
self.pub.publish(wrench_output)
logging.basicConfig(filename=OB.LOG_BASE_DIR + OB.LOG_FILE_NAME, format='%(asctime)s %(levelname)s %(message)s', level=logging.DEBUG)
logging.info('%s', 'Start!')
# Create instances.
TANK = TempSensor(S, OB.TANK_TEMP_SENSOR_ID)
BOILER = TempSensor(S, OB.BOILER_TEMP_SENSOR_ID)
# FALLBACK_TEMP = TempSensor(S, "286B075005000099")
FIRE = FireSensor(S, OB.FIRE_TEMP_SENSOR_ANALOG_PIN)
FAN = AnalogOut(S, OB.FAN_CONTACTOR_PIN, OB.FAN_ANALOG_PIN)
SOND = SerialLambda(S, OB.LAMBDA_SENSOR_CONTACTOR_PIN, OB.LAMBDA_SENSOR_TYPE, OB.LAMBDA_SENSOR_PORT, OB.LAMBDA_SENSOR_BAUD, int(OB.LAMBDA_SENSOR_SYNC_HEADER_ATTEMPT))
SCREW = Screw(S, OB.SCREW_CONTACTOR_PIN)
HMI = Hmi(S, OB.BUTTON_CONTACTOR_PIN)
# PID
p=Pid(10.0,0.0,0)
p.setPoint(30)
HMI.activate_contactor() # Activate HMI buttons
t1 = threading.Thread(target=startWebbServer, args=(q,))
t1.start() # START WEBBSERVER IN NEW THRED
logging.info('%s', "Enter main loop")
while True:
q.put(TANK.get_temp(), BOILER.get_temp(), FIRE.get_temp(), int(FAN.get_rpm()), S.input_get_value(OB.BUTTON_AUTO_MAN), TIME_LEFT_OF_BLOCK_TIMER)# UPPDATE VALUE FOR GUI
if not TIMESTAMP_CFG == time.ctime(os.path.getmtime(CFG)): # RELOAD CONFIG IF TIMESTAMP CHANGED
TIMESTAMP_CFG = time.ctime(os.path.getmtime(CFG))
OB.readConfigFile()
logging.info('%s', "Reload config file")
print ("grab_title:", sys.exc_info()[0])
return ""
def rss_send(self, to):
logging.info('rss_send')
rss = rssPull(['http://rss.orf.at/news.xml', 'http://heroicdebugging.biz/feeds/all.atom.xml']) #should be constructed from configparser
rss_items = rss.getNewItems()
for item in rss_items:
msg_string = 'Title: {0}\nLink: {1}'.format(item['title'],item['link'])
self.send_message(mto=to, mbody=msg_string, mtype='chat')
if __name__ == '__main__':
logging.basicConfig(level=logging.DEBUG, format='%(levelname)-8s %(message)s')
pidinstance = Pid('./.kuhbot_pid')
if(pidinstance.read()==1):
sys.exit(1)
if(pidinstance.write()==1):
sys.exit(1)
#configparser
try:
config = configparser.ConfigParser()
config_file = 'kuhbot_config.txt'
config['LOGIN'] = { 'jid' : '',
'password' : '',
'nick' : '',
}
config['ROOMS'] = {}
class __AutonomyThread(threading.Thread):
def __init__(self):
super().__init__()
self.running = True
self.target_lat = 39
self.target_lon = 70
self.target_yaw = 0
self.target_alt = 0
self.pid_left = Pid(0.1, 0, 1.0, max_integral=1)
self.pid_forward = Pid(0.1, 0, 1.0, max_integral=1)
self.pid_yaw = Pid(0.01, 0.005, 0, max_integral=10)
self.pid_up = Pid(0.001, 0.0005, 0.01, max_integral=1)
self.prev_yaw_error = 0
self.aux_switch_was_flipped = False
self.last_debug_print = time.time()
self.last_gps_update = 0
self.enabled = False
def calc_error(self):
"""
Calculates the distance the drone is from the current target latitude and longitude,
in meters, in each direction.
:return: (left_error, forward_error): left_error is how far the drone is, to the left, from the target.
forward_error is how far the drone is, in the forward direction, from the target.
Either error can be negative or positive.
"""
# TODO: Uncomment the real stuff and comment out the fake values
error_magnitude = gps.findDistanceBetweenLatLon(self.target_lat, self.target_lon)
error_angle = gps.findCurrentBearing(self.target_lat, self.target_lon)
bearing = uavcontrol.get_compass_sensor()
left_error = error_magnitude * math.sin(math.radians(bearing - error_angle))
forward_error = error_magnitude * math.cos(math.radians(bearing - error_angle))
return -left_error, -forward_error
def reset_target(self, lat, lon, yaw, alt):
self.target_lat = lat
self.target_lon = lon
self.target_yaw = yaw
self.target_alt = alt
self.pid_forward.reset()
self.pid_left.reset()
self.pid_yaw.reset()
self.pid_up.reset()
def run(self):
time.sleep(1)
self.reset_target(gps.latitude, gps.longitude, uavcontrol.get_compass_sensor(average=True, continuous=True),
gps.altitude)
while self.running:
if not self.enabled:
time.sleep(UAV_CONTROL_UPDATE_PERIOD)
continue
if uavcontrol.get_aux_input() and not self.aux_switch_was_flipped:
self.aux_switch_was_flipped = True
print("AUX SWITCH FLIPPED")
self.reset_target(gps.latitude, gps.longitude,
uavcontrol.get_compass_sensor(average=True, continuous=True))
elif not uavcontrol.get_aux_input():
self.aux_switch_was_flipped = False
# Set throttle manually from remote control
# throttle = uavcontrol.get_throttle_input()
# uavcontrol.set_throttle(throttle)
# print("set throttle to {}".format(throttle))
yaw_error = uavcontrol.get_compass_sensor(average=True, continuous=True) - self.target_yaw
yaw_correction = self.pid_yaw.update(yaw_error)
yaw_correction = constrain(yaw_correction, min_=-0.8, max_=0.8)
# Positive yaw is right, so negate it to make it left
uavcontrol.set_yaw_signal(-yaw_correction)
# uavcontrol.set_yaw_signal(uavcontrol.get_yaw_input())
if time.time() - self.last_gps_update >= 1:
self.last_gps_update = time.time()
gps.updated = False
left_error, forward_error = self.calc_error()
up_error = gps.altitude - self.target_alt
if math.sqrt(left_error ** 2 + forward_error ** 2) > config.ORIENTATION_ERROR_MARGIN:
self.target_yaw = gps.findCurrentBearing(self.target_lat, self.target_lon)
left_correction = self.pid_left.update(left_error)
forward_correction = self.pid_forward.update(forward_error)
up_correction = self.pid_up.update(up_error)
left_correction = constrain(left_correction, min_=-0.8, max_=0.8)
forward_correction = constrain(forward_correction, min_=-0.8, max_=0.8)
up_correction += (config.DRONE_MASS * 9.81) / \
(config.MAX_THRUST * math.cos(math.radians(left_correction * config.MAX_ROLL)) *
math.cos(math.radians(forward_correction * config.MAX_PITCH)))
up_correction = constrain(up_correction, min_=0, max_=1.0)
# Positive pitch is forward, so this is all good
uavcontrol.set_pitch(forward_correction)
# Positive roll is right, so negate it to make it left
uavcontrol.set_roll(-left_correction)
uavcontrol.set_throttle(up_correction)
print("{:5.1f} Err: (L: {:7.3f}, F: {:7.3f}, Y: {:7.3f}, U: {:7.3f}), "
"PID: (L: {:7.3f}, F: {:7.3f}, Y: {:7.3f}, U: {:7.3f})".format(
time.time() % 100, left_error, forward_error, yaw_error, up_error, left_correction,
forward_correction, yaw_correction, up_correction)
)
# uavcontrol.set_pitch(uavcontrol.get_pitch_input())
# uavcontrol.set_roll(uavcontrol.get_roll_input())
time.sleep(UAV_CONTROL_UPDATE_PERIOD)
def test_pid_update(self):
#baseline test
output = PidOutputTest()
p = Pid(output, 0.0)
p.set_set_point(0.0)
p.update(0)
self.assertEqual(output.correction, 0)
p.update(100)
self.assertEqual(output.correction, 0)
#test kp is working
p = Pid(output, 1.0)
p.update(0)
self.assertEqual(output.correction, 0)
p.update(100)
self.assertEqual(output.correction, 1.0*-100)
p.update(0)
self.assertEqual(output.correction, 0)
#test ki is working
p = Pid(output, 0.0, kI = 1.0)
p.update(0)
self.assertEqual(output.correction, 0)
p.update(100)
p.update(50)
self.assertEqual(output.correction, 1.0*-150)
p.update(-150)
self.assertEqual(output.correction, 0)
#test izone
p = Pid(output, 0.0, kI = 1.0, izone=100)
p.update(0)
self.assertEqual(output.correction, 0)
p.update(100)
p.update(50)
self.assertEqual(output.correction, 1.0*-100)
p.update(-100)
self.assertEqual(output.correction, 0)
p.update(0)
self.assertEqual(output.correction, 0)
p.update(-100)
p.update(-50)
self.assertEqual(output.correction, 1.0*100)
p.update(100)
self.assertEqual(output.correction, 0)
#test kd is working
p = Pid(output, 0.0, kD = 1.0)
p.update(0)
self.assertEqual(output.correction, 0)
p.update(100)
self.assertEqual(output.correction, -100)
p.update(0)
self.assertEqual(output.correction, 100)
p.update(0)
self.assertEqual(output.correction, 0)
#test kf is working
p = Pid(output, 0.0, kF = 1.0)
p.set_set_point(0)
p.update(0)
self.assertEqual(output.correction, 0)
p.set_set_point(100)
p.update(0)
self.assertEqual(output.correction, 100)
p.set_set_point(0)
p.update(0)
self.assertEqual(output.correction, 0)
rospy.Subscriber('cmd_pose', Pose, callback_pos)
rospy.Subscriber("odom", Odometry, callback_odom)
rospy.Subscriber('manual', Bool, callback_manual)
rate = rospy.Rate(30)
val = Cart_values()
vel_setp = Twist()
vel_curr = Twist()
pos_setp = Pose()
pos_curr = Pose()
kp_vel = rospy.get_param('kp_vel')
ki_vel = rospy.get_param('ki_vel')
kd_vel = rospy.get_param('kd_vel')
kp_turn = rospy.get_param('kp_turn')
ki_turn = rospy.get_param('ki_turn')
kd_turn = rospy.get_param('kd_turn')
pid_vel = Pid(kp_vel, ki_vel, kd_vel)
pid_turn = Pid(kp_turn, ki_turn, kd_turn)
unwrapper = Unwrapper(math.pi)
def hardware_controller():
while not rospy.is_shutdown():
rate.sleep()
if __name__ == '__main__':
try:
hardware_controller()
except rospy.ROSInterruptException:
pass
class Robot(object):
VEL_P = 0.0
VEL_I = 0.0
VEL_D = 0.0
VEL_F = 1.0
YAW_P = 0.0
YAW_I = 0
YAW_D = 0
# Pwm and Qep ids for motor controllers a, b and c
MOTOR_A_PWM = "PWM0B-29"
MOTOR_A_QEP = "QEP0"
MOTOR_B_PWM = "PWM1A-36"
MOTOR_B_QEP = "QEP1"
MOTOR_C_PWM = "PWM2A-45"
MOTOR_C_QEP = "QEP2"
INIT_COMMAND = "OmniDrive"
VEL_PID_ENABLED = True
ROBOT_MAX_X_SPEED = math.sin(math.radians(60)) # approx 0.87 (root 2 over 3)
ROBOT_MAX_Y_SPEED = math.cos(math.radians(60)) # 0.50
# the speed of rotation at which we shut down the pid so it does not oscillate
# as you set the set point while still rotating
YAW_MOMENTUM_THRESHOLD = math.radians(10.0) # degrees per second
TIME_TO_AUTO_DISABLE = 10 # seconds till we automatically disable
AUTO_DISABLE_THRESHOLD = 0.05
def __init__(self):
# Velocity PID object setup
self.vel_pid_output_a = VelocityPidOutput()
self.vel_pid_output_b = VelocityPidOutput()
self.vel_pid_output_c = VelocityPidOutput()
self.vel_pid_a = Pid(self.vel_pid_output_a, Robot.VEL_P, Robot.VEL_I, Robot.VEL_D, Robot.VEL_F)
self.vel_pid_b = Pid(self.vel_pid_output_b, Robot.VEL_P, Robot.VEL_I, Robot.VEL_D, Robot.VEL_F)
self.vel_pid_c = Pid(self.vel_pid_output_c, Robot.VEL_P, Robot.VEL_I, Robot.VEL_D, Robot.VEL_F)
# Qep encoder object setup
self.qep_a = Qep(Robot.MOTOR_A_QEP)
self.qep_b = Qep(Robot.MOTOR_B_QEP)
self.qep_c = Qep(Robot.MOTOR_C_QEP)
# Pwm object setup
self.pwm_a = Pwm(Robot.MOTOR_A_PWM)
self.pwm_b = Pwm(Robot.MOTOR_B_PWM)
self.pwm_c = Pwm(Robot.MOTOR_C_PWM)
self.pwms = [self.pwm_a, self.pwm_b, self.pwm_c]
# Create MotorController objects
self.motor_a = MotorController(self.pwm_a, self.vel_pid_a, self.vel_pid_output_a, self.qep_a, self.VEL_PID_ENABLED)
self.motor_b = MotorController(self.pwm_b, self.vel_pid_b, self.vel_pid_output_b, self.qep_b, self.VEL_PID_ENABLED)
self.motor_c = MotorController(self.pwm_c, self.vel_pid_c, self.vel_pid_output_c, self.qep_c, self.VEL_PID_ENABLED)
self.motors = [self.motor_a, self.motor_b, self.motor_c]
# set up pid
self.yaw_pid_output = YawPidOutput()
self.yaw_pid = Pid(self.yaw_pid_output, Robot.YAW_P, Robot.YAW_I, Robot.YAW_D, izone=1.0)
# initialise mpu/imu server module
self.mpu = Mpu()
self.yaw_pid_thread = YawPidThread(self.yaw_pid, self.mpu)
self.current_command = Robot.INIT_COMMAND
# pid *enabled* by default
self.yaw_pid_enabled = False
# pid not in *control* by default, this is automatically set by drive
self.pid_in_control = False
self.field_centered = True
self.enabled = False
self.interrupted = False
self.last_input_time = time.time()
def drive(self, vX, vY, vZ, throttle):
if not self.enabled:
self.last_input_time = time.time()
for motor in self.motors:
motor.set_speed(0.0)
for p in self.pwms:
p.set_speed(0.0)
p.pwm_off()
return
else:
for p in self.pwms:
p.pwm_on()
if math.fabs(vX) <= self.AUTO_DISABLE_THRESHOLD and math.fabs(vY) <= self.AUTO_DISABLE_THRESHOLD and math.fabs(vZ) <= self.AUTO_DISABLE_THRESHOLD:
if time.time() - self.last_input_time > self.TIME_TO_AUTO_DISABLE:
self.enabled = False
else:
self.last_input_time = time.time()
vPID = 0.0
if self.field_centered:
vX, vY = self.field_orient(vX, vY, self.mpu.get_euler()[0])
# Drive equations that translate vX, vY and vZ into commands to be sent to the motors
# front motor
mA = -(((0.0*vX) + (vY * 1.0))/2.0 + vZ/3.0)
# bottom left motor
mB = (((-vX * math.sin(math.radians(60))) + (-vY / 2.0))/2.0 + vZ/3.0)
# bottom right motor
mC = (((vX * math.sin(math.radians(60))) + (-vY / 2.0))/2.0 + vZ/3.0)
motor_input = [mA, mB, mC]
if self.yaw_pid_enabled:
if not vZ == 0:
# spinning under command -> no PID
self.pid_in_control = False
elif math.fabs(self.mpu.get_gyro()[2]) < Robot.YAW_MOMENTUM_THRESHOLD:
# momentum is less than the threshold and we are not under command
# -> PID can now take control
self.pid_in_control = True
if self.pid_in_control:
vPID = self.yaw_pid_output.value
else:
heading = self.mpu.get_euler()[0] # yaw
self.yaw_pid.set_set_point(heading)
# zero the correction value so that the next time the code runs the existing
# correction value will not still be there
self.yaw_pid_output.value = 0.0
max = 1.0
# find the maximum motor speed
for i in range(3):
if math.fabs(motor_input[i]) > max:
max = abs(motor_input[i])
# scale between -1 and 1
for i in range(3):
motor_input[i] /= max
# multiply by throttle
for i in range(3):
motor_input[i] *= throttle
if self.yaw_pid_enabled:
max = 1
for i in range(3):
motor_input[i] -= vPID
if math.fabs(motor_input[i]) > max:
max = math.fabs(motor_input[i])
for i in range(3):
motor_input[i] /= max
# set the speeds of the motors
for i in range(3):
self.motors[i].set_speed(motor_input[i])
def field_orient(self, vX, vY, yaw_angle):
oriented_vx = vX*math.cos(yaw_angle)+vY*math.sin(yaw_angle)
oriented_vy = -vX*math.sin(yaw_angle)+vY*math.cos(yaw_angle)
return oriented_vx, oriented_vy
