Diff: src/initFunction.hpp

Revision:

0:4ff8aeb3e4d1

Child:

1:2594a70c1ddd

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/initFunction.hpp	Fri Apr 02 05:24:49 2021 +0000
@@ -0,0 +1,54 @@
+#ifndef ZETA_STM_KINETIC_INITFUNCTION_H_
+#define ZETA_STM_KINETIC_INITFUNCTION_H_
+#include "mbed.h"
+#include "threadDeclaration.hpp"
+#include "instanceHeader.hpp"
+#include "rosHeader.hpp"
+
+void initThread() {
+    gThread[0].start(IMU_thread);
+    gThread[1].start(bt_thread);
+    gThread[2].start(sonar_thread);
+    #if (NO_ROS)
+    gThread[3].start(print_thread);
+    #else
+    gThread[3].start(module_thread);
+    gThread[4].start(imu_pub_thread);
+    gThread[5].start(sonar_pub_thread);
+    gThread[6].start(estop_pub_thread);
+    gThread[7].start(bt_pub_thread);
+    #endif
+    gThread[8].start(test_thread);
+}
+
+void initBT() {
+    bt.baud(9600);
+    bt_data.rec = '\0';
+    bt_data.sen = '\0';
+}
+
+    #if (NO_ROS)
+void initSerial() {
+    pc.baud(115200);
+}
+    #else
+void initROS() {
+    nh.getHardware()->setBaud(460800);
+    nh.initNode();
+    nh.advertise(IMU_publisher);
+    nh.advertise(EStop_publisher);
+    nh.advertise(Bumper_publisher);
+    nh.advertise(US_publisher);
+    nh.advertise(Bluetooth_publisher);
+    nh.subscribe(ModuleControl_subscriber);
+    nh.subscribe(Bluetooth_subscriber);
+    memset(moduleControlMsg.module_power,false,sizeof(moduleControlMsg.module_power));
+    moduleControlMsg.pulifier = 0U;
+    while(!nh.connected()) nh.spinOnce();
+    waitTmr.start();
+    nh.loginfo("zeta module and sonar controller is initialized!!!");
+    nh.loginfo("==================================================");
+}
+    #endif
+
+#endif
\ No newline at end of file