diff --git a/class_model/src/PID.py b/class_model/src/PID.py deleted file mode 100755 index adf5f39..0000000 --- a/class_model/src/PID.py +++ /dev/null @@ -1,49 +0,0 @@ -import cvzone -import cv2 -import numpy as np -import time - - -class PID: - def __init__(self, pidVals, targetVal, axis=0, limit=None): - self.pidVals = pidVals - self.targetVal = targetVal - self.axis = axis - self.pError = 0 - self.limit = limit - self.I = 0 - self.pTime = 0 - - def update(self, cVal): - # Current Value - Target Value - t = time.time() - self.pTime - error = cVal - self.targetVal - P = self.pidVals[0] * error - self.I = self.I + (self.pidVals[1] * error * t) - D = (self.pidVals[2] * (error - self.pError)) / t - - result = P + self.I + D - - if self.limit is not None: - result = float(np.clip(result, self.limit[0], self.limit[1])) - self.pError = error - self.ptime = time.time() - - return result - - - -def main(): - # For a 640x480 image center target is 320 and 240 - xPID = PID([1, 0.000000000001, 1], 640 // 2) - yPID = PID([1, 0.000000000001, 1], 480 // 2, axis=1, limit=[-100, 100]) - - while True: - - xVal = int(xPID.update(cx)) - yVal = int(yPID.update(cy)) - - - -if __name__ == "__main__": - main() diff --git a/class_model/src/command.py b/class_model/src/command.py deleted file mode 100755 index 1efac16..0000000 --- a/class_model/src/command.py +++ /dev/null @@ -1,81 +0,0 @@ -#! /usr/bin/env python3 -#coding:utf-8 - -import ssl -import rospy -from std_msgs.msg import String -import paho.mqtt.client as mqtt -import json - - - # Ros -def ros_pub(dataJson): - global publisher, rate ,publisher1 ,publisher2,publisher3,publisher4,publisher5 - # data = json.loads(dataJson) - publisher.publish(dataJson) #將date字串發布到topic - publisher1.publish(dataJson) - publisher2.publish(dataJson) - publisher3.publish(dataJson) - publisher4.publish(dataJson) - publisher5.publish(dataJson) - publisher6.publish(dataJson) - - rate.sleep() - # print(f"publish data {data}") - -# MQTT -def on_connect(self, userdata, flags, rc): - print("Connected with result code " + str(rc)) - client.subscribe("cmd/broadcast") - - -def on_message(self, userdata, msg): - #msg = msg.payload.decode('utf-8') - print(f"msg.topic {msg}") - ros_pub(msg.payload.decode('utf-8')) - - -def initialise_clients(clientName): - # callback assignment - initialise_client = mqtt.Client(clientName, False) - initialise_client.topic_ack = [] - return initialise_client - - -if __name__ == '__main__': - # Mqtt - mqtt_config = {"host": "140.120.31.133", "port": 1883, "topic": "cmd/broadcast"} - client = initialise_clients("receiver") - client.on_connect = on_connect - client.on_message = on_message - client.connect(mqtt_config["host"], mqtt_config["port"], 60) - - # Ros - Mqtt_Node = 'publisher_py' - rospy.init_node("cmd_receiver") - # initialize Ros node - topicName = 'cmd_receiver' - publisher = rospy.Publisher(topicName,String,queue_size=10) - - topicName1 = '/drone1/cmd_receiver' - publisher1 = rospy.Publisher(topicName1,String,queue_size=10) - - topicName2 = '/drone2/cmd_receiver' - publisher2 = rospy.Publisher(topicName2,String,queue_size=10) - - topicName3 = '/drone3/cmd_receiver' - publisher3 = rospy.Publisher(topicName3,String,queue_size=10) - - topicName4 = '/drone4/cmd_receiver' - publisher4 = rospy.Publisher(topicName4,String,queue_size=10) - - topicName5 = '/drone5/cmd_receiver' - publisher5 = rospy.Publisher(topicName5,String,queue_size=10) - - topicName6 = '/drone6/cmd_receiver' - publisher6 = rospy.Publisher(topicName6,String,queue_size=10) - - - rate = rospy.Rate(10) - - client.loop_forever() diff --git a/class_model/src/test_pub.py b/class_model/src/test_pub.py deleted file mode 100755 index 0c72b37..0000000 --- a/class_model/src/test_pub.py +++ /dev/null @@ -1,16 +0,0 @@ -#!/usr/bin/env python3 -#coding:utf-8 -import rospy -# from mavros_msgs.msg import Waypoint -from diagnostic_msgs.msg import KeyValue - -def callBack(data): - print (data) - - -if __name__ == '__main__': - nodeName = 'subscriber_py' - rospy.init_node(nodeName) - topicName = '/Flight_Information_reciver' - subscriber = rospy.Subscriber(topicName,KeyValue,callBack) #從topic獲取string再呼叫callback - rospy.spin() \ No newline at end of file diff --git a/class_model/src/tower.py b/class_model/src/tower.py deleted file mode 100755 index 254f93d..0000000 --- a/class_model/src/tower.py +++ /dev/null @@ -1,54 +0,0 @@ -#! /usr/bin/env python3 -#coding:utf-8 - -import paho.mqtt.client as mqtt -import time - - -def on_connect(self, userdata, flags, rc): - global connect_flag - print("Connected with result code " + str(rc)) - connect_flag = True - - - -def initialise_clients(clientName): - # callback assignment - initialise_client = mqtt.Client(clientName, True) - initialise_client.topic_ack = [] - return initialise_client - - -# publish a message -def publish(topics, message, waitForAck=False): - mid = client.publish(topics, message, 1)[1] - print(f"just published {message} to topic") - if waitForAck: - while mid not in client.topic_ack: - print("wait for ack") - time.sleep(0.25) - client.topic_ack.remove(mid) - - -def on_publish(self, userdata, mid): - client.topic_ack.append(mid) - - -connect_flag = False -mqtt_config = {"host": "140.120.31.133", "port": 1883, "topic": "cmd/broadcast", "name": "Tower"} -client = initialise_clients(mqtt_config["name"]) -client.on_publish = on_publish -client.on_connect = on_connect -# client.on_message = on_message - -client.connect(mqtt_config["host"], mqtt_config["port"], 60) -client.loop_start() - -# publish(topicBroadcast, "Connect", True) -while True: - if connect_flag: - break - -while True: - command = input("command: ") - publish(mqtt_config["topic"], command)