semin ahn / Mbed OS zeta_stm_kinetic

Dependencies:   BufferedSerial

Committer:
_seminahn
Date:
Fri Apr 02 05:24:49 2021 +0000
Revision:
0:4ff8aeb3e4d1
Child:
1:2594a70c1ddd
top_module

Who changed what in which revision?

UserRevisionLine numberNew contents of line
_seminahn 0:4ff8aeb3e4d1 1 #ifndef ZETA_STM_KINETIC_INSTANCEHEADER_H_
_seminahn 0:4ff8aeb3e4d1 2 #define ZETA_STM_KINETIC_INSTANCEHEADER_H_
_seminahn 0:4ff8aeb3e4d1 3 #include "mbed.h"
_seminahn 0:4ff8aeb3e4d1 4 #include "rosHeader.hpp"
_seminahn 0:4ff8aeb3e4d1 5 #include "moduleHeader.hpp"
_seminahn 0:4ff8aeb3e4d1 6 #include "callbackHeader.hpp"
_seminahn 0:4ff8aeb3e4d1 7 #include "defineHeader.h"
_seminahn 0:4ff8aeb3e4d1 8
_seminahn 0:4ff8aeb3e4d1 9 /* COM variables ------------------------------------------------------------ */
_seminahn 0:4ff8aeb3e4d1 10 #if (NO_ROS)
_seminahn 0:4ff8aeb3e4d1 11 extern mbed::Serial pc;
_seminahn 0:4ff8aeb3e4d1 12 #else
_seminahn 0:4ff8aeb3e4d1 13 extern ros::NodeHandle nh;
_seminahn 0:4ff8aeb3e4d1 14 #endif
_seminahn 0:4ff8aeb3e4d1 15 extern mbed::Serial bt;
_seminahn 0:4ff8aeb3e4d1 16
_seminahn 0:4ff8aeb3e4d1 17 /* ROS variables ------------------------------------------------------------ */
_seminahn 0:4ff8aeb3e4d1 18 // Publisher
_seminahn 0:4ff8aeb3e4d1 19 sensor_msgs::Imu imu_msg;
_seminahn 0:4ff8aeb3e4d1 20 ros::Publisher IMU_publisher("imu", &imu_msg);
_seminahn 0:4ff8aeb3e4d1 21
_seminahn 0:4ff8aeb3e4d1 22 std_msgs::Bool EStop_msg;
_seminahn 0:4ff8aeb3e4d1 23 ros::Publisher EStop_publisher("estop", &EStop_msg);
_seminahn 0:4ff8aeb3e4d1 24 std_msgs::Bool Bumper_msg;
_seminahn 0:4ff8aeb3e4d1 25 ros::Publisher Bumper_publisher("bumper", &Bumper_msg);
_seminahn 0:4ff8aeb3e4d1 26
_seminahn 0:4ff8aeb3e4d1 27 zetabot_main::SonarArray US_msg;
_seminahn 0:4ff8aeb3e4d1 28 ros::Publisher US_publisher("sonar", &US_msg);
_seminahn 0:4ff8aeb3e4d1 29
_seminahn 0:4ff8aeb3e4d1 30 std_msgs::String Bluetooth_msg;
_seminahn 0:4ff8aeb3e4d1 31 ros::Publisher Bluetooth_publisher("autocharge_state_INO", &Bluetooth_msg);
_seminahn 0:4ff8aeb3e4d1 32
_seminahn 0:4ff8aeb3e4d1 33 // Subscriber
_seminahn 0:4ff8aeb3e4d1 34 void ModuleControlCB(const zetabot_main::ModuleControlMsgs& msg);
_seminahn 0:4ff8aeb3e4d1 35 ros::Subscriber<zetabot_main::ModuleControlMsgs> ModuleControl_subscriber("module_control_NUC",ModuleControlCB);
_seminahn 0:4ff8aeb3e4d1 36 zetabot_main::ModuleControlMsgs moduleControlMsg;
_seminahn 0:4ff8aeb3e4d1 37
_seminahn 0:4ff8aeb3e4d1 38 void BluetoothCB(const std_msgs::UInt8& msg);
_seminahn 0:4ff8aeb3e4d1 39 ros::Subscriber<std_msgs::UInt8> Bluetooth_subscriber("autocharge_state_NUC", &BluetoothCB);
_seminahn 0:4ff8aeb3e4d1 40
_seminahn 0:4ff8aeb3e4d1 41
_seminahn 0:4ff8aeb3e4d1 42 /* Threads and Timers ------------------------------------------------------- */
_seminahn 0:4ff8aeb3e4d1 43 Thread gThread[NUM_THREAD];
_seminahn 0:4ff8aeb3e4d1 44
_seminahn 0:4ff8aeb3e4d1 45 Timer waitTmr;
_seminahn 0:4ff8aeb3e4d1 46
_seminahn 0:4ff8aeb3e4d1 47
_seminahn 0:4ff8aeb3e4d1 48 /* Modules ------------------------------------------------------------------ */
_seminahn 0:4ff8aeb3e4d1 49 // Driving non-related modules
_seminahn 0:4ff8aeb3e4d1 50 extern MODULE module;
_seminahn 0:4ff8aeb3e4d1 51 RELAY relay(CHARGE_RELAYP,CHARGE_RELAYN);
_seminahn 0:4ff8aeb3e4d1 52 // Driving related modules
_seminahn 0:4ff8aeb3e4d1 53
_seminahn 0:4ff8aeb3e4d1 54 MPU9250_SPI mpu(IMU_MOSI, IMU_MISO, IMU_SCK, IMU_NCS, IMU_INT);
_seminahn 0:4ff8aeb3e4d1 55
_seminahn 0:4ff8aeb3e4d1 56 HCSR04 sonar[NUM_SONAR] = {
_seminahn 0:4ff8aeb3e4d1 57 HCSR04(SONAR0_ECHO, SONAR0_TRIG, SONAR_FILTER_WS), HCSR04(SONAR1_ECHO, SONAR1_TRIG, SONAR_FILTER_WS),
_seminahn 0:4ff8aeb3e4d1 58 HCSR04(SONAR2_ECHO, SONAR2_TRIG, SONAR_FILTER_WS), HCSR04(SONAR3_ECHO, SONAR3_TRIG, SONAR_FILTER_WS),
_seminahn 0:4ff8aeb3e4d1 59 HCSR04(SONAR4_ECHO, SONAR4_TRIG, SONAR_FILTER_WS,1), HCSR04(SONAR5_ECHO, SONAR5_TRIG, SONAR_FILTER_WS,1),
_seminahn 0:4ff8aeb3e4d1 60 HCSR04(SONAR6_ECHO, SONAR6_TRIG, SONAR_FILTER_WS), HCSR04(SONAR7_ECHO, SONAR7_TRIG, SONAR_FILTER_WS),
_seminahn 0:4ff8aeb3e4d1 61 HCSR04(SONAR8_ECHO, SONAR8_TRIG, SONAR_FILTER_WS), HCSR04(SONAR9_ECHO, SONAR9_TRIG, SONAR_FILTER_WS),
_seminahn 0:4ff8aeb3e4d1 62 HCSR04(SONAR10_ECHO, SONAR10_TRIG, SONAR_FILTER_WS)
_seminahn 0:4ff8aeb3e4d1 63 };
_seminahn 0:4ff8aeb3e4d1 64
_seminahn 0:4ff8aeb3e4d1 65 EStop estop(ESTOP_PIN);
_seminahn 0:4ff8aeb3e4d1 66
_seminahn 0:4ff8aeb3e4d1 67 /* For test ----------------------------------------------------------------- */
_seminahn 0:4ff8aeb3e4d1 68
_seminahn 0:4ff8aeb3e4d1 69 #endif