Diff: src/threadDeclaration.hpp

Revision:

1:2594a70c1ddd

Parent:

0:4ff8aeb3e4d1

Child:

2:0de4854743f7

--- a/src/threadDeclaration.hpp	Fri Apr 02 05:24:49 2021 +0000
+++ b/src/threadDeclaration.hpp	Wed May 26 05:26:16 2021 +0000
@@ -14,7 +14,7 @@
 using myUtil::calibrationProcess;
 using myUtil::applyCalbratedValue;
 
-    #if (NO_ROS)
+    #ifdef NO_ROS
 void print_thread() {
     const float time2print = 10.0;
     const float print_thread_hz = 3.0;
@@ -215,7 +215,13 @@
     int cnt = 0;
     char temp[2] = {'\0',};
     while(1) {
-        if(bt.readable()) bt_data.rec = bt.getc();
+        if(bt.readable())
+        {
+            bt_data.rec = bt.getc();
+            char log_char[2];
+            sprintf(log_char,"%c",bt_data.rec);
+            nh.loginfo(log_char);
+        }
         if(++cnt % 10 == 0) {
             bt.putc(bt_data.sen);
             cnt = 0;
@@ -226,31 +232,20 @@
 
 void sonar_thread() {
     const float sonar_hz = 15.0;
-    for(int i = 0 ; i < NUM_SONAR; i++) {
-        sonar[i].setRanges(3, 70);
-    }
+    sonar_manager.Begin(sonar_hz);
     while(true) {
-        for(int i = 0; i < NUM_SONAR; i++) {
-            if(sonar[i].isNewDataReady()) {
-                dist[i] = sonar[i].getDistance_cm();
-                //dist_raw[i] = sonar[i]->getDistance_raw();
-            }
-            else sonar[i].startMeasurement();
-        }
+        sonar_manager.GetDistance(dist);
         ThisThread::sleep_for(1000/sonar_hz);
     }
 }
-
+/*
 void module_thread() {
     const float module_hz = 10.0;
     while(1) {
         if(isSubscribe) {
             char send_str[32] = {'\0',};
-            module.setMsg(&moduleControlMsg);
-            module.control();
-            sprintf(send_str,"rec: %d|%d|%d|%d|%u",moduleControlMsg.module_power[0],moduleControlMsg.module_power[1],
-            moduleControlMsg.module_power[2],moduleControlMsg.module_power[3],moduleControlMsg.pulifier);
-            nh.loginfo(send_str);
+            uvc.setMsg(&UVCcontrolMsg);
+            uvc.control();
             isSubscribe = false;
         }
         ThisThread::sleep_for(1000/module_hz);
@@ -258,6 +253,7 @@
     
     ;  
 }
+*/
 
 #define CALIBRATION_MODE 1
 void IMU_thread() {
@@ -279,7 +275,7 @@
             a = mpu.getAccelVect();
             g = mpu.getGyroVect();
             m = mpu.getMagVect();
-    #if (NO_ROS)    
+    #ifdef NO_ROS 
             gAcc_raw.x = a.x*G;
             gAcc_raw.y = a.y*G;
             gAcc_raw.z = a.z*G;
@@ -329,8 +325,18 @@
 }
 extern RELAY relay;
 void test_thread() {
+    
     while(true) {
-        ;
+        bt.putc('a');
+        ThisThread::sleep_for(10);
+        if(bt.readable())
+        {
+            char debug_str[6];
+            sprintf(debug_str,"%c",bt.getc());
+            nh.loginfo(debug_str);
+            nh.spinOnce();
+        }
+        ThisThread::sleep_for(500);
     }   
 }