Diff: src/initFunction.hpp

Revision:

3:a4677501ae87

Parent:

2:0de4854743f7

--- a/src/initFunction.hpp	Thu Jun 10 01:23:00 2021 +0000
+++ b/src/initFunction.hpp	Tue Nov 30 08:13:05 2021 +0000
@@ -2,22 +2,29 @@
 #define ZETA_STM_KINETIC_INITFUNCTION_H_
 #include "mbed.h"
 #include "threadDeclaration.hpp"
-#include "instanceHeader.hpp"
+#include "variables/instanceHeader.hpp"
 #include "rosHeader.hpp"
-
+#include "configurations/robotConfig.h"
 void initThread() {
-    gThread[0].start(IMU_thread);
-    gThread[1].start(bt_thread);
-    gThread[2].start(sonar_thread);
-    #ifdef NO_ROS
-    gThread[3].start(print_thread);
-    #else
+    int index = 0;
+    gThread[index++].start(IMU_thread);
+    gThread[index++].start(bt_thread);
+    gThread[index++].start(sonar_thread);
+    gThread[index++].start(sonar_pub_thread);
+    gThread[index++].start(bt_pub_thread);
+#ifdef NO_ROS
+    gThread[index++].start(print_thread);
+#else
     //gThread[3].start(module_thread);
-    gThread[3].start(imu_pub_thread);
-    gThread[4].start(sonar_pub_thread);
-    gThread[5].start(estop_pub_thread);
-    gThread[7].start(bt_pub_thread);
-    #endif
+    gThread[index++].start(imu_pub_thread);
+    gThread[index++].start(estop_pub_thread);
+#if (ROBOT_TYPE == MODEL_D)
+    gThread[index++].start(level_sensor_thread);
+#endif
+#if (ROBOT_TYPE == MODEL_I)
+    gThread[index++].start(lidar_dusty_pub_thread);
+#endif
+#endif /* #ifdef NO_ROS */
     //gThread[8].start(test_thread);
 }
 
@@ -27,28 +34,37 @@
     bt_data.sen = '\0';
 }
 
-    #ifdef NO_ROS
+#ifdef NO_ROS
 void initSerial() {
     pc.baud(115200);
 }
-    #else
+#else
 void initROS() {
-    const char version[] = "stm_board\t1.1.0";
-    nh.getHardware()->setBaud(460800);
+    nh.getHardware()->setBaud(ROSSERIAL_BUADRATE);
     nh.initNode();
+    US_msg.data = (float*)malloc(sizeof(float) * NUM_SONAR);
+    US_msg.data_length = NUM_SONAR;
     nh.advertise(IMU_publisher);
     nh.advertise(EStop_publisher);
-    nh.advertise(Bumper_publisher);
+    //nh.advertise(Bumper_publisher);
     nh.advertise(US_publisher);
     nh.advertise(Bluetooth_publisher);
-    //nh.subscribe(ModuleControl_subscriber);
-    //nh.subscribe(UVCcontrol_subscriber);
     nh.subscribe(Bluetooth_subscriber);
-    //UVCcontrolMsg.data = false;
+    nh.subscribe(SsrTest_subscriber);
+#if (ROBOT_TYPE == MODEL_D)
+    nh.advertise(water_level_publisher);
+#endif
+#if (ROBOT_TYPE == MODEL_I)
+    nh.subscribe(ScrubberControl_subscriber);
+    nh.advertise(LidarDusty_publisher);
+    nh.advertise(EmergencyStop_publisher);
+    nh.subscribe(WarningFieldSelect_subscriber);
+    nh.subscribe(IgnoreWarningField_subscriber);
+#endif
     while(!nh.connected()) nh.spinOnce();
     waitTmr.start();
-    nh.loginfo(version);
+    nh.loginfo(__FW_VERSION__);
 }
-    #endif
+#endif
 
 #endif