semin ahn / Mbed OS zeta_stm_kinetic

Dependencies:   BufferedSerial

variables/instanceHeader.hpp

Committer:
_seminahn
Date:
2021-05-26
Revision:
1:2594a70c1ddd
Parent:
0:4ff8aeb3e4d1
Child:
2:0de4854743f7

File content as of revision 1:2594a70c1ddd:

#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 UVCcontrolCB(const std_msgs::Bool& msg);
//ros::Subscriber<std_msgs::Bool> UVCcontrol_subscriber("uvc_control_command",UVCcontrolCB);
//std_msgs::Bool UVCcontrolMsg;

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 UVC uvc;
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(SONAR_ECHO0, SONAR_TRIG0, SONAR_FILTER_WS,1), HCSR04(SONAR_ECHO1, SONAR_TRIG0, SONAR_FILTER_WS,2),
    HCSR04(SONAR_ECHO2, SONAR_TRIG0, SONAR_FILTER_WS,3), HCSR04(SONAR_ECHO3, SONAR_TRIG0, SONAR_FILTER_WS,4),
    HCSR04(SONAR_ECHO4, SONAR_TRIG0, SONAR_FILTER_WS,5), HCSR04(SONAR_ECHO5, SONAR_TRIG0, SONAR_FILTER_WS,6),
    HCSR04(SONAR_ECHO6, SONAR_TRIG0, SONAR_FILTER_WS,7), HCSR04(SONAR_ECHO7, SONAR_TRIG0, SONAR_FILTER_WS,8),
    HCSR04(SONAR_ECHO8, SONAR_TRIG0, SONAR_FILTER_WS,9), HCSR04(SONAR_ECHO9, SONAR_TRIG0, SONAR_FILTER_WS,10)
    };
SONAR_MANAGER sonar_manager(SONAR_TRIG0, sonar, NUM_SONAR);

EStop estop(EMERGENCY);

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

#endif