initFunction.hpp

00001 #ifndef ZETA_STM_KINETIC_INITFUNCTION_H_
00002 #define ZETA_STM_KINETIC_INITFUNCTION_H_
00003 #include "mbed.h"
00004 #include "threadDeclaration.hpp"
00005 #include "variables/instanceHeader.hpp"
00006 #include "rosHeader.hpp"
00007 #include "configurations/robotConfig.h"
00008 void initThread() {
00009     int index = 0;
00010     gThread[index++].start(IMU_thread);
00011     gThread[index++].start(bt_thread);
00012     gThread[index++].start(sonar_thread);
00013     gThread[index++].start(sonar_pub_thread);
00014     gThread[index++].start(bt_pub_thread);
00015 #ifdef NO_ROS
00016     gThread[index++].start(print_thread);
00017 #else
00018     //gThread[3].start(module_thread);
00019     gThread[index++].start(imu_pub_thread);
00020     gThread[index++].start(estop_pub_thread);
00021 #if (ROBOT_TYPE == MODEL_D)
00022     gThread[index++].start(level_sensor_thread);
00023 #endif
00024 #if (ROBOT_TYPE == MODEL_I)
00025     gThread[index++].start(lidar_dusty_pub_thread);
00026 #endif
00027 #endif /* #ifdef NO_ROS */
00028     //gThread[8].start(test_thread);
00029 }
00030 
00031 void initBT() {
00032     bt.baud(9600);
00033     bt_data.rec = '\0';
00034     bt_data.sen = '\0';
00035 }
00036 
00037 #ifdef NO_ROS
00038 void initSerial() {
00039     pc.baud(115200);
00040 }
00041 #else
00042 void initROS() {
00043     nh.getHardware()->setBaud(ROSSERIAL_BUADRATE);
00044     nh.initNode();
00045     US_msg.data = (float*)malloc(sizeof(float) * NUM_SONAR);
00046     US_msg.data_length = NUM_SONAR;
00047     nh.advertise(IMU_publisher);
00048     nh.advertise(EStop_publisher);
00049     //nh.advertise(Bumper_publisher);
00050     nh.advertise(US_publisher);
00051     nh.advertise(Bluetooth_publisher);
00052     nh.subscribe(Bluetooth_subscriber);
00053     nh.subscribe(SsrTest_subscriber);
00054 #if (ROBOT_TYPE == MODEL_D)
00055     nh.advertise(water_level_publisher);
00056 #endif
00057 #if (ROBOT_TYPE == MODEL_I)
00058     nh.subscribe(ScrubberControl_subscriber);
00059     nh.advertise(LidarDusty_publisher);
00060     nh.advertise(EmergencyStop_publisher);
00061     nh.subscribe(WarningFieldSelect_subscriber);
00062     nh.subscribe(IgnoreWarningField_subscriber);
00063 #endif
00064     while(!nh.connected()) nh.spinOnce();
00065     waitTmr.start();
00066     nh.loginfo(__FW_VERSION__);
00067 }
00068 #endif
00069 
00070 #endif