블루투스 메모장

#include <SoftwareSerial.h> // 블루투스 시리얼 통신 라이브러리 추가 #define BT_RXD 8 #define BT_TXD 7 SoftwareSerial bluetooth(BT_RXD, BT_TXD); // 블루투스 설정 BTSerial(Tx, Rx) void setup() { Serial.begin(9600); bluetooth.begin(9600); // 블루투스 통신 시작 } void loop() { if (bluetooth.available()) { // 블루투스에서 보낸 내용은 시리얼모니터로 전송 Serial.write(bluetooth.read()); } if (Serial.available()) { // 시리얼모니터에서 보낸 내용은 블루투스로 전송 bluetooth.write(Serial.read()); } }

#!/usr/bin/python # -*- coding: utf-8 -*- import rospy from std_msgs.msg import String import queue avg_num = 15 class KalmanFilter(): def __init__(self, processNoise=0.05, measurementNoise=20): super(KalmanFilter, self).__init__() self.initialized = False # 예측 모델의 noise self.processNoise = processNoise # 측정 값의 노이즈 self.measurementNoise = measurementNoise self.predictedRSSI = 0 self.errorCovariance = 10 # 초기 오차 공분산을 1로 설정합니다. def filtering(self, rssi): if not self.initialized: self.initialized = True self.predictedRSSI = rssi # 초기 예측값은 측정값과 같이 설정합니다. else: # 예측 단계 predictedRSSI = self.predictedRSSI # 오차 공분산 업데이트 errorCovariance = self.errorCovariance + self.processNoise # 업데이트 단계 kalmanGain = errorCovariance / (errorCovariance + self.measurementNoise) # rssi 예측 self.predictedRSSI = predictedRSSI + kalmanGain * (rssi - predictedRSSI) # 오차 공분산 업데이트 self.errorCovariance = (1 - kalmanGain) * errorCovariance return self.predictedRSSI class MovAvgFilter: # 이전 스텝의 평균 prevAvg = 0.0 # 가장 최근 n개의 값을 저장하는 큐 xBuf = queue.Queue() # 참조할 데이터의 갯수 global num def __init__(self, _n = avg_num): # 초기화로 n개의 값을 0으로 둡니다. for _ in range(_n): self.xBuf.put(0) # 참조할 데이터의 갯수를 저장합니다. self.n = _n def filtering(self, x): # 큐의 front 값은 x_(k-n) 에 해당합니다. front = self.xBuf.get() # 이번 스텝에 입력 받은 값을 큐에 넣습니다. self.xBuf.put(x) avg = self.prevAvg + (x - front) / self.n self.prevAvg = avg return avg def cal_distance(rssi, alpha = -60, n = 4): distance = 10**((alpha - rssi)/(10*n)) return distance def rssi_callback(data): # filter = MovAvgFilter(avg_num) filter = KalmanFilter() line = data.data remove_line = line.strip().replace('OK+Get:', '') rssi = float(remove_line) filtered_rssi = filter.filtering(rssi) final_distance = cal_distance(filtered_rssi, alpha=-60, n=4) rospy.loginfo("RSSI to Distance : %f", final_distance) def rssi_listener(): rospy.init_node('rssi_listener', anonymous=True) rospy.Subscriber("rssi", String, rssi_callback, 10) rospy.spin() if __name__ == '__main__': try: rssi_listener() except rospy.ROSInterruptException: pass

#include <SoftwareSerial.h> #include <ros.h> #include <std_msgs/String.h> SoftwareSerial bluetooth(2, 3); // RX, TX ros::NodeHandle nh; std_msgs::String rssi_msg; ros::Publisher rssi_pub("rssi", &rssi_msg); void setup() { Serial.begin(9600); bluetooth.begin(9600); delay(1000); sendATCommand("AT+RENEW"); delay(1000); sendATCommand("AT"); delay(1000); sendATCommand("AT+ROLE1"); delay(1000); sendATCommand("AT+IMME1"); delay(3000); sendATCommand("AT+CON10CEA9FCDF5B"); nh.initNode(); nh.advertise(rssi_pub); } void loop() { sendATCommand("AT+RSSI?"); // cost 40ms delay(10); // cost 10ms // total 50ms // Read RSSI data from the Bluetooth module String rssiData = readATResponse(); // Publish the RSSI data as a string to the "rssi" topic rssi_msg.data = rssiData.c_str(); rssi_pub.publish(&rssi_msg); nh.spinOnce(); } void sendATCommand(String command) { bluetooth.print(command); delay(40); } String readATResponse() { String response = ""; while (bluetooth.available()) { char c = bluetooth.read(); response += c; } return response; }