semin ahn / Mbed OS zeta_stm_kinetic

Dependencies:   BufferedSerial

Committer:
_seminahn
Date:
Thu Jun 10 01:23:00 2021 +0000
Revision:
2:0de4854743f7
Parent:
1:2594a70c1ddd
Child:
3:a4677501ae87
v1.1.0 change ssr logic

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 1:2594a70c1ddd 34
_seminahn 1:2594a70c1ddd 35 //void UVCcontrolCB(const std_msgs::Bool& msg);
_seminahn 1:2594a70c1ddd 36 //ros::Subscriber<std_msgs::Bool> UVCcontrol_subscriber("uvc_control_command",UVCcontrolCB);
_seminahn 1:2594a70c1ddd 37 //std_msgs::Bool UVCcontrolMsg;
_seminahn 0:4ff8aeb3e4d1 38
_seminahn 0:4ff8aeb3e4d1 39 void BluetoothCB(const std_msgs::UInt8& msg);
_seminahn 0:4ff8aeb3e4d1 40 ros::Subscriber<std_msgs::UInt8> Bluetooth_subscriber("autocharge_state_NUC", &BluetoothCB);
_seminahn 0:4ff8aeb3e4d1 41
_seminahn 0:4ff8aeb3e4d1 42
_seminahn 0:4ff8aeb3e4d1 43 /* Threads and Timers ------------------------------------------------------- */
_seminahn 0:4ff8aeb3e4d1 44 Thread gThread[NUM_THREAD];
_seminahn 0:4ff8aeb3e4d1 45
_seminahn 0:4ff8aeb3e4d1 46 Timer waitTmr;
_seminahn 0:4ff8aeb3e4d1 47
_seminahn 0:4ff8aeb3e4d1 48
_seminahn 0:4ff8aeb3e4d1 49 /* Modules ------------------------------------------------------------------ */
_seminahn 0:4ff8aeb3e4d1 50 // Driving non-related modules
_seminahn 1:2594a70c1ddd 51 //extern UVC uvc;
_seminahn 0:4ff8aeb3e4d1 52 RELAY relay(CHARGE_RELAYP,CHARGE_RELAYN);
_seminahn 0:4ff8aeb3e4d1 53 // Driving related modules
_seminahn 0:4ff8aeb3e4d1 54
_seminahn 0:4ff8aeb3e4d1 55 MPU9250_SPI mpu(IMU_MOSI, IMU_MISO, IMU_SCK, IMU_NCS, IMU_INT);
_seminahn 0:4ff8aeb3e4d1 56
_seminahn 0:4ff8aeb3e4d1 57 HCSR04 sonar[NUM_SONAR] = {
_seminahn 1:2594a70c1ddd 58 HCSR04(SONAR_ECHO0, SONAR_TRIG0, SONAR_FILTER_WS,1), HCSR04(SONAR_ECHO1, SONAR_TRIG0, SONAR_FILTER_WS,2),
_seminahn 1:2594a70c1ddd 59 HCSR04(SONAR_ECHO2, SONAR_TRIG0, SONAR_FILTER_WS,3), HCSR04(SONAR_ECHO3, SONAR_TRIG0, SONAR_FILTER_WS,4),
_seminahn 1:2594a70c1ddd 60 HCSR04(SONAR_ECHO4, SONAR_TRIG0, SONAR_FILTER_WS,5), HCSR04(SONAR_ECHO5, SONAR_TRIG0, SONAR_FILTER_WS,6),
_seminahn 1:2594a70c1ddd 61 HCSR04(SONAR_ECHO6, SONAR_TRIG0, SONAR_FILTER_WS,7), HCSR04(SONAR_ECHO7, SONAR_TRIG0, SONAR_FILTER_WS,8),
_seminahn 1:2594a70c1ddd 62 HCSR04(SONAR_ECHO8, SONAR_TRIG0, SONAR_FILTER_WS,9), HCSR04(SONAR_ECHO9, SONAR_TRIG0, SONAR_FILTER_WS,10)
_seminahn 0:4ff8aeb3e4d1 63 };
_seminahn 1:2594a70c1ddd 64 SONAR_MANAGER sonar_manager(SONAR_TRIG0, sonar, NUM_SONAR);
_seminahn 0:4ff8aeb3e4d1 65
_seminahn 1:2594a70c1ddd 66 EStop estop(EMERGENCY);
_seminahn 0:4ff8aeb3e4d1 67
_seminahn 0:4ff8aeb3e4d1 68 /* For test ----------------------------------------------------------------- */
_seminahn 0:4ff8aeb3e4d1 69
_seminahn 2:0de4854743f7 70 #endif