semin ahn / Mbed OS zeta_stm_kinetic

Dependencies:   BufferedSerial

variables/instanceHeader.hpp

Committer:
_seminahn
Date:
2021-04-02
Revision:
0:4ff8aeb3e4d1
Child:
1:2594a70c1ddd

File content as of revision 0:4ff8aeb3e4d1:

#ifndef ZETA_STM_KINETIC_INSTANCEHEADER_H_
#define ZETA_STM_KINETIC_INSTANCEHEADER_H_
#include "mbed.h"
#include "rosHeader.hpp"
#include "moduleHeader.hpp"
#include "callbackHeader.hpp"
#include "defineHeader.h"

/* COM variables ------------------------------------------------------------ */
    #if (NO_ROS)
extern mbed::Serial pc;
    #else
extern ros::NodeHandle nh;
    #endif
extern mbed::Serial bt;

/* ROS variables ------------------------------------------------------------ */
// Publisher
sensor_msgs::Imu imu_msg;
ros::Publisher IMU_publisher("imu", &imu_msg);

std_msgs::Bool EStop_msg;
ros::Publisher EStop_publisher("estop", &EStop_msg);
std_msgs::Bool Bumper_msg;
ros::Publisher Bumper_publisher("bumper", &Bumper_msg);

zetabot_main::SonarArray US_msg;
ros::Publisher US_publisher("sonar", &US_msg);

std_msgs::String Bluetooth_msg;
ros::Publisher Bluetooth_publisher("autocharge_state_INO", &Bluetooth_msg);

// Subscriber
void ModuleControlCB(const zetabot_main::ModuleControlMsgs& msg);
ros::Subscriber<zetabot_main::ModuleControlMsgs> ModuleControl_subscriber("module_control_NUC",ModuleControlCB);
zetabot_main::ModuleControlMsgs moduleControlMsg;

void BluetoothCB(const std_msgs::UInt8& msg);
ros::Subscriber<std_msgs::UInt8> Bluetooth_subscriber("autocharge_state_NUC", &BluetoothCB);


/* Threads and Timers ------------------------------------------------------- */
Thread gThread[NUM_THREAD];

Timer waitTmr;


/* Modules ------------------------------------------------------------------ */
// Driving non-related modules
extern MODULE module;
RELAY relay(CHARGE_RELAYP,CHARGE_RELAYN);
// Driving related modules

MPU9250_SPI mpu(IMU_MOSI, IMU_MISO, IMU_SCK, IMU_NCS, IMU_INT);

HCSR04 sonar[NUM_SONAR] = {
    HCSR04(SONAR0_ECHO, SONAR0_TRIG, SONAR_FILTER_WS), HCSR04(SONAR1_ECHO, SONAR1_TRIG, SONAR_FILTER_WS),
    HCSR04(SONAR2_ECHO, SONAR2_TRIG, SONAR_FILTER_WS), HCSR04(SONAR3_ECHO, SONAR3_TRIG, SONAR_FILTER_WS),
    HCSR04(SONAR4_ECHO, SONAR4_TRIG, SONAR_FILTER_WS,1), HCSR04(SONAR5_ECHO, SONAR5_TRIG, SONAR_FILTER_WS,1),
    HCSR04(SONAR6_ECHO, SONAR6_TRIG, SONAR_FILTER_WS), HCSR04(SONAR7_ECHO, SONAR7_TRIG, SONAR_FILTER_WS),
    HCSR04(SONAR8_ECHO, SONAR8_TRIG, SONAR_FILTER_WS), HCSR04(SONAR9_ECHO, SONAR9_TRIG, SONAR_FILTER_WS),
    HCSR04(SONAR10_ECHO, SONAR10_TRIG, SONAR_FILTER_WS)
    };

EStop estop(ESTOP_PIN);

/* For test ----------------------------------------------------------------- */

#endif